simplify adding new actions

CMakeLists.txt

@@ -13,41 +13,36 @@ pkg_check_modules(readylogpp REQUIRED readylog++)
 pkg_check_modules(parsegologpp REQUIRED parsegolog++)
 
 find_package(catkin REQUIRED COMPONENTS
-  roscpp
-  std_msgs
-  genmsg
-  message_generation
-  actionlib_msgs
-  actionlib
+	roscpp
+	rosconsole
+	std_msgs
+	genmsg
+	message_generation
+	actionlib_msgs
+	actionlib
 )
 
-find_package(catkin OPTIONAL_COMPONENTS
-	actionlib
+set(gologpp_action_pkgs
 	darknet_action_msgs
 	naoqi_wrapper_msgs
 	naoqi_bridge_msgs
 	move_base_msgs
 	opencv_apps_action_msgs
 )
 
+find_package(catkin OPTIONAL_COMPONENTS ${gologpp_action_pkgs})
+
 add_definitions(-DLIBQI_VERSION=${naoqi_libqi_VERSION_MAJOR}${naoqi_libqi_VERSION_MINOR})
 if(CMAKE_COMPILER_IS_GNUCC OR CMAKE_COMPILER_IS_GNUCXX)
 	set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-deprecated-declarations -Wno-placement-new")
 endif()
 
 
-include_directories(
-  ${catkin_INCLUDE_DIRS}
-  ${readylogpp_INCLUDE_DIRS}
-  ${parsegologpp_INCLUDE_DIRS}
-)
-
 catkin_package(
-  #INCLUDE_DIRS include
-  #LIBRARIES ${PROJECT_NAME}
-  CATKIN_DEPENDS actionlib_msgs actionlib naoqi_wrapper_msgs
-  #CATKIN_DEPENDS actionlib actionlib_msgs message_generation roscpp rospy std_msgs
-  #DEPENDS system_lib
+	#INCLUDE_DIRS include
+	#LIBRARIES ${PROJECT_NAME}
+	CATKIN_DEPENDS actionlib_msgs actionlib roscpp
+	${gologpp_action_pkgs}
 )
 
 set(BUILD_TESTS OFF CACHE BOOL "Enable gologpp tests" FORCE)
@@ -61,38 +56,28 @@ add_executable(gologpp_agent
 	src/exog_manager.cpp
 )
 
-#add_dependencies(naoqi_say_server ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-if(naoqi_wrapper_msgs_FOUND)
-	target_sources(gologpp_agent PRIVATE src/naoqi_wrapper_actions.cpp)
-	target_compile_definitions(gologpp_agent PUBLIC -DNAOQI_WRAPPER_MSGS_PKG)
-endif()
-
-if(naoqi_bridge_msgs_FOUND)
-	target_sources(gologpp_agent PRIVATE src/naoqi_bridge_actions.cpp)
-	target_compile_definitions(gologpp_agent PUBLIC -DNAOQI_BRIDGE_MSGS_PKG)
-endif()
-
-
-if(darknet_action_msgs_FOUND)
-	target_sources(gologpp_agent PRIVATE src/darknet_actions.cpp)
-	target_compile_definitions(gologpp_agent PUBLIC -DDARKNET_ACTION_MSGS_PKG)
-endif()
-
-if(move_base_msgs_FOUND)
-	target_sources(gologpp_agent PRIVATE src/move_base_actions.cpp)
-	target_compile_definitions(gologpp_agent PUBLIC -DMOVE_BASE_MSGS_PKG)
-endif()
+target_include_directories(gologpp_agent PUBLIC
+	src/
+	${catkin_INCLUDE_DIRS}
+	${readylogpp_INCLUDE_DIRS}
+	${parsegologpp_INCLUDE_DIRS}
+)
 
-if(opencv_apps_action_msgs_FOUND)
-	target_sources(gologpp_agent PRIVATE src/opencv_apps_actions.cpp)
-	target_compile_definitions(gologpp_agent PUBLIC -DOPENCV_APPS_MSGS_PKG)
-endif()
+foreach(pkg ${gologpp_action_pkgs})
+	if ("${${pkg}_FOUND}")
+		target_sources(gologpp_agent PRIVATE "src/actions/gologpp_${pkg}.cpp")
+		target_compile_definitions(gologpp_agent PUBLIC "-D${pkg}_FOUND")
+	endif()
+endforeach()
 
 target_link_libraries(
-    gologpp_agent
-    ${catkin_LIBRARIES}
-    ${readylogpp_LIBRARIES}
-    ${parsegologpp_LIBRARIES}
+	gologpp_agent
+	roscpp
+	rosconsole
+	actionlib
+	${catkin_LIBRARIES}
+	${readylogpp_LIBRARIES}
+	${parsegologpp_LIBRARIES}
 )
 
 target_compile_features(

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>gologpp_agent</name>
-  <version>0.0.0</version>
+  <version>0.0.1</version>
   <description>The gologpp_agent package</description>
 
   <!-- One maintainer tag required, multiple allowed, one person per tag -->
@@ -52,12 +52,12 @@
 
   <depend>actionlib</depend>
   <depend>actionlib_msgs</depend>
-  <depend>message_generation</depend>
   <depend>roscpp</depend>
-  <depend>std_msgs</depend>
   <depend>naoqi_wrapper_msgs</depend>
   <depend>naoqi_bridge_msgs</depend>
   <depend>darknet_action_msgs</depend>
+  <depend>move_base_msgs</depend>
+  <depend>opencv_apps_action_msgs</depend>
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>

src/darknet_actions.cpp → src/actions/gologpp_darknet_action_msgs.cpp

@@ -1,4 +1,3 @@
-#ifdef DARKNET_ACTION_MSGS_PKG
 #include "action_manager.h"
 #include "exog_manager.h"
 #include "ros_backend.h"
@@ -33,4 +32,3 @@ void RosBackend::define_darknet_actions()
 	create_ActionManager<darknet::obj_detectionAction>("/yolo_obj_detection_position_server");
 
 }
-#endif

src/move_base_actions.cpp → src/actions/gologpp_move_base_msgs.cpp

@@ -1,13 +1,9 @@
-#ifdef MOVE_BASE_MSGS_PKG
 #include "action_manager.h"
 #include "exog_manager.h"
 #include "ros_backend.h"
 
 #include <execution/controller.h>
 
-#include <actionlib/client/simple_action_client.h>
-#include <actionlib/client/terminal_state.h>
-
 #include <move_base_msgs/MoveBaseAction.h>
 
 
@@ -25,8 +21,4 @@ ActionManager<move_base_msgs::MoveBaseAction>::build_goal(const gpp::Activity &a
 }
 
 void RosBackend::define_move_base_actions()
-{
-	create_ActionManager<move_base_msgs::MoveBaseAction>("move_base");
-}
-
-#endif //MOVE_BASE_MSGS_PKG
+{ create_ActionManager<move_base_msgs::MoveBaseAction>("move_base"); }

src/naoqi_bridge_actions.cpp → src/actions/gologpp_naoqi_bridge_msgs.cpp

@@ -1,4 +1,3 @@
-#ifdef NAOQI_BRIDGE_MSGS_PKG
 #include "exog_manager.h"
 #include "ros_backend.h"
 #include <naoqi_bridge_msgs/Bumper.h>
@@ -35,7 +34,3 @@ void RosBackend::define_naoqi_bridge_actions()
 		"/pepper_robot/naoqi_driver/head_touch"
 	);
 }
