diff --git a/CMakeLists.txt b/CMakeLists.txt index 940d6a8c143c1e997635b3470142761948e035bd..19a2be1bb6ba737f5bfce143658f81e78b2e107a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 2.8.3) -project(tiago_modules) +project(tiago_arm_module) ## Add support for C++11, supported in ROS Kinetic and newer add_definitions(-std=c++11) @@ -7,7 +7,7 @@ add_definitions(-std=c++11) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure iri_ros_tools actionlib play_motion_msgs control_msgs geometry_msgs std_srvs move_base_msgs tf nav_msgs pal_navigation_msgs pal_interaction_msgs pal_waypoint_msgs moveit_ros_planning_interface) +find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure iri_ros_tools actionlib geometry_msgs moveit_ros_planning_interface sensor_msgs) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) @@ -86,14 +86,7 @@ find_package(iriutils REQUIRED) ## Generate dynamic reconfigure parameters in the 'cfg' folder generate_dynamic_reconfigure_options( - cfg/GripperModule.cfg - cfg/TorsoModule.cfg - cfg/PlayMotionModule.cfg - cfg/HeadModule.cfg - cfg/NavModule.cfg - cfg/TTSModule.cfg - cfg/ArmModule.cfg - cfg/MovePlatformModule.cfg + cfg/TiagoArmModule.cfg ) ################################### @@ -107,8 +100,8 @@ generate_dynamic_reconfigure_options( ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( INCLUDE_DIRS include - LIBRARIES gripper_module play_motion_module head_module torso_module nav_module tts_module arm_module move_platform_module - CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools actionlib play_motion_msgs control_msgs geometry_msgs std_srvs move_base_msgs tf nav_msgs pal_navigation_msgs pal_waypoint_msgs pal_interaction_msgs moveit_ros_planning_interface + LIBRARIES arm_module + CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools actionlib geometry_msgs moveit_ros_planning_interface sensor_msgs # DEPENDS system_lib ) @@ -124,125 +117,20 @@ include_directories(${catkin_INCLUDE_DIRS}) include_directories(${iriutils_INCLUDE_DIR}) ## Declare a C++ library -add_library(gripper_module - src/gripper_module.cpp +set(module_name tiago_arm_module) +add_library(${module_name} + src/tiago_arm_module.cpp ) -target_link_libraries(gripper_module ${catkin_LIBRARIES}) -target_link_libraries(gripper_module ${iriutils_LIBRARY}) +target_link_libraries(${module_name} ${catkin_LIBRARIES}) +target_link_libraries(${module_name} ${iriutils_LIBRARY}) ##Link to other modules ##target_link_libraries(new_module <module path>/lib<module_name>.so) ## Add cmake target dependencies of the library ## as an example, code may need to be generated before libraries ## either from message generation or dynamic reconfigure -add_dependencies(gripper_module ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -## Add dependencies to other modules -##add_dependencies(new_module <module_name>) - -add_library(torso_module - src/torso_module.cpp -) - -target_link_libraries(torso_module ${catkin_LIBRARIES}) -target_link_libraries(torso_module ${iriutils_LIBRARY}) -##Link to other modules -##target_link_libraries(new_module <module path>/lib<module_name>.so) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -add_dependencies(torso_module ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ library -add_library(play_motion_module - src/play_motion_module.cpp -) - -target_link_libraries(play_motion_module ${catkin_LIBRARIES}) -target_link_libraries(play_motion_module ${iriutils_LIBRARY}) -##Link to other modules -##target_link_libraries(new_module <module path>/lib<module_name>.so) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -add_dependencies(play_motion_module ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ library -add_library(head_module - src/head_module.cpp -) - -target_link_libraries(head_module ${catkin_LIBRARIES}) -target_link_libraries(head_module ${iriutils_LIBRARY}) -##Link to other modules -##target_link_libraries(new_module <module path>/lib<module_name>.so) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -add_dependencies(head_module ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ library -add_library(nav_module - src/nav_module.cpp -) - -target_link_libraries(nav_module ${catkin_LIBRARIES}) -target_link_libraries(nav_module ${iriutils_LIBRARY}) -##Link to other modules -##target_link_libraries(new_module <module path>/lib<module_name>.so) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -add_dependencies(nav_module ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ library -add_library(tts_module - src/tts_module.cpp -) - -target_link_libraries(tts_module ${catkin_LIBRARIES}) -target_link_libraries(tts_module ${iriutils_LIBRARY}) -##Link to other modules -##target_link_libraries(new_module <module path>/lib<module_name>.so) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -add_dependencies(tts_module ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ library -add_library(arm_module - src/arm_module.cpp -) - -target_link_libraries(arm_module ${catkin_LIBRARIES}) -target_link_libraries(arm_module ${iriutils_LIBRARY}) -##Link to other modules -##target_link_libraries(new_module <module path>/lib<module_name>.so) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -add_dependencies(arm_module ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ library -add_library(move_platform_module - src/move_platform_module.cpp -) - -target_link_libraries(move_platform_module ${catkin_LIBRARIES}) -target_link_libraries(move_platform_module ${iriutils_LIBRARY}) -##Link to other modules -##target_link_libraries(new_module <module path>/lib<module_name>.so) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -add_dependencies(move_platform_module ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(${module_name} ${${PROJECT_NAME}_EXPORTED_TARGETS}) ## Declare a C++ executable ## With catkin_make all packages are built within a single CMake context diff --git a/cfg/ArmModule.cfg b/cfg/TiagoArmModule.cfg similarity index 83% rename from cfg/ArmModule.cfg rename to cfg/TiagoArmModule.cfg index b6e1125839997b1d357b8d12ad91b023dbf38b1e..b9112b0db2b723b81cf5a7b2b09bc9652264d083 100755 --- a/cfg/ArmModule.cfg +++ b/cfg/TiagoArmModule.cfg @@ -31,9 +31,10 @@ # Author: -PACKAGE='arm_module' +PACKAGE='tiago_arm_module' from dynamic_reconfigure.parameter_generator_catkin import * +from iri_ros_tools.dyn_params import add_module_action_params,add_module_params gen = ParameterGenerator() @@ -45,14 +46,8 @@ constraints = gen.add_group("Joints Orientation Constraints") # Name Type Reconfiguration level Description Default Min Max #gen.add("velocity_scale_factor", double_t, 0, "Maximum velocity scale factor", 0.5, 0.0, 1.0) -gen.add("rate_hz", double_t, 0, "Gripper module rate in Hz", 1.0, 1.0, 100.0) - -arm_action.add("action_max_retries",int_t, 0, "Maximum number of retries for the action start",1,1,10) -arm_action.add("feedback_watchdog_time_s",double_t,0, "Maximum time between feedback messages",1, 0.01, 600) -arm_action.add("enable_watchdog", bool_t, 0, "Enable action watchdog", True) -arm_action.add("timeout_s", double_t, 0, "Maximum time allowed to complete the action",1,0.1,30) -arm_action.add("enable_timeout", bool_t, 0, "Enable action timeout", True) -arm_action.add("enabled", bool_t, 0, "Enable action execution ", True) +add_module_params(gen,"arm_module") +play_motion_action=add_module_action_params(gen,"move") joints_subs.add("js_watchdog_time_s",double_t,0, "Maximum time between joint state messages",1,0.01,50) @@ -79,4 +74,4 @@ constraints.add("torso_tolerance_above", double_t, 0, "Tolerance above tors constraints.add("torso_tolerance_below", double_t, 0, "Tolerance below torso joint", 0.05, 0.05, 0.1) constraints.add("set_constraints", bool_t, 0, "Set the given constraints", False) -exit(gen.generate(PACKAGE, "ArmModuleAlgorithm", "ArmModule")) +exit(gen.generate(PACKAGE, "TiagoArmModuleAlgorithm", "TiagoArmModule")) diff --git a/config/arm_module_default.yaml b/config/tiago_arm_module_default.yaml similarity index 64% rename from config/arm_module_default.yaml rename to config/tiago_arm_module_default.yaml index 5ee833af548d4ad0fd85bb00ca49e4079c6fe1c1..28457660a92cb30dfaee51dd1fb85dc0668fe21d 100644 --- a/config/arm_module_default.yaml +++ b/config/tiago_arm_module_default.yaml @@ -1,9 +1,11 @@ -action_max_retries: 1 -feedback_watchdog_time_s: 1.0 -enable_watchdog: True -timeout_s: 1.0 -enable_timeout: True -enabled: True +arm_module_rate: 1.0 + +move_num_retries: 1 +move_feedback_watchdog_time_s: 1.0 +move_enable_watchdog: True +move_timeout_s: 1.0 +move_enable_timeout: True +move_enabled: True #joint states watchdog time js_watchdog_time_s: 1.0 diff --git a/include/tiago_modules/arm_module.h b/include/tiago_arm_module/tiago_arm_module.h similarity index 74% rename from include/tiago_modules/arm_module.h rename to include/tiago_arm_module/tiago_arm_module.h index c8587cdc467369861608a7c161a4606b435b0dfd..9f8d9e518d8d66f9c32d14ceaca2ec1ff1b09bf8 100644 --- a/include/tiago_modules/arm_module.h +++ b/include/tiago_arm_module/tiago_arm_module.h @@ -1,10 +1,10 @@ -#ifndef _ARM_MODULE_H -#define _ARM_MODULE_H +#ifndef _TIAGO_ARM_MODULE_H +#define _TIAGO_ARM_MODULE_H #include <iri_ros_tools/module.h> #include <iri_ros_tools/module_action.h> -#include <tiago_modules/ArmModuleConfig.h> +#include <tiago_arm_module/TiagoArmModuleConfig.h> //Action #include <moveit_msgs/MoveGroupAction.h> @@ -16,18 +16,18 @@ typedef enum {unknown_group,arm_group,arm_torso_group} group_id_t; //States -typedef enum {ARM_MODULE_IDLE,ARM_MODULE_START,ARM_MODULE_WAIT} arm_module_state_t; +typedef enum {TIAGO_ARM_MODULE_IDLE,TIAGO_ARM_MODULE_START,TIAGO_ARM_MODULE_WAIT} tiago_arm_module_state_t; //Status -typedef enum {ARM_MODULE_RUNNING, - ARM_MODULE_SUCCESS, - ARM_MODULE_ACTION_START_FAIL, - ARM_MODULE_TIMEOUT, - ARM_MODULE_FB_WATCHDOG, - ARM_MODULE_ABORTED, - ARM_MODULE_PREEMPTED, - ARM_MODULE_REJECTED, - ARM_MODULE_NO_JOINT_STATES} arm_module_status_t; +typedef enum {TIAGO_ARM_MODULE_RUNNING, + TIAGO_ARM_MODULE_SUCCESS, + TIAGO_ARM_MODULE_ACTION_START_FAIL, + TIAGO_ARM_MODULE_TIMEOUT, + TIAGO_ARM_MODULE_FB_WATCHDOG, + TIAGO_ARM_MODULE_ABORTED, + TIAGO_ARM_MODULE_PREEMPTED, + TIAGO_ARM_MODULE_REJECTED, + TIAGO_ARM_MODULE_NO_JOINT_STATES} tiago_arm_module_status_t; typedef struct { @@ -39,13 +39,13 @@ typedef struct typedef enum {X_PLANE,Y_PLANE,Z_PLANE} path_const_plane_t; -class CArmModule : public CModule<arm_module::ArmModuleConfig> +class CTiagoArmModule : public CModule<tiago_arm_module::TiagoArmModuleConfig> { private: - arm_module::ArmModuleConfig config; + tiago_arm_module::TiagoArmModuleConfig config; - CModuleAction<moveit_msgs::MoveGroupAction> arm_action; + CModuleAction<moveit_msgs::MoveGroupAction,tiago_arm_module::TiagoArmModuleConfig> arm_action; moveit_msgs::MoveGroupGoal goal; //constraints variables @@ -59,15 +59,15 @@ class CArmModule : public CModule<arm_module::ArmModuleConfig> void joint_states_callback(const sensor_msgs::JointState::ConstPtr& msg); //Variables - arm_module_state_t state; - arm_module_status_t status; + tiago_arm_module_state_t state; + tiago_arm_module_status_t status; bool new_motion; bool cancel_pending; // planning objects std::vector<TVisualObject> objects; ros::Publisher scene_publisher; - CModuleService<moveit_msgs::GetPlanningScene> get_scene; + CModuleService<moveit_msgs::GetPlanningScene,tiago_arm_module::TiagoArmModuleConfig> get_scene; moveit_msgs::PlanningScene current_pscene; // path constraints @@ -77,10 +77,10 @@ class CArmModule : public CModule<arm_module::ArmModuleConfig> protected: void state_machine(void); - void reconfigure_callback(arm_module::ArmModuleConfig &config, uint32_t level); + void reconfigure_callback(tiago_arm_module::TiagoArmModuleConfig &config, uint32_t level); public: - CArmModule(const std::string &name,const std::string &name_space=std::string("")); + CTiagoArmModule(const std::string &name,const std::string &name_space=std::string("")); // general functions void set_planner(std::string &planner_id); std::string get_planner(void); @@ -118,9 +118,9 @@ class CArmModule : public CModule<arm_module::ArmModuleConfig> void get_current_pose(geometry_msgs::PoseStamped &pose); void stop(void); bool is_finished(void); - arm_module_status_t get_status(void); + tiago_arm_module_status_t get_status(void); - ~CArmModule(); + ~CTiagoArmModule(); }; #endif diff --git a/package.xml b/package.xml index 41f80f3a0b6fc2767957bb3f77f5be2dcc15bdd4..5363b631bff2511ecfccdf495c13834b7e8aecdc 100644 --- a/package.xml +++ b/package.xml @@ -1,19 +1,19 @@ <?xml version="1.0"?> <package> - <name>tiago_modules</name> + <name>tiago_arm_module</name> <version>0.0.0</version> - <description>Module tiago_modules package</description> + <description>Module tiago_arm_module package</description> <!-- One maintainer tag required, multiple allowed, one person per tag --> <!-- Example: --> <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> --> - <maintainer email="sergi@todo.todo">sergi</maintainer> + <maintainer email="labrobotica@iri.upc.edu">labrobotica</maintainer> <!-- One license tag required, multiple allowed, one license per tag --> <!-- Commonly used license strings: --> <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 --> - <license>TODO</license> + <license>LGPL</license> <!-- Url tags are optional, but multiple are allowed, one per tag --> @@ -25,7 +25,7 @@ <!-- Author tags are optional, multiple are allowed, one per tag --> <!-- Authors do not have to be maintainers, but could be --> <!-- Example: --> - <!-- <author email="jane.doe@example.com">Jane Doe</author> --> + <author email="shernand@iri.upc.edu">Sergi Hernandez Juan</author> --> <!-- The *_depend tags are used to specify dependencies --> @@ -44,16 +44,8 @@ <build_depend>roscpp</build_depend> <build_depend>dynamic_reconfigure</build_depend> <build_depend>actionlib</build_depend> - <build_depend>std_srvs</build_depend> - <build_depend>play_motion_msgs</build_depend> - <build_depend>control_msgs</build_depend> <build_depend>geometry_msgs</build_depend> - <build_depend>move_base_msgs</build_depend> - <build_depend>nav_msgs</build_depend> - <build_depend>pal_navigation_msgs</build_depend> - <build_depend>pal_waypoint_msgs</build_depend> - <build_depend>pal_interaction_msgs</build_depend> - <build_depend>tf</build_depend> + <build_depend>sensor_msgs</build_depend> <build_depend>moveit_ros_planning_interface</build_depend> <!-- new dependencies --> <!--<build_depend>new build dependency</build_depend>--> @@ -61,16 +53,8 @@ <run_depend>roscpp</run_depend> <run_depend>dynamic_reconfigure</run_depend> <run_depend>actionlib</run_depend> - <run_depend>std_srvs</run_depend> - <run_depend>play_motion_msgs</run_depend> - <run_depend>control_msgs</run_depend> <run_depend>geometry_msgs</run_depend> - <run_depend>move_base_msgs</run_depend> - <run_depend>nav_msgs</run_depend> - <run_depend>pal_navigation_msgs</run_depend> - <run_depend>pal_waypoint_msgs</run_depend> - <run_depend>pal_interaction_msgs</run_depend> - <run_depend>tf</run_depend> + <run_depend>sensor_msgs</run_depend> <run_depend>moveit_ros_planning_interface</run_depend> <!-- new dependencies --> <!--<run_depend>new run dependency</run_depend>--> diff --git a/src/arm_module.cpp b/src/tiago_arm_module.cpp similarity index 74% rename from src/arm_module.cpp rename to src/tiago_arm_module.cpp index a84bcfec602352232bd6bd894cda99002155cba9..bfd72b49ad3ee9a82d7ea2dd78ea39dfd00acd04 100644 --- a/src/arm_module.cpp +++ b/src/tiago_arm_module.cpp @@ -1,6 +1,6 @@ -#include <tiago_modules/arm_module.h> +#include <tiago_arm_module/tiago_arm_module.h> -CArmModule::CArmModule(const std::string &name,const std::string &name_space) : CModule(name,name_space), +CTiagoArmModule::CTiagoArmModule(const std::string &name,const std::string &name_space) : CModule(name,name_space), arm_action("arm",this->module_nh.getNamespace()), get_scene("get_planning_scene", this->module_nh.getNamespace()) { @@ -8,11 +8,11 @@ CArmModule::CArmModule(const std::string &name,const std::string &name_space) : // initialize odometry subscriber this->joint_states_watchdog.reset(ros::Duration(this->config.js_watchdog_time_s)); - this->joint_states_subscriber = this->module_nh.subscribe("joint_states",1,&CArmModule::joint_states_callback,this); + this->joint_states_subscriber = this->module_nh.subscribe("joint_states",1,&CTiagoArmModule::joint_states_callback,this); //Variables - this->state=ARM_MODULE_IDLE; - this->status=ARM_MODULE_SUCCESS; + this->state=TIAGO_ARM_MODULE_IDLE; + this->status=TIAGO_ARM_MODULE_SUCCESS; this->new_motion=false; this->cancel_pending=false; @@ -24,73 +24,73 @@ CArmModule::CArmModule(const std::string &name,const std::string &name_space) : } /* State machine */ -void CArmModule::state_machine(void) +void CTiagoArmModule::state_machine(void) { switch(this->state) { - case ARM_MODULE_IDLE: ROS_DEBUG("CArmModule: Idle"); + case TIAGO_ARM_MODULE_IDLE: ROS_DEBUG("CTiagoArmModule: Idle"); if(this->new_motion) { this->new_motion=false; - this->state=ARM_MODULE_START; + this->state=TIAGO_ARM_MODULE_START; } else - this->state=ARM_MODULE_IDLE; + this->state=TIAGO_ARM_MODULE_IDLE; break; - case ARM_MODULE_START: ROS_DEBUG("CArmModule: Start"); + case TIAGO_ARM_MODULE_START: ROS_DEBUG("CTiagoArmModule: Start"); switch(this->arm_action.make_request(this->goal)) { - case ACT_SRV_SUCCESS: this->state=ARM_MODULE_WAIT; - ROS_DEBUG("CArmModule : goal start successfull"); + case ACT_SRV_SUCCESS: this->state=TIAGO_ARM_MODULE_WAIT; + ROS_DEBUG("CTiagoArmModule : goal start successfull"); break; - case ACT_SRV_PENDING: this->state=ARM_MODULE_START; - ROS_WARN("CArmModule : goal start pending"); + case ACT_SRV_PENDING: this->state=TIAGO_ARM_MODULE_START; + ROS_WARN("CTiagoArmModule : goal start pending"); break; - case ACT_SRV_FAIL: this->state=ARM_MODULE_IDLE; - this->status=ARM_MODULE_ACTION_START_FAIL; - ROS_ERROR("CArmModule: goal start failed"); + case ACT_SRV_FAIL: this->state=TIAGO_ARM_MODULE_IDLE; + this->status=TIAGO_ARM_MODULE_ACTION_START_FAIL; + ROS_ERROR("CTiagoArmModule: goal start failed"); break; } break; - case ARM_MODULE_WAIT: ROS_DEBUG("CArmModule : state WAIT"); + case TIAGO_ARM_MODULE_WAIT: ROS_DEBUG("CTiagoArmModule : state WAIT"); switch(this->arm_action.get_state()) { case ACTION_IDLE: - case ACTION_RUNNING: ROS_DEBUG("CArmModule : action running"); - this->state=ARM_MODULE_WAIT; + case ACTION_RUNNING: ROS_DEBUG("CTiagoArmModule : action running"); + this->state=TIAGO_ARM_MODULE_WAIT; break; - case ACTION_SUCCESS: ROS_INFO("CArmModule : action ended successfully"); - this->state=ARM_MODULE_IDLE; - this->status=ARM_MODULE_SUCCESS; + case ACTION_SUCCESS: ROS_INFO("CTiagoArmModule : action ended successfully"); + this->state=TIAGO_ARM_MODULE_IDLE; + this->status=TIAGO_ARM_MODULE_SUCCESS; this->arm_action.stop_timeout(); break; - case ACTION_TIMEOUT: ROS_ERROR("CArmModule : action did not finish in the allowed time"); + case ACTION_TIMEOUT: ROS_ERROR("CTiagoArmModule : action did not finish in the allowed time"); this->arm_action.cancel(); - this->state=ARM_MODULE_IDLE; - this->status=ARM_MODULE_TIMEOUT; + this->state=TIAGO_ARM_MODULE_IDLE; + this->status=TIAGO_ARM_MODULE_TIMEOUT; this->arm_action.stop_timeout(); break; - case ACTION_FB_WATCHDOG: ROS_ERROR("CArmModule : No feeback received for a long time"); + case ACTION_FB_WATCHDOG: ROS_ERROR("CTiagoArmModule : No feeback received for a long time"); this->arm_action.cancel(); - this->state=ARM_MODULE_IDLE; - this->status=ARM_MODULE_FB_WATCHDOG; + this->state=TIAGO_ARM_MODULE_IDLE; + this->status=TIAGO_ARM_MODULE_FB_WATCHDOG; this->arm_action.stop_timeout(); break; - case ACTION_ABORTED: ROS_ERROR("CArmModule : Action failed to complete"); - this->state=ARM_MODULE_IDLE; - this->status=ARM_MODULE_ABORTED; + case ACTION_ABORTED: ROS_ERROR("CTiagoArmModule : Action failed to complete"); + this->state=TIAGO_ARM_MODULE_IDLE; + this->status=TIAGO_ARM_MODULE_ABORTED; this->arm_action.stop_timeout(); break; - case ACTION_PREEMPTED: ROS_ERROR("CArmModule : Action was interrupted by another request"); - this->state=ARM_MODULE_IDLE; - this->status=ARM_MODULE_PREEMPTED; + case ACTION_PREEMPTED: ROS_ERROR("CTiagoArmModule : Action was interrupted by another request"); + this->state=TIAGO_ARM_MODULE_IDLE; + this->status=TIAGO_ARM_MODULE_PREEMPTED; this->arm_action.stop_timeout(); break; - case ACTION_REJECTED: ROS_ERROR("CArmModule : Action was rejected by server"); - this->state=ARM_MODULE_IDLE; - this->status=ARM_MODULE_REJECTED; + case ACTION_REJECTED: ROS_ERROR("CTiagoArmModule : Action was rejected by server"); + this->state=TIAGO_ARM_MODULE_IDLE; + this->status=TIAGO_ARM_MODULE_REJECTED; this->arm_action.stop_timeout(); break; } @@ -103,27 +103,15 @@ void CArmModule::state_machine(void) } } -void CArmModule::reconfigure_callback(arm_module::ArmModuleConfig &config, uint32_t level) +void CTiagoArmModule::reconfigure_callback(tiago_arm_module::TiagoArmModuleConfig &config, uint32_t level) { - ROS_INFO("CArmModule : reconfigure callback"); + ROS_INFO("CTiagoArmModule : reconfigure callback"); this->lock(); this->config=config; /* set the module rate */ - this->set_rate(config.rate_hz); + this->dynamic_reconfigure(config,"arm_module"); /* motion action parameters */ - this->arm_action.set_max_num_retries(config.action_max_retries); - if(config.enable_watchdog) - this->arm_action.enable_watchdog(config.feedback_watchdog_time_s); - else - this->arm_action.disable_watchdog(); - if(this->config.enable_timeout) - this->arm_action.enable_timeout(config.timeout_s); - else - this->arm_action.disable_timeout(); - if(this->config.enabled) - this->arm_action.enable(); - else - this->arm_action.disable(); + this->arm_action.dynamic_reconfigure(config,"move"); this->unlock(); // planner params this->set_planner(config.planner_id); @@ -140,9 +128,9 @@ void CArmModule::reconfigure_callback(arm_module::ArmModuleConfig &config, uint3 this->set_workspace(config.ws_frame_id,config.ws_x_min,config.ws_y_min,config.ws_z_min,config.ws_x_max,config.ws_y_max,config.ws_z_max); } -void CArmModule::joint_states_callback(const sensor_msgs::JointState::ConstPtr& msg) +void CTiagoArmModule::joint_states_callback(const sensor_msgs::JointState::ConstPtr& msg) { - ROS_DEBUG("CArmModule: joint states callback"); + ROS_DEBUG("CTiagoArmModule: joint states callback"); this->lock(); // reset the odometry callback @@ -163,19 +151,19 @@ void CArmModule::joint_states_callback(const sensor_msgs::JointState::ConstPtr& } // general functions -void CArmModule::set_planner(std::string &planner_id) +void CTiagoArmModule::set_planner(std::string &planner_id) { this->lock(); this->goal.request.planner_id=planner_id; this->unlock(); } -std::string CArmModule::get_planner(void) +std::string CTiagoArmModule::get_planner(void) { return this->goal.request.planner_id; } -bool CArmModule::set_group_name(group_id_t group_id) +bool CTiagoArmModule::set_group_name(group_id_t group_id) { this->lock(); if(group_id==arm_group) @@ -191,7 +179,7 @@ bool CArmModule::set_group_name(group_id_t group_id) return true; } -group_id_t CArmModule::get_group_name(void) +group_id_t CTiagoArmModule::get_group_name(void) { if(this->goal.request.group_name=="arm") return arm_group; @@ -201,31 +189,31 @@ group_id_t CArmModule::get_group_name(void) return unknown_group; } -void CArmModule::set_max_planning_attempts(unsigned int num) +void CTiagoArmModule::set_max_planning_attempts(unsigned int num) { this->lock(); this->goal.request.num_planning_attempts=num; this->unlock(); } -unsigned int CArmModule::get_max_planning_attempts(void) +unsigned int CTiagoArmModule::get_max_planning_attempts(void) { return this->goal.request.num_planning_attempts; } -void CArmModule::set_max_planning_time(double time) +void CTiagoArmModule::set_max_planning_time(double time) { this->lock(); this->goal.request.allowed_planning_time=time; this->unlock(); } -double CArmModule::get_max_planning_time(void) +double CTiagoArmModule::get_max_planning_time(void) { return this->goal.request.allowed_planning_time; } -void CArmModule::set_velocity_scale_factor(double scale) +void CTiagoArmModule::set_velocity_scale_factor(double scale) { this->lock(); if(scale>1.0) @@ -237,13 +225,13 @@ void CArmModule::set_velocity_scale_factor(double scale) this->unlock(); } -double CArmModule::get_velocity_scale_factor(void) +double CTiagoArmModule::get_velocity_scale_factor(void) { return this->goal.request.max_velocity_scaling_factor; } // object functions -void CArmModule::add_object(const std::string &name,geometry_msgs::PoseStamped &pose,double width, double length, double height) +void CTiagoArmModule::add_object(const std::string &name,geometry_msgs::PoseStamped &pose,double width, double length, double height) { unsigned int i=0; TVisualObject new_object; @@ -254,7 +242,7 @@ void CArmModule::add_object(const std::string &name,geometry_msgs::PoseStamped & { this->update_object_pose(name,pose); this->update_object_size(name,width,length,height); - ROS_INFO_STREAM ("CArmModule:: Object already existed, has been updated: " << name); + ROS_INFO_STREAM ("CTiagoArmModule:: Object already existed, has been updated: " << name); return; } } @@ -279,10 +267,10 @@ void CArmModule::add_object(const std::string &name,geometry_msgs::PoseStamped & new_object.on_environment=false; this->objects.push_back(new_object); - ROS_INFO_STREAM ("CArmModule:: Object added to VVObjects: " << name); + ROS_INFO_STREAM ("CTiagoArmModule:: Object added to VVObjects: " << name); } -void CArmModule::update_object_pose(const std::string &name,geometry_msgs::PoseStamped &pose) +void CTiagoArmModule::update_object_pose(const std::string &name,geometry_msgs::PoseStamped &pose) { unsigned int i; @@ -303,7 +291,7 @@ void CArmModule::update_object_pose(const std::string &name,geometry_msgs::PoseS } } -void CArmModule::update_object_size(const std::string &name,double width, double length, double height) +void CTiagoArmModule::update_object_size(const std::string &name,double width, double length, double height) { unsigned int i; @@ -330,14 +318,14 @@ void CArmModule::update_object_size(const std::string &name,double width, double } } -bool CArmModule::remove_object(const std::string &name) +bool CTiagoArmModule::remove_object(const std::string &name) { if (this->dettach_object_from_robot(name) or this->remove_object_from_environment(name)) return true; return false; } -void CArmModule::add_object_to_environment(const std::string &name) +void CTiagoArmModule::add_object_to_environment(const std::string &name) { get_current_planning_scene(0); moveit_msgs::PlanningScene planning_scene = this->current_pscene; @@ -353,15 +341,15 @@ void CArmModule::add_object_to_environment(const std::string &name) planning_scene.is_diff = true; this->scene_publisher.publish(planning_scene); ros::Duration(1.0).sleep(); // well... - ROS_INFO_STREAM ("CArmModule:: Object added to environment: " << name); + ROS_INFO_STREAM ("CTiagoArmModule:: Object added to environment: " << name); return; } } - ROS_INFO_STREAM("CArmModule:: Object could not be added to environment: " << name); + ROS_INFO_STREAM("CTiagoArmModule:: Object could not be added to environment: " << name); } -bool CArmModule::remove_object_from_environment(const std::string &name) +bool CTiagoArmModule::remove_object_from_environment(const std::string &name) { get_current_planning_scene(0); moveit_msgs::PlanningScene planning_scene = this->current_pscene; @@ -376,7 +364,7 @@ bool CArmModule::remove_object_from_environment(const std::string &name) planning_scene.is_diff = true; this->scene_publisher.publish(planning_scene); ros::Duration(1.0).sleep(); // well... - ROS_INFO_STREAM ("CArmModule:: Object removed from environment: " << name); + ROS_INFO_STREAM ("CTiagoArmModule:: Object removed from environment: " << name); this->objects.erase(this->objects.begin()+i); return true; @@ -386,17 +374,17 @@ bool CArmModule::remove_object_from_environment(const std::string &name) return false; } -void CArmModule::attach_object_to_robot(const std::string &name,const std::string &link) +void CTiagoArmModule::attach_object_to_robot(const std::string &name,const std::string &link) { } -bool CArmModule::dettach_object_from_robot(const std::string &name) +bool CTiagoArmModule::dettach_object_from_robot(const std::string &name) { return true; } -bool CArmModule::disable_collision (const std::string &name) { +bool CTiagoArmModule::disable_collision (const std::string &name) { if (!get_current_planning_scene(moveit_msgs::PlanningSceneComponents::ALLOWED_COLLISION_MATRIX)) return false; moveit_msgs::AllowedCollisionMatrix acm = this->current_pscene.allowed_collision_matrix; @@ -413,7 +401,7 @@ bool CArmModule::disable_collision (const std::string &name) { this->current_pscene.allowed_collision_matrix = acm; this->current_pscene.is_diff = true; this->scene_publisher.publish(this->current_pscene); - ROS_INFO_STREAM("CArmModule:: Object collision disabled: " << name); + ROS_INFO_STREAM("CTiagoArmModule:: Object collision disabled: " << name); ros::Duration(1.0).sleep(); // well... return true; @@ -431,30 +419,30 @@ bool CArmModule::disable_collision (const std::string &name) { this->current_pscene.allowed_collision_matrix = acm; this->current_pscene.is_diff = true; //To substract! this->scene_publisher.publish(this->current_pscene); - ROS_INFO_STREAM ("CArmModule:: Object added to ACM with collision disabled: " << name); + ROS_INFO_STREAM ("CTiagoArmModule:: Object added to ACM with collision disabled: " << name); ros::Duration(1.0).sleep();//sleep here? TODO return true; } -bool CArmModule::get_current_planning_scene(int component) { +bool CTiagoArmModule::get_current_planning_scene(int component) { moveit_msgs::GetPlanningScene ps; ps.request.components.components=component; switch (this->get_scene.call(ps)) { case ACT_SRV_SUCCESS: this->current_pscene = ps.response.scene; - ROS_INFO_STREAM ("CArmModule:: Current planning scene service success"); + ROS_INFO_STREAM ("CTiagoArmModule:: Current planning scene service success"); return true; break; case ACT_SRV_PENDING: - ROS_WARN_STREAM ("CArmModule:: Current planning scene service pending"); + ROS_WARN_STREAM ("CTiagoArmModule:: Current planning scene service pending"); break; case ACT_SRV_FAIL: - ROS_ERROR_STREAM ("CArmModule:: Current planning scene service failed"); + ROS_ERROR_STREAM ("CTiagoArmModule:: Current planning scene service failed"); break; } @@ -462,7 +450,7 @@ bool CArmModule::get_current_planning_scene(int component) { } // constraint functions -void CArmModule::clear_path_constraints(void) +void CTiagoArmModule::clear_path_constraints(void) { this->goal.request.goal_constraints[0].joint_constraints.clear(); this->path_constraints.joint_constraints.clear(); @@ -471,7 +459,7 @@ void CArmModule::clear_path_constraints(void) this->path_constraints.visibility_constraints.clear(); } -void CArmModule::add_orientation_path_constraint(geometry_msgs::QuaternionStamped &quat,double orientation_tol) +void CTiagoArmModule::add_orientation_path_constraint(geometry_msgs::QuaternionStamped &quat,double orientation_tol) { moveit_msgs::OrientationConstraint new_const; @@ -487,7 +475,7 @@ void CArmModule::add_orientation_path_constraint(geometry_msgs::QuaternionStampe this->path_constraints.orientation_constraints.push_back(new_const); } -void CArmModule::add_plane_path_constraint(path_const_plane_t plane,std::string &frame_id,double offset,double position_tol) +void CTiagoArmModule::add_plane_path_constraint(path_const_plane_t plane,std::string &frame_id,double offset,double position_tol) { moveit_msgs::PositionConstraint new_const; geometry_msgs::Pose pose; @@ -547,12 +535,12 @@ void CArmModule::add_plane_path_constraint(path_const_plane_t plane,std::string this->path_constraints.position_constraints.push_back(new_const); } -void CArmModule::add_plane_path_constraint(std::vector<double> &normal,std::string &frame_id,double offset,double position_tol) +void CTiagoArmModule::add_plane_path_constraint(std::vector<double> &normal,std::string &frame_id,double offset,double position_tol) { } -void CArmModule::add_joint_goal_constraint(std::vector<std::string> &joint_names,std::vector<double> &position, std::vector<double> &tol) +void CTiagoArmModule::add_joint_goal_constraint(std::vector<std::string> &joint_names,std::vector<double> &position, std::vector<double> &tol) { moveit_msgs::JointConstraint joint_const; @@ -568,7 +556,7 @@ void CArmModule::add_joint_goal_constraint(std::vector<std::string> &joint_names } // joint motion -bool CArmModule::set_workspace(std::string &frame_id,double x_min,double y_min, double z_min, double x_max, double y_max, double z_max) +bool CTiagoArmModule::set_workspace(std::string &frame_id,double x_min,double y_min, double z_min, double x_max, double y_max, double z_max) { this->lock(); if(x_max>x_min && y_max>y_min && z_max>z_min) @@ -593,12 +581,12 @@ bool CArmModule::set_workspace(std::string &frame_id,double x_min,double y_min, return false; } -bool CArmModule::move_to(sensor_msgs::JointState &target_joints,double joint_tol) +bool CTiagoArmModule::move_to(sensor_msgs::JointState &target_joints,double joint_tol) { this->lock(); if(this->joint_states_watchdog.is_active()) { - this->status=ARM_MODULE_NO_JOINT_STATES; + this->status=TIAGO_ARM_MODULE_NO_JOINT_STATES; this->unlock(); return false; } @@ -630,7 +618,7 @@ bool CArmModule::move_to(sensor_msgs::JointState &target_joints,double joint_tol return true; } -void CArmModule::get_current_joint_angles(sensor_msgs::JointState &angles) +void CTiagoArmModule::get_current_joint_angles(sensor_msgs::JointState &angles) { this->lock(); angles=this->goal.request.start_state.joint_state; @@ -638,7 +626,7 @@ void CArmModule::get_current_joint_angles(sensor_msgs::JointState &angles) } // pose motion -void CArmModule::move_to(geometry_msgs::PoseStamped &pose,double position_tol,double orientation_tol) +void CTiagoArmModule::move_to(geometry_msgs::PoseStamped &pose,double position_tol,double orientation_tol) { this->lock(); if(!this->is_finished()) @@ -682,30 +670,30 @@ void CArmModule::move_to(geometry_msgs::PoseStamped &pose,double position_tol,do this->unlock(); } -void CArmModule::get_current_pose(geometry_msgs::PoseStamped &pose) +void CTiagoArmModule::get_current_pose(geometry_msgs::PoseStamped &pose) { } -void CArmModule::stop(void) +void CTiagoArmModule::stop(void) { - if(this->state!=ARM_MODULE_IDLE) + if(this->state!=TIAGO_ARM_MODULE_IDLE) this->cancel_pending=true; } -bool CArmModule::is_finished(void) +bool CTiagoArmModule::is_finished(void) { - if(this->state==ARM_MODULE_IDLE && this->new_motion==false) + if(this->state==TIAGO_ARM_MODULE_IDLE && this->new_motion==false) return true; else return false; } -arm_module_status_t CArmModule::get_status(void) +tiago_arm_module_status_t CTiagoArmModule::get_status(void) { return this->status; } -CArmModule::~CArmModule() +CTiagoArmModule::~CTiagoArmModule() { if(!this->arm_action.is_finished()) {