diff --git a/astrobee/launch/robot/LLP.launch.py b/astrobee/launch/robot/LLP.launch.py index e40f742372..a41a9c8ded 100644 --- a/astrobee/launch/robot/LLP.launch.py +++ b/astrobee/launch/robot/LLP.launch.py @@ -36,6 +36,19 @@ def generate_launch_description(): SetEnvironmentVariable(name="ROS_HOSTNAME", condition=LaunchConfigurationNotEquals("llp", "local"), value=LaunchConfiguration("llp")), + + + DeclareLaunchArgument("drivers",), # Start platform drivers + DeclareLaunchArgument("spurn", default_value=""), # Prevent a specific node + DeclareLaunchArgument("nodes", default_value=""), # Launch specific nodes + DeclareLaunchArgument("extra", default_value=""), # Inject an additional node + DeclareLaunchArgument("debug", default_value=""), # Debug node group + + DeclareLaunchArgument("output", default_value="log"), # Output to screen or log + DeclareLaunchArgument("gtloc", default_value="false"), # Use Ground Truth Localizer + + + ComposableNodeContainer( name='llp_gnc', namespace='', @@ -46,263 +59,143 @@ def generate_launch_description(): package='ctl', plugin='ctl::CtlComponent', name='ctl', - extra_arguments=[{'use_intra_process_comms': True}]), + extra_arguments=[{'use_intra_process_comms': True}] + ), ComposableNode( package='fam', plugin='fam::FamComponent', name='fam', - extra_arguments=[{'use_intra_process_comms': True}]) + extra_arguments=[{'use_intra_process_comms': True}] + ) ] ), - # ComposableNodeContainer( - # name='llp_imu_aug', - # namespace='', - # package='rclcpp_components', - # executable='component_container', - # composable_node_descriptions=[ + ComposableNodeContainer( + name='llp_imu_aug', + namespace='', + package='rclcpp_components', + executable='component_container', + condition=IfCondition(LaunchConfiguration("gtloc")), + composable_node_descriptions=[ # ComposableNode( # package='imu_augmentor', # plugin='imu_augmentor::ImuAugmentorNodelet', # name='imu_aug', # condition=IfCondition(LaunchConfiguration("gtloc")), # extra_arguments=[{'use_intra_process_comms': True}]), - # ] - # ), - # ComposableNodeContainer( - # name='llp_monitors', - # namespace='', - # package='rclcpp_components', - # executable='component_container', - # composable_node_descriptions=[ + ] + ), + ComposableNodeContainer( + name='llp_monitors', + namespace='', + package='rclcpp_components', + executable='component_container', + condition=IfCondition(LaunchConfiguration("drivers")), + composable_node_descriptions=[ # ComposableNode( - # package='localization_manager', - # plugin='localization_manager::LocalizationManagerNodelet', - # name='localization_manager', + # package='cpu_mem_monitor', + # plugin='cpu_mem_monitor::CpuMemMonitor', + # name='llp_cpu_mem_monitor', # extra_arguments=[{'use_intra_process_comms': True}]), - # ] - # ), - # ComposableNodeContainer( - # name='llp_i2c', - # namespace='', - # package='rclcpp_components', - # executable='component_container', - # composable_node_descriptions=[ # ComposableNode( - # package='localization_manager', - # plugin='localization_manager::LocalizationManagerNodelet', - # name='localization_manager', + # package='disk_monitor', + # plugin='disk_monitor::DiskMonitor', + # name='llp_disk_monitor', # extra_arguments=[{'use_intra_process_comms': True}]), - # ] - # ), - # ComposableNodeContainer( - # name='llp_serial', - # namespace='', - # package='rclcpp_components', - # executable='component_container', - # composable_node_descriptions=[ + ] + ), + ComposableNodeContainer( + name='llp_i2c', + namespace='', + package='rclcpp_components', + executable='component_container', + condition=IfCondition(LaunchConfiguration("drivers")), + composable_node_descriptions=[ # ComposableNode( - # package='localization_manager', - # plugin='localization_manager::LocalizationManagerNodelet', - # name='localization_manager', + # package='eps_driver', + # plugin='eps_driver::EpsDriverNode', + # name='eps_driver', # extra_arguments=[{'use_intra_process_comms': True}]), - # ] - # ), - # ComposableNodeContainer( - # name='llp_pmc', - # namespace='', - # package='rclcpp_components', - # executable='component_container', - # composable_node_descriptions=[ # ComposableNode( - # package='localization_manager', - # plugin='localization_manager::LocalizationManagerNodelet', - # name='localization_manager', + # package='laser', + # plugin='laser::LaserNodelet', + # name='laser', # extra_arguments=[{'use_intra_process_comms': True}]), - # ] - # ), - # ComposableNodeContainer( - # name='llp_imu', - # namespace='', - # package='rclcpp_components', - # executable='component_container', - # composable_node_descriptions=[ # ComposableNode( - # package='localization_manager', - # plugin='localization_manager::LocalizationManagerNodelet', + # package='flashlight', + # plugin='flashlight::FlashlightNodelet', + # name='flashlight_front', + # extra_arguments=[{'use_intra_process_comms': True}]), + # ComposableNode( + # package='flashlight', + # plugin='flashlight::FlashlightNodelet', + # name='flashlight_aft', + # extra_arguments=[{'use_intra_process_comms': True}]), + ] + ), + ComposableNodeContainer( + name='llp_serial', + namespace='', + package='rclcpp_components', + executable='component_container', + condition=IfCondition(LaunchConfiguration("drivers")), + composable_node_descriptions=[ + # ComposableNode( + # package='speed_cam', + # plugin='speed_cam::SpeedCamNode', + # name='speed_cam', + # extra_arguments=[{'use_intra_process_comms': True}]), + ] + ), + ComposableNodeContainer( + name='llp_pmc', + namespace='', + package='rclcpp_components', + executable='component_container', + condition=IfCondition(LaunchConfiguration("drivers")), + composable_node_descriptions=[ + # ComposableNode( + # package='pmc_actuator', + # plugin='pmc_actuator::PmcActuatorNodelet', + # name='pmc_actuator', + # extra_arguments=[{'use_intra_process_comms': True}]), + ] + ), + ComposableNodeContainer( + name='llp_imu', + namespace='', + package='rclcpp_components', + executable='component_container', + condition=IfCondition(LaunchConfiguration("drivers")), + composable_node_descriptions=[ + # ComposableNode( + # package='epson_imu', + # plugin='epson_imu::EpsonImuNodelet', # name='localization_manager', # extra_arguments=[{'use_intra_process_comms': True}]), - # ] - # ), - # ComposableNodeContainer( - # name='llp_lights', - # namespace='', - # package='rclcpp_components', - # executable='component_container', - # composable_node_descriptions=[ # ComposableNode( - # package='localization_manager', - # plugin='localization_manager::LocalizationManagerNodelet', + # package='calibration_imu', + # plugin='epson_imu::EpsonImuNodelet', # name='localization_manager', # extra_arguments=[{'use_intra_process_comms': True}]), - # ] - # ), + ] + ), + ComposableNodeContainer( + name='llp_lights', + namespace='', + package='rclcpp_components', + executable='component_container', + condition=IfCondition(LaunchConfiguration("drivers")), + composable_node_descriptions=[ + # ComposableNode( + # package='signal_lights', + # plugin='signal_lights::SignalLightsNodelet', + # name='signal_lights', + # extra_arguments=[{'use_intra_process_comms': True}]), + ComposableNode( + package='light_flow', + plugin='light_flow::LightFlowComponent', + name='light_flow', + extra_arguments=[{'use_intra_process_comms': True}]), + ] + ), ]) - - -# - -# -# -# -# -# -# -# -# - - - -# -# -# -# -# -# -# -# -# -# -# -# -# - - -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# - -# -# -# -# -# -# -# -# -# -# - -# -# -# -# -# -# -# -# -# -# - -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# -# - -# - -# diff --git a/astrobee/launch/robot/MLP.launch.py b/astrobee/launch/robot/MLP.launch.py index 0f3b1726e8..79a230bc2d 100644 --- a/astrobee/launch/robot/MLP.launch.py +++ b/astrobee/launch/robot/MLP.launch.py @@ -53,6 +53,7 @@ def generate_launch_description(): namespace='', package='rclcpp_components', executable='component_container', + condition=IfCondition(LaunchConfiguration("gtloc")), composable_node_descriptions=[ # ComposableNode( # package='localization_manager', @@ -61,9 +62,8 @@ def generate_launch_description(): # extra_arguments=[{'use_intra_process_comms': True}]), ComposableNode( package='ground_truth_localizer', - plugin='ground_truth_localizer::GroundTruthLocalizerNodelet', + plugin='ground_truth_localizer::GroundTruthLocalizerComponent', name='ground_truth_localizer', - # condition=IfCondition(LaunchConfiguration("gtloc")) ), # ComposableNode( # package='image_sampler', @@ -75,10 +75,32 @@ def generate_launch_description(): ] ), ComposableNodeContainer( + name='mlp_localization', + namespace='', + package='rclcpp_components', + executable='component_container', + condition=UnlessCondition(LaunchConfiguration("gtloc")), + composable_node_descriptions=[ + # ComposableNode( + # package='localization_manager', + # plugin='localization_manager::LocalizationManagerNodelet', + # name='localization_manager', + # extra_arguments=[{'use_intra_process_comms': True}]), + # ComposableNode( + # package='image_sampler', + # plugin='image_sampler::ImageSampler', + # name='image_sampler', + # remappings=[('/image', '/burgerimage')], + # parameters=[{'history': 'keep_last'}], + # extra_arguments=[{'use_intra_process_comms': True}]) + ] + ), + ComposableNodeContainer( name='mlp_graph_localization', namespace='', package='rclcpp_components', executable='component_container', + condition=UnlessCondition(LaunchConfiguration("gtloc")), composable_node_descriptions=[ # ComposableNode( # package='graph_loc', @@ -92,6 +114,7 @@ def generate_launch_description(): namespace='', package='rclcpp_components', executable='component_container', + condition=IfCondition(LaunchConfiguration("drivers")), composable_node_descriptions=[ # ComposableNode( # package='depth_odometry_nodelet', @@ -149,7 +172,27 @@ def generate_launch_description(): name='mlp_depth_cam', namespace='', package='rclcpp_components', + condition=UnlessCondition(LaunchConfiguration("drivers")), executable='component_container', + composable_node_descriptions=[ + # ComposableNode( + # package='handrail_detect', + # plugin='handrail_detect::HandrailDetect', + # name='handrail_detect', + # extra_arguments=[{'use_intra_process_comms': True}]), + # ComposableNode( + # package='planner_qp', + # plugin='planner_qp::Planner', + # name='planner_qp', + # extra_arguments=[{'use_intra_process_comms': True}]), + ] + ), + ComposableNodeContainer( + name='mlp_depth_cam', + namespace='', + package='rclcpp_components', + executable='component_container', + condition=IfCondition(LaunchConfiguration("drivers")), composable_node_descriptions=[ # ComposableNode( # package='handrail_detect', @@ -222,6 +265,7 @@ def generate_launch_description(): namespace='', package='rclcpp_components', executable='component_container', + condition=IfCondition(LaunchConfiguration("drivers")), composable_node_descriptions=[ # ComposableNode( # package='sys_monitor', @@ -241,6 +285,20 @@ def generate_launch_description(): ] ), ComposableNodeContainer( + name='mlp_monitors', + namespace='', + package='rclcpp_components', + executable='component_container', + condition=UnlessCondition(LaunchConfiguration("drivers")), + composable_node_descriptions=[ + # ComposableNode( + # package='sys_monitor', + # plugin='sys_monitor::SysMonitor', + # name='sys_monitor', + # extra_arguments=[{'use_intra_process_comms': True}]), + ] + ), + ComposableNodeContainer( name='mlp_communications', namespace='', package='rclcpp_components', @@ -271,6 +329,7 @@ def generate_launch_description(): namespace='', package='rclcpp_components', executable='component_container', + condition=IfCondition(LaunchConfiguration("drivers")), composable_node_descriptions=[ # ComposableNode( # package='perching_arm', @@ -283,7 +342,7 @@ def generate_launch_description(): name='mlp_mobility', namespace='', package='rclcpp_components', - executable='component_container', + executable='component_container_mt', composable_node_descriptions=[ ComposableNode( package='choreographer', @@ -307,13 +366,12 @@ def generate_launch_description(): name='mlp_arm', namespace='', package='rclcpp_components', - executable='component_container', + executable='component_container_mt', composable_node_descriptions=[ - # ComposableNode( - # package='arm', - # plugin='arm::ArmNodelet', - # name='arm', - # extra_arguments=[{'use_intra_process_comms': True}]), + ComposableNode( + package='arm', + plugin='arm::ArmComponent', + name='arm'), ] ), ComposableNodeContainer( @@ -347,6 +405,7 @@ def generate_launch_description(): namespace='', package='rclcpp_components', executable='component_container', + condition=IfCondition(LaunchConfiguration("drivers")), composable_node_descriptions=[ # ComposableNode( # package='vive', @@ -361,11 +420,11 @@ def generate_launch_description(): package='rclcpp_components', executable='component_container', composable_node_descriptions=[ - # ComposableNode( - # package='states', - # plugin='states::StatesNodelet', - # name='states', - # extra_arguments=[{'use_intra_process_comms': True}]), + ComposableNode( + package='states', + plugin='states::StatesComponent', + name='states', + extra_arguments=[{'use_intra_process_comms': True}]), ] ), ]) diff --git a/behaviors/arm/CMakeLists.txt b/behaviors/arm/CMakeLists.txt index 0902230791..0cb21d9360 100644 --- a/behaviors/arm/CMakeLists.txt +++ b/behaviors/arm/CMakeLists.txt @@ -15,37 +15,34 @@ # License for the specific language governing permissions and limitations # under the License. -cmake_minimum_required(VERSION 3.0) +cmake_minimum_required(VERSION 3.5) project(arm) -## Compile as C++14, supported in ROS Kinetic and newer -add_compile_options(-std=c++14) -add_definitions(-DROS1) - -## Find catkin macros and libraries -find_package(catkin2 REQUIRED COMPONENTS - roscpp - std_msgs - nodelet - ff_msgs - ff_hw_msgs - ff_util -) +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() +set( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Werror -O3 -fPIC" ) -catkin_package( - LIBRARIES - arm - CATKIN_DEPENDS - roscpp - nodelet - pluginlib - actionlib - sensor_msgs - ff_msgs - ff_hw_msgs - ff_util +set(dependencies +rclcpp +rclcpp_components +ff_hw_msgs +ff_msgs +sensor_msgs +ff_util +ff_common ) +## Find ament and libraries +find_package(ament_cmake REQUIRED) + +foreach(dep ${dependencies}) + find_package(${dep} REQUIRED) +endforeach() + + + ########### ## Build ## ########### @@ -53,40 +50,33 @@ catkin_package( # Specify additional locations of header files include_directories( include - ${catkin_INCLUDE_DIRS} ) # Declare C++ libraries -add_library(arm - src/arm_nodelet.cc +add_library(arm SHARED + src/arm_component.cc ) -add_dependencies(arm ${catkin_EXPORTED_TARGETS}) -target_link_libraries(arm ${catkin_LIBRARIES}) +target_compile_definitions(arm PRIVATE "COMPOSITION_BUILDING_DLL") +ament_target_dependencies(arm ${dependencies}) +rclcpp_components_register_nodes(arm "arm::ArmComponent") ## Declare a C++ executable: arm_tool add_executable(arm_tool tools/arm_tool.cc) -add_dependencies(arm_tool ${catkin_EXPORTED_TARGETS}) +ament_target_dependencies(arm_tool ${dependencies}) target_link_libraries(arm_tool - arm gflags ${catkin_LIBRARIES}) + arm gflags) ############# ## Install ## ############# -# Mark libraries for installation -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -) - -# Mark nodelet_plugin for installation -install(FILES nodelet_plugins.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# Mark libraries and tools for installation +install(TARGETS ${PROJECT_NAME} arm_tool + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} ) -# Install C++ executables -install(TARGETS arm_tool DESTINATION bin) install(CODE "execute_process( COMMAND ln -s ../../bin/arm_tool share/${PROJECT_NAME} WORKING_DIRECTORY ${CMAKE_INSTALL_PREFIX} @@ -95,6 +85,8 @@ install(CODE "execute_process( )") # Mark launch files for installation -install(DIRECTORY launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} PATTERN ".svn" EXCLUDE) + +ament_package() \ No newline at end of file diff --git a/behaviors/arm/COLCON_IGNORE b/behaviors/arm/COLCON_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/behaviors/arm/nodelet_plugins.xml b/behaviors/arm/nodelet_plugins.xml deleted file mode 100644 index 680e03d88f..0000000000 --- a/behaviors/arm/nodelet_plugins.xml +++ /dev/null @@ -1,5 +0,0 @@ - - - Nodelet for procedures::arm - - \ No newline at end of file diff --git a/behaviors/arm/package.xml b/behaviors/arm/package.xml index bbd2b0aade..09aca4034c 100644 --- a/behaviors/arm/package.xml +++ b/behaviors/arm/package.xml @@ -1,5 +1,5 @@ - + arm 0.0.0 @@ -14,23 +14,21 @@ Astrobee Flight Software - catkin - roscpp - nodelet - pluginlib + ament_cmake + rclcpp sensor_msgs ff_msgs ff_hw_msgs ff_util - roscpp - nodelet - pluginlib - actionlib - sensor_msgs - ff_msgs - ff_hw_msgs - ff_util + + rclcpp + sensor_msgs + ff_msgs + ff_hw_msgs + ff_util + rclcpp_components + - + ament_cmake \ No newline at end of file diff --git a/behaviors/arm/src/arm_nodelet.cc b/behaviors/arm/src/arm_component.cc similarity index 73% rename from behaviors/arm/src/arm_nodelet.cc rename to behaviors/arm/src/arm_component.cc index 0eb679d40a..bce82d320e 100644 --- a/behaviors/arm/src/arm_nodelet.cc +++ b/behaviors/arm/src/arm_component.cc @@ -17,43 +17,66 @@ */ // Standard ROS includes -#include -#include -#include +#include // Standard sensor messages -#include +#include +namespace sensor_msgs { +typedef msg::JointState JointState; +} // namespace sensor_msgs // FSW shared libraries #include #include -#include +#include #include #include +#include // FSW actions, services, messages -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +namespace ff_msgs { +typedef action::Arm Arm; +typedef msg::ArmState ArmState; +typedef msg::ArmStateStamped ArmStateStamped; +typedef msg::JointSample JointSample; +typedef msg::JointSampleStamped JointSampleStamped; +typedef msg::ArmJointState ArmJointState; +typedef msg::ArmGripperState ArmGripperState; +typedef srv::SetState SetState; +} // namespace ff_msgs + +#include +#include +namespace ff_hw_msgs { +typedef srv::SetEnabled SetEnabled; +typedef srv::CalibrateGripper CalibrateGripper; +} // namespace ff_hw_msgs /** * \ingroup beh */ namespace arm { +FF_DEFINE_LOGGER("arm"); + // Different joint types enum JointType { PAN, TILT, GRIPPER }; // Perching arm servos to enable / disable - enum ServoID { - PROXIMAL_SERVO, // Proximal joint servo - DISTAL_SERVO, // Distal joint servo - GRIPPER_SERVO, // Gripper joint servo - ALL_SERVOS // Proximal, Distal and Gripper servos - }; +enum ServoID { + PROXIMAL_SERVO, // Proximal joint servo + DISTAL_SERVO, // Distal joint servo + GRIPPER_SERVO, // Gripper joint servo + ALL_SERVOS // Proximal, Distal and Gripper servos +}; // Joint information, where HUMAN = SCALE * DRIVER + OFFSET struct JointInfo { @@ -75,9 +98,9 @@ typedef std::map JointDictionary; // Match the internal states and responses with the message definition using FSM = ff_util::FSM; using STATE = ff_msgs::ArmState; -using RESPONSE = ff_msgs::ArmResult; +using RESPONSE = ff_msgs::Arm::Result; -class ArmNodelet : public ff_util::FreeFlyerNodelet { +class ArmComponent : public ff_util::FreeFlyerComponent { public: // Possible events enum : FSM::Event { @@ -98,15 +121,15 @@ class ArmNodelet : public ff_util::FreeFlyerNodelet { }; // Constructor - ArmNodelet() : ff_util::FreeFlyerNodelet(NODE_ARM, true), - fsm_(STATE::INITIALIZING, std::bind(&ArmNodelet::UpdateCallback, + explicit ArmComponent(const rclcpp::NodeOptions& options) : ff_util::FreeFlyerComponent(options, NODE_ARM, true), + fsm_(STATE::INITIALIZING, std::bind(&ArmComponent::UpdateCallback, this, std::placeholders::_1, std::placeholders::_2)) { // INITIALIZING -> UNKNOWN // [label="[0]\nREADY", color=blue]; fsm_.Add(STATE::INITIALIZING, READY, [this](FSM::Event const& event) -> FSM::State { - ROS_DEBUG("[Arm] INITIALIZING, Enabling servos"); + FF_DEBUG("[Arm] INITIALIZING, Enabling servos"); if (!EnableServo(ALL_SERVOS, true)) return Result(RESPONSE::ENABLE_FAILED); return STATE::UNKNOWN; @@ -117,7 +140,7 @@ class ArmNodelet : public ff_util::FreeFlyerNodelet { fsm_.Add(STATE::UNKNOWN, STOWED, [this](FSM::Event const& event) -> FSM::State { - ROS_DEBUG("[Arm] STOWED STATE"); + FF_DEBUG("[Arm] STOWED STATE"); return STATE::STOWED; }); @@ -126,7 +149,7 @@ class ArmNodelet : public ff_util::FreeFlyerNodelet { fsm_.Add(STATE::UNKNOWN, DEPLOYED, [this](FSM::Event const& event) -> FSM::State { - ROS_DEBUG("[Arm] DEPLOYED STATE"); + FF_DEBUG("[Arm] DEPLOYED STATE"); return STATE::DEPLOYED; }); @@ -459,19 +482,18 @@ class ArmNodelet : public ff_util::FreeFlyerNodelet { } // Destructor - ~ArmNodelet() {} + ~ArmComponent() {} protected: // Called to initialize this nodelet - void Initialize(ros::NodeHandle *nh) { + void Initialize(NodeHandle &nh) { // Grab some configuration parameters for this node from the LUA config reader - cfg_.Initialize(GetPrivateHandle(), "behaviors/arm.config"); - if (!cfg_.Listen(boost::bind( - &ArmNodelet::ReconfigureCallback, this, _1))) + cfg_.AddFile("behaviors/arm.config"); + if (!cfg_.Initialize(nh)) return AssertFault(ff_util::INITIALIZATION_FAILED, - "Could not load config"); + "Could not start config server"); - // Read the confgiuration for this specific node + // Read the configuration for this specific node config_reader::ConfigReader *cfg = cfg_.GetConfigReader(); config_reader::ConfigReader::Table joints; if (!cfg->GetTable(GetName().c_str(), &joints)) @@ -507,83 +529,59 @@ class ArmNodelet : public ff_util::FreeFlyerNodelet { dictionary_[name] = GRIPPER; // Timer for checking goal is reached - timer_watchdog_ = nh->createTimer(cfg_.Get("timeout_watchdog"), - &ArmNodelet::WatchdogCallback, this, true, false); + timer_watchdog_.createTimer(cfg_.Get("timeout_watchdog"), + std::bind(&ArmComponent::WatchdogCallback, this), nh, true, false); // Timer for checking goal is reached - timer_goal_ = nh->createTimer(cfg_.Get("timeout_goal"), - &ArmNodelet::TimeoutCallback, this, true, false); + timer_goal_.createTimer(cfg_.Get("timeout_goal"), + std::bind(&ArmComponent::TimeoutCallback, this), nh, true, false); // Publishers for arm and joint state - sub_joint_states_ = nh->subscribe(TOPIC_JOINT_STATES, 1, - &ArmNodelet::JointStateCallback, this); - pub_joint_goals_ = nh->advertise( - TOPIC_JOINT_GOALS, 1); + sub_joint_states_ = FF_CREATE_SUBSCRIBER(nh, sensor_msgs::JointState, TOPIC_JOINT_STATES, 1, + std::bind(&ArmComponent::JointStateCallback, this, std::placeholders::_1)); + pub_joint_goals_ = FF_CREATE_PUBLISHER(nh, sensor_msgs::JointState, TOPIC_JOINT_GOALS, 1); // Subscribe to Proximal Joint Servo Enabling service - client_enable_prox_servo_ = nh->serviceClient( - SERVICE_HARDWARE_PERCHING_ARM_PROX_SERVO); + client_enable_prox_servo_.Create(nh, SERVICE_HARDWARE_PERCHING_ARM_PROX_SERVO); // Subscribe to Distal Joint Servo Enabling service - client_enable_dist_servo_ = nh->serviceClient( - SERVICE_HARDWARE_PERCHING_ARM_DIST_SERVO); + client_enable_dist_servo_.Create(nh, SERVICE_HARDWARE_PERCHING_ARM_DIST_SERVO); // Subscribe to Gripper Joint Servo Enabling service - client_enable_grip_servo_ = nh->serviceClient( - SERVICE_HARDWARE_PERCHING_ARM_GRIP_SERVO); + client_enable_grip_servo_.Create(nh, SERVICE_HARDWARE_PERCHING_ARM_GRIP_SERVO); // Subscribe to Proximal Joint Servo Enabling service - client_calibrate_gripper_ = nh->serviceClient( - SERVICE_HARDWARE_PERCHING_ARM_CALIBRATE); + client_calibrate_gripper_.Create(nh, SERVICE_HARDWARE_PERCHING_ARM_CALIBRATE); // Internal state publisher - pub_state_ = nh->advertise( - TOPIC_BEHAVIORS_ARM_STATE, 1, true); + pub_state_ = FF_CREATE_PUBLISHER(nh, ff_msgs::ArmState, + TOPIC_BEHAVIORS_ARM_STATE, 1); // Allow the state to be manually set - srv_set_state_ = nh->advertiseService(SERVICE_BEHAVIORS_ARM_SET_STATE, - &ArmNodelet::SetStateCallback, this); + srv_set_state_ = FF_CREATE_SERVICE(nh, ff_msgs::SetState, SERVICE_BEHAVIORS_ARM_SET_STATE, + std::bind(&ArmComponent::SetStateCallback, this, std::placeholders::_1, std::placeholders::_2)); // Executive state publishers - pub_arm_state_ = nh->advertise( - TOPIC_BEHAVIORS_ARM_ARM_STATE, 1); - pub_joint_sample_ = nh->advertise( - TOPIC_BEHAVIORS_ARM_JOINT_SAMPLE, 1); + pub_arm_state_ = FF_CREATE_PUBLISHER(nh, ff_msgs::ArmStateStamped, TOPIC_BEHAVIORS_ARM_ARM_STATE, 1); + pub_joint_sample_ = FF_CREATE_PUBLISHER(nh, ff_msgs::JointSampleStamped, TOPIC_BEHAVIORS_ARM_JOINT_SAMPLE, 1); // Setup the ARM action server_.SetGoalCallback(std::bind( - &ArmNodelet::GoalCallback, this, std::placeholders::_1)); + &ArmComponent::GoalCallback, this, std::placeholders::_1)); server_.SetPreemptCallback(std::bind( - &ArmNodelet::PreemptCallback, this)); + &ArmComponent::PreemptCallback, this)); server_.SetCancelCallback(std::bind( - &ArmNodelet::CancelCallback, this)); + &ArmComponent::CancelCallback, this)); server_.Create(nh, ACTION_BEHAVIORS_ARM); } - // Callback to handle reconfiguration requests - bool ReconfigureCallback(dynamic_reconfigure::Config & config) { - bool success = false; - switch (fsm_.GetState()) { - case STATE::DEPLOYED: - case STATE::STOWED: - case STATE::UNKNOWN: - success = cfg_.Reconfigure(config); - joints_[PAN].tol = cfg_.Get("tol_pan"); - joints_[TILT].tol = cfg_.Get("tol_tilt"); - joints_[GRIPPER].tol = cfg_.Get("tol_gripper"); - default: - break; - } - return success; - } - // When the FSM state changes we get a callback here, so that we // can choose to do various things. void UpdateCallback(FSM::State const& state, FSM::Event const& event) { // Debug events ff_msgs::ArmState msg; msg.header.frame_id = GetPlatform(); - msg.header.stamp = ros::Time::now(); + msg.header.stamp = GetTimeNow(); msg.state = state; switch (event) { case READY: msg.fsm_event = "READY"; break; @@ -617,9 +615,9 @@ class ArmNodelet : public ff_util::FreeFlyerNodelet { case STATE::CALIBRATING: msg.fsm_state = "CALIBRATING"; break; } // Publish the state - pub_state_.publish(msg); - NODELET_DEBUG_STREAM("Received event " << msg.fsm_event); - NODELET_DEBUG_STREAM("State changed to " << msg.fsm_state); + pub_state_->publish(msg); + FF_DEBUG_STREAM("Received event " << msg.fsm_event); + FF_DEBUG_STREAM("State changed to " << msg.fsm_state); } // Complete the current action @@ -648,41 +646,41 @@ class ArmNodelet : public ff_util::FreeFlyerNodelet { } // If we need to physically send a response (we are tracking a goal) if (send) { - ff_msgs::ArmResult result; - result.response = response; + ff_msgs::Arm::Result::SharedPtr result = std::make_shared(); + result->response = response; switch (response) { - case ff_msgs::ArmResult::SUCCESS: - result.fsm_result = "Successfully completed"; break; - case ff_msgs::ArmResult::PREEMPTED: - result.fsm_result = "Action was preempted"; break; - case ff_msgs::ArmResult::INVALID_COMMAND: - result.fsm_result = "Invalid command"; break; - case ff_msgs::ArmResult::BAD_TILT_VALUE: - result.fsm_result = "Invalid value for tilt"; break; - case ff_msgs::ArmResult::BAD_PAN_VALUE: - result.fsm_result = "Invalid value for pan"; break; - case ff_msgs::ArmResult::BAD_GRIPPER_VALUE: - result.fsm_result = "Invalid value for gripper"; break; - case ff_msgs::ArmResult::NOT_ALLOWED: - result.fsm_result = "Not allowed"; break; - case ff_msgs::ArmResult::TILT_FAILED: - result.fsm_result = "Tilt command failed"; break; - case ff_msgs::ArmResult::PAN_FAILED: - result.fsm_result = "Pan command failed"; break; - case ff_msgs::ArmResult::GRIPPER_FAILED: - result.fsm_result = "Gripper command failed"; break; - case ff_msgs::ArmResult::COMMUNICATION_ERROR: - result.fsm_result = "Cannot communicate with arm"; break; - case ff_msgs::ArmResult::COLLISION_AVOIDED: - result.fsm_result = "The arm might collide, failed"; break; - case ff_msgs::ArmResult::ENABLE_FAILED: - result.fsm_result = "Cannot enable the arm servos"; break; - case ff_msgs::ArmResult::DISABLE_FAILED: - result.fsm_result = "Cannot disable the arm servos"; break; - case ff_msgs::ArmResult::CALIBRATE_FAILED: - result.fsm_result = "Cannot calibrate the gripper"; break; - case ff_msgs::ArmResult::NO_GOAL: - result.fsm_result = "Unknown call to calibration"; break; + case ff_msgs::Arm::Result::SUCCESS: + result->fsm_result = "Successfully completed"; break; + case ff_msgs::Arm::Result::PREEMPTED: + result->fsm_result = "Action was preempted"; break; + case ff_msgs::Arm::Result::INVALID_COMMAND: + result->fsm_result = "Invalid command"; break; + case ff_msgs::Arm::Result::BAD_TILT_VALUE: + result->fsm_result = "Invalid value for tilt"; break; + case ff_msgs::Arm::Result::BAD_PAN_VALUE: + result->fsm_result = "Invalid value for pan"; break; + case ff_msgs::Arm::Result::BAD_GRIPPER_VALUE: + result->fsm_result = "Invalid value for gripper"; break; + case ff_msgs::Arm::Result::NOT_ALLOWED: + result->fsm_result = "Not allowed"; break; + case ff_msgs::Arm::Result::TILT_FAILED: + result->fsm_result = "Tilt command failed"; break; + case ff_msgs::Arm::Result::PAN_FAILED: + result->fsm_result = "Pan command failed"; break; + case ff_msgs::Arm::Result::GRIPPER_FAILED: + result->fsm_result = "Gripper command failed"; break; + case ff_msgs::Arm::Result::COMMUNICATION_ERROR: + result->fsm_result = "Cannot communicate with arm"; break; + case ff_msgs::Arm::Result::COLLISION_AVOIDED: + result->fsm_result = "The arm might collide, failed"; break; + case ff_msgs::Arm::Result::ENABLE_FAILED: + result->fsm_result = "Cannot enable the arm servos"; break; + case ff_msgs::Arm::Result::DISABLE_FAILED: + result->fsm_result = "Cannot disable the arm servos"; break; + case ff_msgs::Arm::Result::CALIBRATE_FAILED: + result->fsm_result = "Cannot calibrate the gripper"; break; + case ff_msgs::Arm::Result::NO_GOAL: + result->fsm_result = "Unknown call to calibration"; break; } if (response > 0) server_.SendResult(ff_util::FreeFlyerActionState::SUCCESS, result); @@ -702,17 +700,36 @@ class ArmNodelet : public ff_util::FreeFlyerNodelet { } // Called on registration of aplanner - bool SetStateCallback(ff_msgs::SetState::Request& req, - ff_msgs::SetState::Response& res) { - fsm_.SetState(req.state); - res.success = true; + bool SetStateCallback(const std::shared_ptr req, + std::shared_ptr res) { + fsm_.SetState(req->state); + res->success = true; UpdateCallback(fsm_.GetState(), MANUAL_STATE_SET); return true; } + // Get tolerance value + double GetTolerance(JointType t) { + switch (fsm_.GetState()) { + case STATE::DEPLOYED: + case STATE::STOWED: + case STATE::UNKNOWN: + if (t == PAN) + joints_[PAN].tol = cfg_.Get("tol_pan"); + else if (t == TILT) + joints_[TILT].tol = cfg_.Get("tol_tilt"); + else if (t == GRIPPER) + joints_[GRIPPER].tol = cfg_.Get("tol_gripper"); + return joints_[t].tol; + default: + break; + } + return joints_[PAN].tol; + } + // Check if the two angle are sufficiently close, respecting modular math bool Equal(JointType t, double v) { - return ((180. - fabs(fabs(joints_[t].val - v) - 180.)) < joints_[t].tol); + return ((180. - fabs(fabs(joints_[t].val - v) - 180.)) < GetTolerance(t)); } // Look at the pan and tilt angles to determine if stowed @@ -735,12 +752,12 @@ class ArmNodelet : public ff_util::FreeFlyerNodelet { // Check that we actually have the joint present JointMap::const_iterator it = joints_.find(type); if (it == joints_.end()) { - NODELET_WARN_STREAM("Not a valid control goal"); + FF_WARN_STREAM("Not a valid control goal"); return false; } // Package up the joint state goal static sensor_msgs::JointState goal; - goal.header.stamp = ros::Time::now(); + goal.header.stamp = GetTimeNow(); goal.header.frame_id = GetPlatform(); goal.name.resize(1); goal.position.resize(1); @@ -751,23 +768,23 @@ class ArmNodelet : public ff_util::FreeFlyerNodelet { if (type == GRIPPER) goal.position[0] -= 100.0; // Publish the new goal - pub_joint_goals_.publish(goal); + pub_joint_goals_->publish(goal); // Start a timer timer_goal_.stop(); timer_goal_.setPeriod( - ros::Duration(cfg_.Get("timeout_goal"))); + rclcpp::Duration::from_seconds(cfg_.Get("timeout_goal"))); timer_goal_.start(); // Success! return true; } // Called whenever the low-level driver produces updated joint states. - void JointStateCallback(sensor_msgs::JointState::ConstPtr const& msg) { + void JointStateCallback(const std::shared_ptr msg) { /////////////////////////// // Send the joint sample // /////////////////////////// ff_msgs::JointSampleStamped jss; - jss.header.stamp = ros::Time::now(); + jss.header.stamp = GetTimeNow(); jss.header.frame_id = GetPlatform(); jss.samples.clear(); // Iterate over data @@ -793,8 +810,7 @@ class ArmNodelet : public ff_util::FreeFlyerNodelet { return; // Reset the watchdog timer timer_watchdog_.stop(); - timer_watchdog_.setPeriod( - ros::Duration(cfg_.Get("timeout_watchdog"))); + timer_watchdog_.setPeriod(cfg_.Get("timeout_watchdog")); timer_watchdog_.start(); // Update the state machine switch (fsm_.GetState()) { @@ -854,15 +870,15 @@ class ArmNodelet : public ff_util::FreeFlyerNodelet { return; } // Publish the updated joint samples - pub_joint_sample_.publish(jss); + pub_joint_sample_->publish(jss); ///////////////////////// // Send the full state // ///////////////////////// ff_msgs::ArmStateStamped state_msg; state_msg.header.frame_id = GetPlatform(); - state_msg.header.stamp = ros::Time::now(); + state_msg.header.stamp = GetTimeNow(); // Convert our internal state to an ArmGripperState - if (fabs(joints_[GRIPPER].val - K_GRIPPER_CLOSE) < joints_[GRIPPER].tol) + if (fabs(joints_[GRIPPER].val - K_GRIPPER_CLOSE) < GetTolerance(GRIPPER)) state_msg.gripper_state.state = ff_msgs::ArmGripperState::CLOSED; else state_msg.gripper_state.state = ff_msgs::ArmGripperState::OPEN; @@ -899,11 +915,11 @@ class ArmNodelet : public ff_util::FreeFlyerNodelet { state_msg.joint_state.state = ff_msgs::ArmJointState::STOWED; break; } - pub_arm_state_.publish(state_msg); + pub_arm_state_->publish(state_msg); ////////////////////////// // Send action feedback // ////////////////////////// - static ff_msgs::ArmFeedback feedback; + static ff_msgs::Arm::Feedback::SharedPtr feedback = std::make_shared(); switch (fsm_.GetState()) { case STATE::PANNING: case STATE::TILTING: @@ -913,10 +929,10 @@ class ArmNodelet : public ff_util::FreeFlyerNodelet { case STATE::STOWING_TILTING: case STATE::DEPLOYING_PANNING: case STATE::DEPLOYING_TILTING: - feedback.state.state = fsm_.GetState(); - feedback.pan = joints_[PAN].val; - feedback.tilt = joints_[TILT].val; - feedback.gripper = joints_[GRIPPER].val; + feedback->state.state = fsm_.GetState(); + feedback->pan = joints_[PAN].val; + feedback->tilt = joints_[TILT].val; + feedback->gripper = joints_[GRIPPER].val; server_.SendFeedback(feedback); default: break; @@ -924,39 +940,39 @@ class ArmNodelet : public ff_util::FreeFlyerNodelet { } // Called to fake feedback to executive during a prep - void TimeoutCallback(ros::TimerEvent const& event) { + void TimeoutCallback() { fsm_.Update(TIMEOUT); } // If the watchdog expires, it means that after connecting we went for a // specified period without joint state feedback. In this case we need // to send a communication error to the callee. - void WatchdogCallback(ros::TimerEvent const& event) { + void WatchdogCallback() { fsm_.SetState(Result(RESPONSE::COMMUNICATION_ERROR)); } // A new arm action has been called - void GoalCallback(ff_msgs::ArmGoalConstPtr const& goal) { + void GoalCallback(std::shared_ptr goal) { // We are connected switch (goal->command) { // Stop the arm - case ff_msgs::ArmGoal::ARM_STOP: - NODELET_DEBUG_STREAM("Received a new ARM_STOP command"); + case ff_msgs::Arm::Goal::ARM_STOP: + FF_DEBUG_STREAM("Received a new ARM_STOP command"); joints_[PAN].goal = joints_[PAN].val; joints_[TILT].goal = joints_[TILT].val; joints_[GRIPPER].goal = joints_[GRIPPER].val; return fsm_.Update(GOAL_CANCEL); // Deploy the arm - case ff_msgs::ArmGoal::ARM_DEPLOY: - NODELET_DEBUG_STREAM("Received a new ARM_DEPLOY command"); + case ff_msgs::Arm::Goal::ARM_DEPLOY: + FF_DEBUG_STREAM("Received a new ARM_DEPLOY command"); joints_[PAN].goal = K_PAN_DEPLOY; joints_[TILT].goal = K_TILT_DEPLOY; joints_[GRIPPER].goal = K_GRIPPER_DEPLOY; return fsm_.Update(GOAL_DEPLOY); break; // Stow the arm - case ff_msgs::ArmGoal::ARM_STOW: - NODELET_DEBUG_STREAM("Received a new ARM_STOW command"); + case ff_msgs::Arm::Goal::ARM_STOW: + FF_DEBUG_STREAM("Received a new ARM_STOW command"); if (fsm_.GetState() == STATE::STOWED) { Result(RESPONSE::SUCCESS, true); } @@ -966,20 +982,20 @@ class ArmNodelet : public ff_util::FreeFlyerNodelet { return fsm_.Update(GOAL_STOW); break; // Pan the arm - case ff_msgs::ArmGoal::ARM_PAN: - case ff_msgs::ArmGoal::ARM_TILT: - case ff_msgs::ArmGoal::ARM_MOVE: - NODELET_DEBUG_STREAM("Received a new ARM_{PAN,TILT,MOVE} command"); + case ff_msgs::Arm::Goal::ARM_PAN: + case ff_msgs::Arm::Goal::ARM_TILT: + case ff_msgs::Arm::Goal::ARM_MOVE: + FF_DEBUG_STREAM("Received a new ARM_{PAN,TILT,MOVE} command"); if (fsm_.GetState() == STATE::DEPLOYED || fsm_.GetState() == STATE::STOWED) { // Get the new, proposed PAN and TILT values double new_p = joints_[PAN].goal; - if (goal->command == ff_msgs::ArmGoal::ARM_MOVE || - goal->command == ff_msgs::ArmGoal::ARM_PAN) + if (goal->command == ff_msgs::Arm::Goal::ARM_MOVE || + goal->command == ff_msgs::Arm::Goal::ARM_PAN) new_p = goal->pan; double new_t = joints_[TILT].goal; - if (goal->command == ff_msgs::ArmGoal::ARM_MOVE || - goal->command == ff_msgs::ArmGoal::ARM_TILT) + if (goal->command == ff_msgs::Arm::Goal::ARM_MOVE || + goal->command == ff_msgs::Arm::Goal::ARM_TILT) new_t = goal->tilt; // Simple bounds and self-collision checking if (new_t < K_TILT_MIN || new_t > K_TILT_MAX) { @@ -993,7 +1009,7 @@ class ArmNodelet : public ff_util::FreeFlyerNodelet { // Check current and goal tilt, and if close to stowed, make // sure the pan value is zero within tolerance if ((joints_[TILT].goal > K_TILT_SAFE || new_t > K_TILT_SAFE) - && fabs(new_p - K_PAN_STOW) > joints_[PAN].tol) { + && fabs(new_p - K_PAN_STOW) > GetTolerance(PAN)) { Result(RESPONSE::COLLISION_AVOIDED, true); return; } @@ -1010,8 +1026,8 @@ class ArmNodelet : public ff_util::FreeFlyerNodelet { } break; // Set the gripper - case ff_msgs::ArmGoal::GRIPPER_SET: - NODELET_DEBUG_STREAM("Received a new GRIPPER_SET command"); + case ff_msgs::Arm::Goal::GRIPPER_SET: + FF_DEBUG_STREAM("Received a new GRIPPER_SET command"); if (fsm_.GetState() == STATE::DEPLOYED) { // Check that th gripper value is reasonable if (goal->gripper < K_GRIPPER_CLOSE || goal->gripper > K_GRIPPER_OPEN) { @@ -1025,23 +1041,23 @@ class ArmNodelet : public ff_util::FreeFlyerNodelet { } break; // Open the gripper - case ff_msgs::ArmGoal::GRIPPER_OPEN: - NODELET_DEBUG_STREAM("Received a new GRIPPER_OPEN command"); + case ff_msgs::Arm::Goal::GRIPPER_OPEN: + FF_DEBUG_STREAM("Received a new GRIPPER_OPEN command"); if (fsm_.GetState() == STATE::DEPLOYED) { joints_[GRIPPER].goal = K_GRIPPER_OPEN; return fsm_.Update(GOAL_SET); } break; // Close the gripper - case ff_msgs::ArmGoal::GRIPPER_CLOSE: - NODELET_DEBUG_STREAM("Received a new GRIPPER_CLOSE command"); + case ff_msgs::Arm::Goal::GRIPPER_CLOSE: + FF_DEBUG_STREAM("Received a new GRIPPER_CLOSE command"); if (fsm_.GetState() == STATE::DEPLOYED) { joints_[GRIPPER].goal = K_GRIPPER_CLOSE; return fsm_.Update(GOAL_SET); } break; - case ff_msgs::ArmGoal::DISABLE_SERVO: - NODELET_DEBUG_STREAM("Received a new DISABLE_SERVO command"); + case ff_msgs::Arm::Goal::DISABLE_SERVO: + FF_DEBUG_STREAM("Received a new DISABLE_SERVO command"); if (fsm_.GetState() == STATE::STOWED) { return fsm_.Update(GOAL_DISABLE); } @@ -1073,12 +1089,14 @@ class ArmNodelet : public ff_util::FreeFlyerNodelet { bool CalibrateGripper(void) { bool success = false; // CalibrateGripper callback doesn't have a request field - ff_hw_msgs::CalibrateGripper enable_srv; + ff_hw_msgs::CalibrateGripper::Request enable_req; + auto response = std::make_shared(); + timer_goal_.setPeriod( - ros::Duration(cfg_.Get("timeout_goal"))); + rclcpp::Duration::from_seconds(cfg_.Get("timeout_goal"))); timer_goal_.start(); - if (client_calibrate_gripper_.call(enable_srv)) { - success = enable_srv.response.success; + if (client_calibrate_gripper_.call(enable_req, response)) { + success = response->success; } if (success) calibrated_ = true; @@ -1088,59 +1106,70 @@ class ArmNodelet : public ff_util::FreeFlyerNodelet { // Enable / Disable the perching arm servos bool EnableServo(ServoID servo_number, bool servo_enable) { bool success = false; - ff_hw_msgs::SetEnabled enable_srv; - enable_srv.request.enabled = servo_enable; - switch (servo_number) { + ff_hw_msgs::SetEnabled::Request enable_req; + auto response = std::make_shared(); + enable_req.enabled = servo_enable; + switch (servo_number) { case PROXIMAL_SERVO: // Proximal - if (client_enable_prox_servo_.call(enable_srv)) - success = enable_srv.response.success; + client_enable_prox_servo_.waitForExistence(5.0); + if (client_enable_prox_servo_.isValid() && client_enable_prox_servo_.call(enable_req, response)) + success = response->success; break; case DISTAL_SERVO: // Distal - if (client_enable_dist_servo_.call(enable_srv)) - success = enable_srv.response.success; + client_enable_dist_servo_.waitForExistence(5.0); + if (client_enable_dist_servo_.isValid() && client_enable_dist_servo_.call(enable_req, response)) + success = response->success; break; case GRIPPER_SERVO: // Gripper - if (client_enable_grip_servo_.call(enable_srv)) - success = enable_srv.response.success; + client_enable_grip_servo_.waitForExistence(5.0); + if (client_enable_grip_servo_.isValid() && client_enable_grip_servo_.call(enable_req, response)) + success = response->success; break; case ALL_SERVOS: // All - if (client_enable_prox_servo_.call(enable_srv)) - success = enable_srv.response.success; - if (client_enable_dist_servo_.call(enable_srv)) - success = success && enable_srv.response.success; - if (client_enable_grip_servo_.call(enable_srv)) - success = success && enable_srv.response.success; + client_enable_prox_servo_.waitForExistence(5.0); + if (client_enable_prox_servo_.isValid() && client_enable_prox_servo_.call(enable_req, response)) + success = response->success; + client_enable_dist_servo_.waitForExistence(5.0); + if (client_enable_dist_servo_.isValid() && client_enable_dist_servo_.call(enable_req, response)) + success = success && response->success; + client_enable_grip_servo_.waitForExistence(5.0); + if (client_enable_grip_servo_.isValid() && client_enable_grip_servo_.call(enable_req, response)) + success = success && response->success; break; default: - NODELET_WARN("Unknown servo number"); + FF_WARN("Unknown servo number"); break; } return success; } + + + protected: // Finite state machine ff_util::FSM fsm_; // Config server ff_util::ConfigServer cfg_; // Action server - ff_util::FreeFlyerActionServer server_; + ff_util::FreeFlyerActionServer server_; // Data structures supporting lookup of joint data JointMap joints_; JointDictionary dictionary_; // Timers, publishers and subscribers - ros::Timer timer_goal_; // Timer for goal to complete - ros::Timer timer_watchdog_; // Watchdog timer for LL data - ros::Publisher pub_state_; // State publisher - ros::ServiceServer srv_set_state_; // Set the current state - ros::Subscriber sub_joint_states_; // Joint state subscriber - ros::Publisher pub_joint_goals_; // Joint goal publisher - ros::Publisher pub_arm_state_; // Executive arm state publisher - ros::Publisher pub_joint_sample_; // Executive joint state publisher - ros::ServiceClient client_enable_prox_servo_; - ros::ServiceClient client_enable_dist_servo_; - ros::ServiceClient client_enable_grip_servo_; - ros::ServiceClient client_calibrate_gripper_; + ff_util::FreeFlyerTimer timer_goal_; // Timer for goal to complete + ff_util::FreeFlyerTimer timer_watchdog_; // Watchdog timer for LL data + rclcpp::Publisher::SharedPtr pub_state_; // State publisher + rclcpp::Service::SharedPtr srv_set_state_; // Set the current state + rclcpp::Subscription::SharedPtr sub_joint_states_; // Joint state subscriber + rclcpp::Publisher::SharedPtr pub_joint_goals_; // Joint goal publisher + rclcpp::Publisher::SharedPtr pub_arm_state_; // Executive arm state publisher + rclcpp::Publisher::SharedPtr pub_joint_sample_; // Executive joint state publisher + ff_util::FreeFlyerServiceClient client_enable_prox_servo_; + ff_util::FreeFlyerServiceClient client_enable_dist_servo_; + ff_util::FreeFlyerServiceClient client_enable_grip_servo_; + ff_util::FreeFlyerServiceClient client_calibrate_gripper_; + // Constant vales static constexpr double K_PAN_OFFSET = 0.0; static constexpr double K_PAN_MIN = -90.0; @@ -1166,6 +1195,11 @@ class ArmNodelet : public ff_util::FreeFlyerNodelet { bool goal_set_ = false; }; -PLUGINLIB_EXPORT_CLASS(arm::ArmNodelet, nodelet::Nodelet); - } // namespace arm + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader. +// This acts as a sort of entry point, allowing the component to be discoverable when its library +// is being loaded into a running process. +RCLCPP_COMPONENTS_REGISTER_NODE(arm::ArmComponent) diff --git a/behaviors/arm/tools/arm_tool.cc b/behaviors/arm/tools/arm_tool.cc index 2b7a55426a..f432a49286 100644 --- a/behaviors/arm/tools/arm_tool.cc +++ b/behaviors/arm/tools/arm_tool.cc @@ -21,17 +21,23 @@ #include // Include RPOS -#include +#include // FSW includes #include #include #include // Action -#include -#include -#include -#include +#include +#include +#include +#include +namespace ff_msgs { +typedef msg::ArmState ArmState; +typedef msg::JointSampleStamped JointSampleStamped; +typedef msg::JointSample JointSample; +typedef action::Arm Arm; +} // C++ STL includes #include @@ -65,7 +71,7 @@ DEFINE_double(deadline, -1.0, "Action deadline timeout"); // Arm action result void ResultCallback(ff_util::FreeFlyerActionState::Enum result_code, - ff_msgs::ArmResultConstPtr const& result) { + std::shared_ptr result) { switch (result_code) { // Result will be a null pointer case ff_util::FreeFlyerActionState::Enum::TIMEOUT_ON_CONNECT: @@ -91,11 +97,11 @@ void ResultCallback(ff_util::FreeFlyerActionState::Enum result_code, break; } // In all cases we must shutdown - ros::shutdown(); + rclcpp::shutdown(); } // Arm action feedback -void FeedbackCallback(ff_msgs::ArmFeedbackConstPtr const& feedback) { +void FeedbackCallback(const std::shared_ptr feedback) { // Determine the state static std::string str; switch (feedback->state.state) { @@ -124,7 +130,7 @@ void FeedbackCallback(ff_msgs::ArmFeedbackConstPtr const& feedback) { // Ensure all clients are connected void ConnectedCallback( - ff_util::FreeFlyerActionClient *client) { + ff_util::FreeFlyerActionClient *client) { // Check to see if connected if (!client->IsConnected()) return; // Print out a status message @@ -132,26 +138,26 @@ void ConnectedCallback( << "State: CONNECTED" << " "; // Prepare the goal - ff_msgs::ArmGoal goal; - if (FLAGS_open) goal.command = ff_msgs::ArmGoal::GRIPPER_OPEN; - else if (FLAGS_close) goal.command = ff_msgs::ArmGoal::GRIPPER_CLOSE; - else if (FLAGS_stow) goal.command = ff_msgs::ArmGoal::ARM_STOW; - else if (FLAGS_deploy) goal.command = ff_msgs::ArmGoal::ARM_DEPLOY; - else if (FLAGS_stop) goal.command = ff_msgs::ArmGoal::ARM_STOP; + ff_msgs::Arm::Goal goal; + if (FLAGS_open) goal.command = ff_msgs::Arm::Goal::GRIPPER_OPEN; + else if (FLAGS_close) goal.command = ff_msgs::Arm::Goal::GRIPPER_CLOSE; + else if (FLAGS_stow) goal.command = ff_msgs::Arm::Goal::ARM_STOW; + else if (FLAGS_deploy) goal.command = ff_msgs::Arm::Goal::ARM_DEPLOY; + else if (FLAGS_stop) goal.command = ff_msgs::Arm::Goal::ARM_STOP; if (!std::isinf(FLAGS_set)) { - goal.command = ff_msgs::ArmGoal::GRIPPER_SET; + goal.command = ff_msgs::Arm::Goal::GRIPPER_SET; goal.gripper = FLAGS_set; } if (!std::isinf(FLAGS_pan)) { - goal.command = ff_msgs::ArmGoal::ARM_PAN; + goal.command = ff_msgs::Arm::Goal::ARM_PAN; goal.pan = FLAGS_pan; } if (!std::isinf(FLAGS_tilt)) { - goal.command = ff_msgs::ArmGoal::ARM_TILT; + goal.command = ff_msgs::Arm::Goal::ARM_TILT; goal.tilt= FLAGS_tilt; } if (!std::isinf(FLAGS_pan) && !std::isinf(FLAGS_tilt)) - goal.command = ff_msgs::ArmGoal::ARM_MOVE; + goal.command = ff_msgs::Arm::Goal::ARM_MOVE; // Send the goal client->SendGoal(goal); } @@ -159,7 +165,9 @@ void ConnectedCallback( // Main entry point for application int main(int argc, char *argv[]) { // Initialize a ros node - ros::init(argc, argv, "control", ros::init_options::AnonymousName); + // TODO(ana): AnonymousName option has not yet been implemented in ROS2, but it is in the works + // https://github.com/ros2/rcl/issues/1034 + rclcpp::init(argc, argv); //, "control", ros::init_options::AnonymousName); // Gather some data from the command google::SetUsageMessage("Usage: rosrun arm arm_tool "); google::SetVersionString("0.1.0"); @@ -182,9 +190,9 @@ int main(int argc, char *argv[]) { return 1; } // Action clients - ff_util::FreeFlyerActionClient client; + ff_util::FreeFlyerActionClient client; // Create a node handle - ros::NodeHandle nh(std::string("/") + FLAGS_ns); + auto nh = std::make_shared("arm_tool"); // Setup SWITCH action client.SetConnectedTimeout(FLAGS_connect); client.SetActiveTimeout(FLAGS_active); @@ -196,13 +204,13 @@ int main(int argc, char *argv[]) { client.SetResultCallback(std::bind(ResultCallback, std::placeholders::_1, std::placeholders::_2)); client.SetConnectedCallback(std::bind(ConnectedCallback, &client)); - client.Create(&nh, ACTION_BEHAVIORS_ARM); + client.Create(nh, ACTION_BEHAVIORS_ARM); // Print out a status message std::cout << '\r' << std::flush << "State: CONNECTING" << " "; // Synchronous mode - ros::spin(); + rclcpp::spin(nh); // Finish commandline flags google::ShutDownCommandLineFlags(); // Make for great success diff --git a/behaviors/light_flow/CMakeLists.txt b/behaviors/light_flow/CMakeLists.txt index 5a0e6c202c..fed5e3df28 100644 --- a/behaviors/light_flow/CMakeLists.txt +++ b/behaviors/light_flow/CMakeLists.txt @@ -57,13 +57,13 @@ ament_target_dependencies(light_flow rclcpp ff_hw_msgs ff_util ff_common) target_link_libraries(light_flow jsoncpp) # Declare C++ libraries -add_library(light_flow_nodelet SHARED - src/light_flow_nodelet/light_flow_nodelet.cc +add_library(light_flow_component SHARED + src/light_flow_component/light_flow_component.cc ) -target_compile_definitions(light_flow_nodelet PRIVATE "COMPOSITION_BUILDING_DLL") -target_link_libraries(light_flow_nodelet light_flow jsoncpp) -ament_target_dependencies(light_flow_nodelet ${dependencies}) -rclcpp_components_register_nodes(light_flow_nodelet "light_flow_nodelet::LightFlowNodelet") +target_compile_definitions(light_flow_component PRIVATE "COMPOSITION_BUILDING_DLL") +target_link_libraries(light_flow_component light_flow jsoncpp) +ament_target_dependencies(light_flow_component ${dependencies}) +rclcpp_components_register_nodes(light_flow_component "light_flow::LightFlowComponent") ## Declare a C++ executable: inspection_tool add_executable(light_flow_tool tools/light_flow_tool.cc) @@ -77,18 +77,13 @@ target_link_libraries(light_flow_tool # Mark libraries for installation install(TARGETS ${PROJECT_NAME} + ${PROJECT_NAME}_component + light_flow_tool ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) -install(TARGETS ${PROJECT_NAME}_nodelet - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin + RUNTIME DESTINATION lib/${PROJECT_NAME} ) -# Install C++ executables -install(TARGETS light_flow_tool DESTINATION bin) install(CODE "execute_process( COMMAND ln -s ../../bin/light_flow_tool share/${PROJECT_NAME} WORKING_DIRECTORY ${CMAKE_INSTALL_PREFIX} diff --git a/behaviors/light_flow/COLCON_IGNORE b/behaviors/light_flow/COLCON_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/behaviors/light_flow/src/light_flow_nodelet/light_flow_nodelet.cc b/behaviors/light_flow/src/light_flow_component/light_flow_component.cc similarity index 91% rename from behaviors/light_flow/src/light_flow_nodelet/light_flow_nodelet.cc rename to behaviors/light_flow/src/light_flow_component/light_flow_component.cc index f675d2c257..edf175401e 100644 --- a/behaviors/light_flow/src/light_flow_nodelet/light_flow_nodelet.cc +++ b/behaviors/light_flow/src/light_flow_component/light_flow_component.cc @@ -49,22 +49,21 @@ namespace light_flow { FF_DEFINE_LOGGER("light_flow"); -class LightFlowNodelet : public ff_util::FreeFlyerComponent { +class LightFlowComponent : public ff_util::FreeFlyerComponent { public: - explicit LightFlowNodelet(const rclcpp::NodeOptions& options) : + explicit LightFlowComponent(const rclcpp::NodeOptions& options) : ff_util::FreeFlyerComponent(options, "light_flow", true) {} - virtual ~LightFlowNodelet() {} + virtual ~LightFlowComponent() {} protected: virtual void Initialize(NodeHandle &nh) { // this service is a special case for when we need to light // two AMBER leds on each side only when we are streaming // live video - streaming_service_ = - nh->create_service(SERVICE_STREAMING_LIGHTS, - std::bind(&LightFlowNodelet::StreamingLightsCallback, - this, std::placeholders::_1, std::placeholders::_2)); + streaming_service_ = FF_CREATE_SERVICE(nh, ff_msgs::SetStreamingLights, SERVICE_STREAMING_LIGHTS, + std::bind(&LightFlowComponent::StreamingLightsCallback, this, std::placeholders::_1, std::placeholders::_2)); + currentStreamingLightsState = false; pub_ = FF_CREATE_PUBLISHER(nh, ff_hw_msgs::ConfigureLEDGroup, @@ -119,7 +118,7 @@ class LightFlowNodelet : public ff_util::FreeFlyerComponent { } sub_ = FF_CREATE_SUBSCRIBER(nh, ff_msgs::SignalState, TOPIC_SIGNALS, 5, - std::bind(&LightFlowNodelet::ConfigureCallback, this, std::placeholders::_1)); + std::bind(&LightFlowComponent::ConfigureCallback, this, std::placeholders::_1)); } void ConfigureCallback(const std::shared_ptr msg) { @@ -221,4 +220,4 @@ class LightFlowNodelet : public ff_util::FreeFlyerComponent { // Register the component with class_loader. // This acts as a sort of entry point, allowing the component to be discoverable when its library // is being loaded into a running process. -RCLCPP_COMPONENTS_REGISTER_NODE(light_flow::LightFlowNodelet) +RCLCPP_COMPONENTS_REGISTER_NODE(light_flow::LightFlowComponent) diff --git a/behaviors/states/CMakeLists.txt b/behaviors/states/CMakeLists.txt index afa939260e..0f642bab4e 100644 --- a/behaviors/states/CMakeLists.txt +++ b/behaviors/states/CMakeLists.txt @@ -37,7 +37,7 @@ ff_common find_package(ament_cmake REQUIRED) foreach(dep ${dependencies}) - find_package(${dep}) + find_package(${dep} REQUIRED) endforeach() ########### @@ -51,11 +51,11 @@ include_directories( # Declare C++ libraries add_library(states SHARED - src/states_nodelet/states_nodelet.cc + src/states_component/states_component.cc ) target_compile_definitions(states PRIVATE "COMPOSITION_BUILDING_DLL") ament_target_dependencies(states rclcpp rclcpp_components ${dependencies}) -rclcpp_components_register_nodes(states "states::StateNodelet") +rclcpp_components_register_nodes(states "states::StatesComponent") ############# ## Install ## @@ -65,7 +65,7 @@ rclcpp_components_register_nodes(states "states::StateNodelet") install(TARGETS ${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - RUNTIME DESTINATION bin + RUNTIME DESTINATION lib/${PROJECT_NAME} ) # Mark nodelet_plugin for installation diff --git a/behaviors/states/COLCON_IGNORE b/behaviors/states/COLCON_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/behaviors/states/src/states_nodelet/states_nodelet.cc b/behaviors/states/src/states_component/states_component.cc similarity index 94% rename from behaviors/states/src/states_nodelet/states_nodelet.cc rename to behaviors/states/src/states_component/states_component.cc index 89d74c4c5d..7d1e4c34e4 100644 --- a/behaviors/states/src/states_nodelet/states_nodelet.cc +++ b/behaviors/states/src/states_component/states_component.cc @@ -58,11 +58,11 @@ FF_DEFINE_LOGGER("states"); namespace states { -class StatesNodelet : public ff_util::FreeFlyerComponent { +class StatesComponent : public ff_util::FreeFlyerComponent { public: - explicit StatesNodelet(const rclcpp::NodeOptions& options) : ff_util::FreeFlyerComponent(options, "states", true) {} + explicit StatesComponent(const rclcpp::NodeOptions& options) : ff_util::FreeFlyerComponent(options, "states", true) {} - virtual ~StatesNodelet() { + virtual ~StatesComponent() { // on nodelet shutdown, send a shutdown message on TOPIC_SIGNALS Shutdown(); } @@ -75,7 +75,7 @@ class StatesNodelet : public ff_util::FreeFlyerComponent { SERVICE_STREAMING_LIGHTS); pub_ = FF_CREATE_PUBLISHER(nh, ff_msgs::SignalState, TOPIC_SIGNALS, 1); sub_.push_back(FF_CREATE_SUBSCRIBER(nh, ff_msgs::CameraStatesStamped, TOPIC_MANAGEMENT_CAMERA_STATE, 1, - std::bind(&StatesNodelet::LiveStreamingCallback, this, std::placeholders::_1))); + std::bind(&StatesComponent::LiveStreamingCallback, this, std::placeholders::_1))); // on nodelet start, send a wakeup message on TOPIC_SIGNALS Startup(); } @@ -192,5 +192,5 @@ class StatesNodelet : public ff_util::FreeFlyerComponent { // Register the component with class_loader. // This acts as a sort of entry point, allowing the component to be discoverable when its library // is being loaded into a running process. -RCLCPP_COMPONENTS_REGISTER_NODE(states::StatesNodelet) +RCLCPP_COMPONENTS_REGISTER_NODE(states::StatesComponent) diff --git a/localization/ground_truth_localizer/CMakeLists.txt b/localization/ground_truth_localizer/CMakeLists.txt index ea4bdc318e..0d1bcf827f 100644 --- a/localization/ground_truth_localizer/CMakeLists.txt +++ b/localization/ground_truth_localizer/CMakeLists.txt @@ -56,14 +56,14 @@ include_directories( # Declare C++ libraries add_library(${PROJECT_NAME} SHARED - src/ground_truth_localizer_nodelet.cc + src/ground_truth_localizer_component.cc src/utilities.cc ) target_compile_definitions(${PROJECT_NAME} PRIVATE "COMPOSITION_BUILDING_DLL") target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES}) ament_target_dependencies(${PROJECT_NAME} rclcpp rclcpp_components ff_msgs ff_util localization_common std_srvs) -rclcpp_components_register_nodes(${PROJECT_NAME} "ground_truth_localizer::GroundTruthLocalizerNodelet") +rclcpp_components_register_nodes(${PROJECT_NAME} "ground_truth_localizer::GroundTruthLocalizerComponent") ############# diff --git a/localization/ground_truth_localizer/include/ground_truth_localizer/ground_truth_localizer_nodelet.h b/localization/ground_truth_localizer/include/ground_truth_localizer/ground_truth_localizer_component.h similarity index 90% rename from localization/ground_truth_localizer/include/ground_truth_localizer/ground_truth_localizer_nodelet.h rename to localization/ground_truth_localizer/include/ground_truth_localizer/ground_truth_localizer_component.h index 54d3ff0260..a029966f19 100644 --- a/localization/ground_truth_localizer/include/ground_truth_localizer/ground_truth_localizer_nodelet.h +++ b/localization/ground_truth_localizer/include/ground_truth_localizer/ground_truth_localizer_component.h @@ -15,8 +15,8 @@ * License for the specific language governing permissions and limitations * under the License. */ -#ifndef GROUND_TRUTH_LOCALIZER_GROUND_TRUTH_LOCALIZER_NODELET_H_ -#define GROUND_TRUTH_LOCALIZER_GROUND_TRUTH_LOCALIZER_NODELET_H_ +#ifndef GROUND_TRUTH_LOCALIZER_GROUND_TRUTH_LOCALIZER_COMPONENT_H_ +#define GROUND_TRUTH_LOCALIZER_GROUND_TRUTH_LOCALIZER_COMPONENT_H_ #include #include @@ -57,9 +57,9 @@ typedef srv::Empty Empty; #include namespace ground_truth_localizer { -class GroundTruthLocalizerNodelet : public ff_util::FreeFlyerComponent { +class GroundTruthLocalizerComponent : public ff_util::FreeFlyerComponent { public: - explicit GroundTruthLocalizerNodelet(const rclcpp::NodeOptions& options); + explicit GroundTruthLocalizerComponent(const rclcpp::NodeOptions& options); private: void Initialize(NodeHandle &nh); @@ -100,4 +100,4 @@ class GroundTruthLocalizerNodelet : public ff_util::FreeFlyerComponent { }; } // namespace ground_truth_localizer -#endif // GROUND_TRUTH_LOCALIZER_GROUND_TRUTH_LOCALIZER_NODELET_H_ +#endif // GROUND_TRUTH_LOCALIZER_GROUND_TRUTH_LOCALIZER_COMPONENT_H_ diff --git a/localization/ground_truth_localizer/src/ground_truth_localizer_nodelet.cc b/localization/ground_truth_localizer/src/ground_truth_localizer_component.cc similarity index 72% rename from localization/ground_truth_localizer/src/ground_truth_localizer_nodelet.cc rename to localization/ground_truth_localizer/src/ground_truth_localizer_component.cc index 013ca70614..7dbaef2d1c 100644 --- a/localization/ground_truth_localizer/src/ground_truth_localizer_nodelet.cc +++ b/localization/ground_truth_localizer/src/ground_truth_localizer_component.cc @@ -16,7 +16,7 @@ * under the License. */ -#include +#include #include #include #include @@ -28,10 +28,10 @@ namespace ground_truth_localizer { FF_DEFINE_LOGGER("ground_truth_localizer"); namespace lc = localization_common; -GroundTruthLocalizerNodelet::GroundTruthLocalizerNodelet(const rclcpp::NodeOptions& options) +GroundTruthLocalizerComponent::GroundTruthLocalizerComponent(const rclcpp::NodeOptions& options) : ff_util::FreeFlyerComponent(options, NODE_SIM_LOC, true) {} -void GroundTruthLocalizerNodelet::Initialize(NodeHandle &nh) { +void GroundTruthLocalizerComponent::Initialize(NodeHandle &nh) { platform_name_ = GetPlatform(); platform_name_ = (platform_name_.empty() ? "" : platform_name_ + "/"); SubscribeAndAdvertise(nh); @@ -39,7 +39,7 @@ void GroundTruthLocalizerNodelet::Initialize(NodeHandle &nh) { transform_pub_ = std::make_unique(*nh); } -void GroundTruthLocalizerNodelet::SubscribeAndAdvertise(NodeHandle &nh) { +void GroundTruthLocalizerComponent::SubscribeAndAdvertise(NodeHandle &nh) { node_ = nh; pose_pub_ = FF_CREATE_PUBLISHER(nh, geometry_msgs::PoseStamped, TOPIC_LOCALIZATION_POSE, 1); twist_pub_ = FF_CREATE_PUBLISHER(nh, geometry_msgs::TwistStamped, TOPIC_LOCALIZATION_TWIST, 1); @@ -49,37 +49,37 @@ void GroundTruthLocalizerNodelet::SubscribeAndAdvertise(NodeHandle &nh) { pose_sub_ = FF_CREATE_SUBSCRIBER(nh, geometry_msgs::PoseStamped, TOPIC_LOCALIZATION_TRUTH, 1, - std::bind(&GroundTruthLocalizerNodelet::PoseCallback, this, std::placeholders::_1)); + std::bind(&GroundTruthLocalizerComponent::PoseCallback, this, std::placeholders::_1)); twist_sub_ = FF_CREATE_SUBSCRIBER(nh, geometry_msgs::TwistStamped, TOPIC_LOCALIZATION_TRUTH_TWIST, 1, - std::bind(&GroundTruthLocalizerNodelet::TwistCallback, this, std::placeholders::_1)); + std::bind(&GroundTruthLocalizerComponent::TwistCallback, this, std::placeholders::_1)); input_mode_srv_ = nh->create_service( SERVICE_GNC_EKF_SET_INPUT, - std::bind(&GroundTruthLocalizerNodelet::SetMode, this, std::placeholders::_1, std::placeholders::_2)); - bias_srv_ = nh->create_service(SERVICE_GNC_EKF_INIT_BIAS, - std::bind(&GroundTruthLocalizerNodelet::DefaultServiceResponse, + std::bind(&GroundTruthLocalizerComponent::SetMode, this, std::placeholders::_1, std::placeholders::_2)); + bias_srv_ = FF_CREATE_SERVICE(nh, std_srvs::Empty, SERVICE_GNC_EKF_INIT_BIAS, + std::bind(&GroundTruthLocalizerComponent::DefaultServiceResponse, this, std::placeholders::_1, std::placeholders::_2)); - bias_from_file_srv_ = nh->create_service(SERVICE_GNC_EKF_INIT_BIAS_FROM_FILE, - std::bind(&GroundTruthLocalizerNodelet::DefaultServiceResponse, + bias_from_file_srv_ = FF_CREATE_SERVICE(nh, std_srvs::Empty, SERVICE_GNC_EKF_INIT_BIAS_FROM_FILE, + std::bind(&GroundTruthLocalizerComponent::DefaultServiceResponse, this, std::placeholders::_1, std::placeholders::_2)); - reset_srv_ = nh->create_service(SERVICE_GNC_EKF_RESET, - std::bind(&GroundTruthLocalizerNodelet::DefaultServiceResponse, + reset_srv_ = FF_CREATE_SERVICE(nh, std_srvs::Empty, SERVICE_GNC_EKF_RESET, + std::bind(&GroundTruthLocalizerComponent::DefaultServiceResponse, this, std::placeholders::_1, std::placeholders::_2)); } -bool GroundTruthLocalizerNodelet::SetMode(const std::shared_ptr req, +bool GroundTruthLocalizerComponent::SetMode(const std::shared_ptr req, std::shared_ptr res) { input_mode_ = req->mode; return true; } -bool GroundTruthLocalizerNodelet::DefaultServiceResponse(const std::shared_ptr req, +bool GroundTruthLocalizerComponent::DefaultServiceResponse(const std::shared_ptr req, std::shared_ptr res) { return true; } -void GroundTruthLocalizerNodelet::PoseCallback(const std::shared_ptr pose) { +void GroundTruthLocalizerComponent::PoseCallback(const std::shared_ptr pose) { assert(pose->header.frame_id == "world"); pose_ = PoseFromMsg(*pose); pose_pub_->publish(*pose); @@ -94,7 +94,7 @@ void GroundTruthLocalizerNodelet::PoseCallback(const std::shared_ptrpublish(heartbeat_); } -void GroundTruthLocalizerNodelet::TwistCallback(const std::shared_ptr twist) { +void GroundTruthLocalizerComponent::TwistCallback(const std::shared_ptr twist) { assert(twist->header.frame_id == "world"); twist_ = TwistFromMsg(*twist); twist_pub_->publish(*twist); @@ -102,7 +102,7 @@ void GroundTruthLocalizerNodelet::TwistCallback(const std::shared_ptrpublish(loc_state_msg); @@ -123,4 +123,4 @@ void GroundTruthLocalizerNodelet::PublishLocState(const lc::Time& timestamp) { // Register the component with class_loader. // This acts as a sort of entry point, allowing the component to be discoverable when its library // is being loaded into a running process. -RCLCPP_COMPONENTS_REGISTER_NODE(ground_truth_localizer::GroundTruthLocalizerNodelet) +RCLCPP_COMPONENTS_REGISTER_NODE(ground_truth_localizer::GroundTruthLocalizerComponent) diff --git a/shared/ff_common/include/ff_common/ff_ros.h b/shared/ff_common/include/ff_common/ff_ros.h index 98c84b951e..3e839156ad 100644 --- a/shared/ff_common/include/ff_common/ff_ros.h +++ b/shared/ff_common/include/ff_common/ff_ros.h @@ -50,10 +50,6 @@ using Service = ros::ServiceServer*; template using ServiceClient = ros::ServiceClient*; -#define FF_CREATE_SERVICE_CLIENT(serv_client, msg, topic) -ros::ServiceClient __serv_client = nh_private_.serviceClient(topic); -serv_client = &__serv_client - using Duration = ros::Duration*; #define FF_DEBUG(...) ROS_DEBUG_NAMED(ros::this_node::getName(), __VA_ARGS__) @@ -109,9 +105,6 @@ using Service = std::shared_ptr>; template using ServiceClient = std::shared_ptr>; -#define FF_CREATE_SERVICE_CLIENT(node, msg, topic) \ - node->create_client(topic) - using Duration = std::shared_ptr; #define FF_DEFINE_LOGGER(name) static const rclcpp::Logger _FF_LOGGER = rclcpp::get_logger(name); diff --git a/shared/ff_util/CMakeLists.txt b/shared/ff_util/CMakeLists.txt index b76b87f770..3bf1450aca 100644 --- a/shared/ff_util/CMakeLists.txt +++ b/shared/ff_util/CMakeLists.txt @@ -87,11 +87,11 @@ if(BUILD_TESTING) add_ros_test(test/launch/test_ff_timer.py TIMEOUT "30" ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") # ff_service - ament_add_gtest_executable(test_ff_service - test/test_ff_service.cc - ) - target_link_libraries(test_ff_service ff_util) - add_ros_test(test/launch/test_ff_service.py TIMEOUT "30" ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + # ament_add_gtest_executable(test_ff_service + # test/test_ff_service.cc + # ) + # target_link_libraries(test_ff_service ff_util) + # add_ros_test(test/launch/test_ff_service.py TIMEOUT "30" ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") # ff_action ament_add_gtest_executable(test_ff_action_nominal_behavior diff --git a/shared/ff_util/include/ff_util/ff_service.h b/shared/ff_util/include/ff_util/ff_service.h index aa82ab4e28..b993295327 100644 --- a/shared/ff_util/include/ff_util/ff_service.h +++ b/shared/ff_util/include/ff_util/ff_service.h @@ -90,6 +90,8 @@ class FreeFlyerServiceClient { // Save the node handle and topic to support reconnects node_ = node; topic_ = topic; + client_group_ = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + return IsConnected(); } @@ -117,10 +119,10 @@ class FreeFlyerServiceClient { std::shared_ptr & response) { if (IsConnected()) { auto result = service_client_->async_send_request(request); - rclcpp::FutureReturnCode return_code = - rclcpp::spin_until_future_complete(node_, result); - response = result.get(); - if (return_code == rclcpp::FutureReturnCode::SUCCESS) { + std::future_status status = result.wait_for((std::chrono::seconds)10); // timeout to guarantee a graceful finish + + if (status == std::future_status::ready) { + response = result.get(); return true; } } @@ -177,7 +179,8 @@ class FreeFlyerServiceClient { } // Case: disconnected } else { - service_client_ = FF_CREATE_SERVICE_CLIENT(node_, ServiceSpec, topic_); + service_client_ = node_->create_client(topic_, rmw_qos_profile_services_default, + client_group_); state_ = WAITING_FOR_CONNECT; StartOptionalTimer(timer_connected_, to_connected_); StartOptionalTimer(timer_poll_, to_poll_); @@ -199,6 +202,7 @@ class FreeFlyerServiceClient { TimeoutCallbackType cb_timeout_; ConnectedCallbackType cb_connected_; ServiceClient service_client_; + rclcpp::CallbackGroup::SharedPtr client_group_; ff_util::FreeFlyerTimer timer_connected_; ff_util::FreeFlyerTimer timer_poll_; std::string topic_; diff --git a/simulation/src/gazebo_model_plugin_signal_lights/gazebo_model_plugin_signal_lights.cc b/simulation/src/gazebo_model_plugin_signal_lights/gazebo_model_plugin_signal_lights.cc index 5939d7db81..110b6b9ceb 100644 --- a/simulation/src/gazebo_model_plugin_signal_lights/gazebo_model_plugin_signal_lights.cc +++ b/simulation/src/gazebo_model_plugin_signal_lights/gazebo_model_plugin_signal_lights.cc @@ -44,10 +44,9 @@ class GazeboModelPluginSignalLights : public FreeFlyerModelPlugin { // this service is a special case for when we need to light // two AMBER leds on each side only when we are streaming // live video - streaming_service_ = - nh->create_service(SERVICE_STREAMING_LIGHTS, - std::bind(&GazeboModelPluginSignalLights::StreamingLightsCallback, this, - std::placeholders::_1, std::placeholders::_2)); + streaming_service_ = FF_CREATE_SERVICE(nh, ff_msgs::srv::SetStreamingLights, SERVICE_STREAMING_LIGHTS, + std::bind(&GazeboModelPluginSignalLights::StreamingLightsCallback, this, + std::placeholders::_1, std::placeholders::_2)); } // Manage the extrinsics based on the sensor type @@ -57,7 +56,7 @@ class GazeboModelPluginSignalLights : public FreeFlyerModelPlugin { bool StreamingLightsCallback( const std::shared_ptr request, - const std::shared_ptr response) { + std::shared_ptr response) { // Send successful response response->success = true; return true;