-
-
-#endif //NAOQI_BRIDGE_MSGS_PKG
-

src/naoqi_wrapper_actions.cpp → src/actions/gologpp_naoqi_wrapper_msgs.cpp

@@ -1,4 +1,3 @@
-#ifdef NAOQI_WRAPPER_MSGS_PKG
 #include "action_manager.h"
 #include "exog_manager.h"
 #include "ros_backend.h"
@@ -123,4 +122,4 @@ void RosBackend::define_naoqi_wrapper_actions()
 	create_ActionManager<naoqi::NaoQi_subscribeAction>("/naoqi_subscribe_server/subscribe");
 	create_ServiceManager<naoqi::FaceTracking>("/face_tracking");
 }
-#endif // NAOQI_WRAPPER_MSGS_PKG
+

src/pepper_actions.cpp → src/actions/pepper_actions.cpp

@@ -7,19 +7,19 @@
 #include <actionlib/client/simple_action_client.h>
 #include <actionlib/client/terminal_state.h>
 
-#ifdef MOVE_BASE_MSGS_PKG
+#ifdef move_base_msgs_FOUND
 #include <move_base_msgs/MoveBaseAction.h>
 #endif
 
-#ifdef DARKNET_ACTION_MSGS_PKG
+#ifdef darknet_action_msgs_FOUND
 #include <darknet_action_msgs/obj_detectionAction.h>
 #endif
 
-#ifdef NAOQI_BRIDGE_MSGS_PKG
+#ifdef naoqi_bridge_msgs_FOUND
 #include <naoqi_bridge_msgs/Bumper.h>
 #endif
 
-#ifdef NAOQI_WRAPPER_MSGS_PKG
+#ifdef naoqi_wrapper_msgs_FOUND
 #include <naoqi_wrapper_msgs/NaoQi_animatedSayAction.h>
 #include <naoqi_wrapper_msgs/NaoQi_animationAction.h>
 #include <naoqi_wrapper_msgs/NaoQi_dialogAction.h>
@@ -32,7 +32,7 @@
 namespace naoqi = naoqi_wrapper_msgs;
 #endif
 
-#ifdef DARKNET_ACTION_MSGS_PKG
+#ifdef darknet_action_msgs_FOUND
 namespace darknet = darknet_action_msgs;
 
 
@@ -41,9 +41,9 @@ gpp::Value *
 ActionManager<darknet::obj_detectionAction>::to_golog_constant(ResultT result) {
 	return new gpp::Value(gpp::NumberType::name(),result->obj_pos);
 }
-#endif //DARKNET_ACTION_MSGS_PKG
+#endif // darknet_action_msgs_FOUND
 
-#ifdef NAOQI_WRAPPER_MSGS_PKG
+#ifdef naoqi_wrapper_msgs_FOUND
 template<>
 gpp::Value *
 ActionManager<naoqi::NaoQi_dialogAction>::to_golog_constant(ResultT result){
@@ -125,9 +125,9 @@ ActionManager<naoqi::NaoQi_subscribeAction>::build_goal(const gpp::Activity &a)
 	return goal;
 }
 
-#endif // NAOQI_WRAPPER_MSGS_PKG
+#endif // naoqi_wrapper_msgs_FOUND
 
-#ifdef DARKNET_ACTION_MSGS_PKG
+#ifdef darknet_action_msgs_FOUND
 template<>
 ActionManager<darknet::obj_detectionAction>::GoalT
 ActionManager<darknet::obj_detectionAction>::build_goal(const gpp::Activity &a)
@@ -136,9 +136,9 @@ ActionManager<darknet::obj_detectionAction>::build_goal(const gpp::Activity &a)
 	goal.to_detected_obj = std::string(a.mapped_arg_value("to_detected_obj"));
 	return goal;
 }
-#endif // DARKNET_ACTION_MSGS_PKG
+#endif // darknet_action_msgs_FOUND
 
-#ifdef MOVE_BASE_MSGS_PKG
+#ifdef move_base_msgs_FOUND
 template<>
 ActionManager<move_base_msgs::MoveBaseAction>::GoalT
 ActionManager<move_base_msgs::MoveBaseAction>::build_goal(const gpp::Activity &a)
@@ -151,12 +151,12 @@ ActionManager<move_base_msgs::MoveBaseAction>::build_goal(const gpp::Activity &a
 	goal.target_pose.pose.orientation.w = int(a.mapped_arg_value("w"));;
 	return goal;
 }
-#endif //MOVE_BASE_MSGS_PKG
+#endif // move_base_msgs_FOUND
 
 
 void RosBackend::define_actions()
 {
-#ifdef NAOQI_WRAPPER_MSGS_PKG
+#ifdef naoqi_wrapper_msgs_FOUND
 	define_action_client<naoqi::NaoQi_dialogAction>("/naoqi_dialog_server");
 	define_action_client<naoqi::NaoQi_sayAction>("/naoqi_say_server/naoqi_say");
 	define_action_client<naoqi::NaoQi_lookAtAction>("/naoqi_lookAt_server/lookAt");
@@ -166,18 +166,18 @@ void RosBackend::define_actions()
 	define_action_client<naoqi::NaoQi_subscribeAction>("/naoqi_subscribe_server/subscribe");
 #endif
 
-#ifdef DARKNET_ACTION_MSGS_PKG
+#ifdef darknet_action_msgs_FOUND
 	define_action_client<darknet::obj_detectionAction>("/yolo_obj_detection_position_server");
 #endif
 
-#ifdef MOVE_BASE_MSGS_PKG
+#ifdef move_base_msgs_FOUND
 	define_action_client<move_base_msgs::MoveBaseAction>("move_base");
 #endif
 }
 
 
 
-#ifdef NAOQI_BRIDGE_MSGS_PKG
+#ifdef naoqi_bridge_msgs_FOUND
 template<>
 void
 ExogManager<naoqi_bridge_msgs::Bumper>::topic_cb(const naoqi_bridge_msgs::Bumper::ConstPtr& msg)
@@ -188,11 +188,11 @@ ExogManager<naoqi_bridge_msgs::Bumper>::topic_cb(const naoqi_bridge_msgs::Bumper
 		std::move(param)
 	);
 }
-#endif //NAOQI_BRIDGE_MSGS_PKG
+#endif // naoqi_bridge_msgs_FOUND
 
 void RosBackend::init_exog()
 {
-#ifdef NAOQI_BRIDGE_MSGS_PKG
+#ifdef naoqi_bridge_msgs_FOUND
 	sub_exog_event<naoqi_bridge_msgs::Bumper>(
 		"/pepper_robot/naoqi_driver/bumper"
 	);

src/agent_node.cpp

@@ -45,6 +45,8 @@ void load_n_exec_program(std::string program)
 	);
 	gpp::ReadylogContext &ctx = gpp::ReadylogContext::instance();
 
+	mainproc->attach_semantics(ctx.semantics_factory());
+
 	rosbackend->ctx_ready = true;
 	ctx.run(*mainproc);
 	rosbackend->ctx_ready = false;

src/exog_manager.h

@@ -6,19 +6,19 @@ namespace gpp = gologpp;
 
 class RosBackend;
 
+
 class AbstractExogManager {
 public:
 	AbstractExogManager(RosBackend &backend);
 	virtual ~AbstractExogManager() = default;
 
 protected:
 	RosBackend &backend;
-
 };
 
+
 template<class ExogT>
 class ExogManager : public AbstractExogManager {
-
 public:
 	ExogManager(RosBackend &backend, const std::string& topic, int msgs_queue_size = 1000);
 
@@ -27,10 +27,12 @@ class ExogManager : public AbstractExogManager {
 	void topic_cb(const typename ExogT::ConstPtr&);
 	void exog_event_to_queue(std::unordered_map< std::string, gpp::unique_ptr<gpp::Value> > &&params_to_map);
 	std::unordered_map< std::string, gpp::unique_ptr<gpp::Value> > params_to_map(const typename ExogT::ConstPtr& msg);
+
 private:
 	gpp::shared_ptr< gpp::ExogAction> exog_;
 };
 
+
 template<class ExogT>
 ExogManager<ExogT>::ExogManager(RosBackend &backend, const std::string& topic, int msgs_queue_size)
 : AbstractExogManager (backend)
@@ -53,6 +55,7 @@ ExogManager<ExogT>::ExogManager(RosBackend &backend, const std::string& topic, i
 	}
 }
 
+
 template<class ExogT>
 void
 ExogManager<ExogT>::topic_cb(const typename ExogT::ConstPtr& msg)
@@ -65,6 +68,7 @@ ExogManager<ExogT>::topic_cb(const typename ExogT::ConstPtr& msg)
 	}
 }
 
+
 template<class ExogT>
 void ExogManager<ExogT>::exog_event_to_queue( std::unordered_map< std::string, gpp::unique_ptr<gpp::Value> > &&params_to_map)
 {
@@ -81,6 +85,7 @@ void ExogManager<ExogT>::exog_event_to_queue( std::unordered_map< std::string, g
 		ctx.exog_queue_push(ev);
 }
 
+
 template<class ExogT>
 void RosBackend::create_ExogManger(const std::string &topic)
 {