diff --git a/astrobee/config/robots/sim.config b/astrobee/config/robots/sim.config index 2510117a97..0bda16c4c4 100644 --- a/astrobee/config/robots/sim.config +++ b/astrobee/config/robots/sim.config @@ -108,7 +108,7 @@ agent_name = "sim" heartbeat_queue_size = 20 -nodes_not_running = {"light_flow"} +nodes_not_running = {"dds_ros_bridge", "astrobee_astrobee_bridge", "light_flow", "data_bagger", "handrail_detect", "image_sampler", "pico_driver", "planner_qp"} bags_save_directory = "/tmp/bags" diff --git a/astrobee/config/simulation/simulation.config b/astrobee/config/simulation/simulation.config index 51bd34bb26..1e55bb2763 100644 --- a/astrobee/config/simulation/simulation.config +++ b/astrobee/config/simulation/simulation.config @@ -28,7 +28,7 @@ disable_cameras_on_speedup = true; -- cost. Setting the rate to zero effectively disables the camera. By default -- we only enable the haz_cam, because it's needed for obstacle detection. -nav_cam_rate = 0.0; -dock_cam_rate = 0.0; +nav_cam_rate = 1.0; +dock_cam_rate = 1.0; perch_cam_rate = 5.0; haz_cam_rate = 5.0; diff --git a/astrobee/launch/astrobee.launch.py b/astrobee/launch/astrobee.launch.py index c12a8cf905..cd8065f57f 100644 --- a/astrobee/launch/astrobee.launch.py +++ b/astrobee/launch/astrobee.launch.py @@ -17,7 +17,8 @@ from utilities.utilities import * - +from launch.actions import GroupAction +from launch_ros.actions import PushRosNamespace def generate_launch_description(): robot_description = Command(['xacro ', get_path('urdf/model.urdf.xacro', 'description'), @@ -63,56 +64,71 @@ def generate_launch_description(): # Set the TF prefix, create a robot description and joint state publisher Node( package="robot_state_publisher", - namespace="", + namespace=LaunchConfiguration("ns"), executable="robot_state_publisher", name="robot_state_publisher", - parameters=[{'robot_description': ParameterValue(robot_description) }], + parameters=[ + {'use_sim_time': True}, + {'robot_description': ParameterValue(robot_description) } + ], ), # If we need to load synthetic drivers (we are not running on a real robot) # TODO(asymingt) - pass nodes, spurn and extra into gazebo - IncludeLaunchDescription( - get_launch_file("launch/controller/synthetic.launch.py"), - launch_arguments={ - "world": LaunchConfiguration("world"), # Don't start driver nodes - "ns" : LaunchConfiguration("ns"), # Prevent node - "sim" : LaunchConfiguration("sim"), # Launch node group - "pose" : LaunchConfiguration("pose"), # Inject extra nodes - "bag" : LaunchConfiguration("bag"), # Debug a node set - "robot_description" : robot_description, # Robot description - }.items(), - condition=UnlessCondition(LaunchConfiguration("drivers")) + GroupAction( + actions=[ + PushRosNamespace(LaunchConfiguration('ns')), + IncludeLaunchDescription( + get_launch_file("launch/controller/synthetic.launch.py"), + launch_arguments={ + "world": LaunchConfiguration("world"), # Don't start driver nodes + "ns" : LaunchConfiguration("ns"), # Prevent node + "sim" : LaunchConfiguration("sim"), # Launch node group + "pose" : LaunchConfiguration("pose"), # Inject extra nodes + "bag" : LaunchConfiguration("bag"), # Debug a node set + "robot_description" : robot_description, # Robot description + }.items(), + condition=UnlessCondition(LaunchConfiguration("drivers")) + ) + ] ), # LLP -# -# -# - IncludeLaunchDescription( - get_launch_file("launch/robot/LLP.launch.py"), - launch_arguments={ - "drivers": LaunchConfiguration("drivers"), # Don't start driver nodes - "spurn" : LaunchConfiguration("spurn"), # Prevent node - "nodes" : LaunchConfiguration("nodes"), # Launch node group - "extra" : LaunchConfiguration("extra"), # Inject extra nodes - "debug" : LaunchConfiguration("debug"), # Debug a node set - "output" : LaunchConfiguration("output"), - "gtloc" : LaunchConfiguration("gtloc"), # Use Ground Truth Localizer - }.items(), - condition=LaunchConfigurationNotEquals("llp", "disabled"), + # + # + # + GroupAction( + actions=[ + PushRosNamespace(LaunchConfiguration('ns')), + IncludeLaunchDescription( + get_launch_file("launch/robot/LLP.launch.py"), + launch_arguments={ + "drivers": LaunchConfiguration("drivers"), # Don't start driver nodes + "spurn" : LaunchConfiguration("spurn"), # Prevent node + "nodes" : LaunchConfiguration("nodes"), # Launch node group + "extra" : LaunchConfiguration("extra"), # Inject extra nodes + "debug" : LaunchConfiguration("debug"), # Debug a node set + "output" : LaunchConfiguration("output"), + "gtloc" : LaunchConfiguration("gtloc"), # Use Ground Truth Localizer + }.items(), + condition=LaunchConfigurationNotEquals("llp", "disabled"), + ) + ] ), - # MLP -# -# -# - IncludeLaunchDescription( - get_launch_file("launch/robot/MLP.launch.py"), - launch_arguments={ + # + # + # + GroupAction( + actions=[ + PushRosNamespace(LaunchConfiguration('ns')), + IncludeLaunchDescription( + get_launch_file("launch/robot/MLP.launch.py"), + launch_arguments={ "drivers": LaunchConfiguration("drivers"), # Don't start driver nodes "spurn" : LaunchConfiguration("spurn"), # Prevent node "nodes" : LaunchConfiguration("nodes"), # Launch node group @@ -120,9 +136,12 @@ def generate_launch_description(): "debug" : LaunchConfiguration("debug"), # Debug a node set "output" : LaunchConfiguration("output"), "gtloc" : LaunchConfiguration("gtloc"), # Use Ground Truth Localizer - }.items(), - condition=LaunchConfigurationNotEquals("llp", "disabled"), - ), + }.items(), + condition=LaunchConfigurationNotEquals("llp", "disabled"), + ) + ] + ) + ]) diff --git a/astrobee/launch/controller/rviz.launch.py b/astrobee/launch/controller/rviz.launch.py index ff9f5bf27b..a1246b1dda 100644 --- a/astrobee/launch/controller/rviz.launch.py +++ b/astrobee/launch/controller/rviz.launch.py @@ -33,6 +33,7 @@ def generate_launch_description(): namespace="", executable="rviz2", name="rviz_node", + parameters=[{'use_sim_time': True}], arguments=["-d", PathJoinSubstitution([get_package_share_directory("astrobee"),"resources/rviz",LaunchConfiguration("rviz_file")])], respawn=True, ), diff --git a/astrobee/launch/robot/LLP.launch.py b/astrobee/launch/robot/LLP.launch.py index d2f5b94485..59959404ae 100644 --- a/astrobee/launch/robot/LLP.launch.py +++ b/astrobee/launch/robot/LLP.launch.py @@ -59,12 +59,14 @@ def generate_launch_description(): package='ctl', plugin='ctl::CtlComponent', name='ctl', - extra_arguments=[{'use_intra_process_comms': False, 'use_sim_time': True}]), + parameters=[{'use_sim_time': True}], + extra_arguments=[{'use_intra_process_comms': False}]), ComposableNode( package='fam', plugin='fam::FamComponent', name='fam', - extra_arguments=[{'use_intra_process_comms': False, 'use_sim_time': True}]), + parameters=[{'use_sim_time': True}], + extra_arguments=[{'use_intra_process_comms': False}]), ] ), ComposableNodeContainer( @@ -193,7 +195,8 @@ def generate_launch_description(): package='light_flow', plugin='light_flow::LightFlowComponent', name='light_flow', - extra_arguments=[{'use_intra_process_comms': False, 'use_sim_time': True}]), + parameters=[{'use_sim_time': True}], + extra_arguments=[{'use_intra_process_comms': False}]), ] ), ]) diff --git a/astrobee/launch/robot/MLP.launch.py b/astrobee/launch/robot/MLP.launch.py index efb535109b..31259375ea 100644 --- a/astrobee/launch/robot/MLP.launch.py +++ b/astrobee/launch/robot/MLP.launch.py @@ -59,12 +59,14 @@ def generate_launch_description(): package='localization_manager', plugin='localization_manager::LocalizationManagerComponent', name='localization_manager', - extra_arguments=[{'use_intra_process_comms': False, 'use_sim_time': True}]), + parameters=[{'use_sim_time': True}], + extra_arguments=[{'use_intra_process_comms': False}]), ComposableNode( package='ground_truth_localizer', plugin='ground_truth_localizer::GroundTruthLocalizerComponent', name='ground_truth_localizer', - extra_arguments=[{'use_intra_process_comms': False, 'use_sim_time': True}]), + parameters=[{'use_sim_time': True}], + extra_arguments=[{'use_intra_process_comms': False}]), # ComposableNode( # package='image_sampler', # plugin='image_sampler::ImageSampler', @@ -85,7 +87,8 @@ def generate_launch_description(): package='localization_manager', plugin='localization_manager::LocalizationManagerNodelet', name='localization_manager', - extra_arguments=[{'use_intra_process_comms': False, 'use_sim_time': True}]), + parameters=[{'use_sim_time': True}], + extra_arguments=[{'use_intra_process_comms': False}]), # ComposableNode( # package='image_sampler', # plugin='image_sampler::ImageSampler', @@ -226,25 +229,28 @@ def generate_launch_description(): package='mapper', plugin='mapper::MapperComponent', name='mapper', - extra_arguments=[{'use_intra_process_comms': False, 'use_sim_time': True}]), + parameters=[{'use_sim_time': True}], + extra_arguments=[{'use_intra_process_comms': False}]), ] ), ComposableNodeContainer( name='mlp_management', namespace='', package='rclcpp_components', - executable='component_container', + executable='component_container_mt', composable_node_descriptions=[ - # ComposableNode( - # package='executive', - # plugin='executive::Executive', - # name='executive', - # extra_arguments=[{'use_intra_process_comms': True}]), - # ComposableNode( - # package='access_control', - # plugin='access_control::AccessControl', - # name='access_control', - # extra_arguments=[{'use_intra_process_comms': True}]), + ComposableNode( + package='executive', + plugin='executive::Executive', + name='executive', + parameters=[{'use_sim_time': True}], + extra_arguments=[{'use_intra_process_comms': False}]), + ComposableNode( + package='access_control', + plugin='access_control::AccessControl', + name='access_control', + parameters=[{'use_sim_time': True}], + extra_arguments=[{'use_intra_process_comms': False}]), ] ), ComposableNodeContainer( @@ -291,11 +297,12 @@ def generate_launch_description(): 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}]), + ComposableNode( + package='sys_monitor', + plugin='sys_monitor::SysMonitor', + name='sys_monitor', + parameters=[{'use_sim_time': True}], + extra_arguments=[{'use_intra_process_comms': False}]), ] ), ComposableNodeContainer( @@ -348,17 +355,20 @@ def generate_launch_description(): package='choreographer', plugin='choreographer::ChoreographerComponent', name='choreographer', - extra_arguments=[{'use_intra_process_comms': False, 'use_sim_time': True}]), + parameters=[{'use_sim_time': True}], + extra_arguments=[{'use_intra_process_comms': False}]), ComposableNode( package='planner_trapezoidal', plugin='planner_trapezoidal::PlannerTrapezoidalComponent', name='planner_trapezoidal', - extra_arguments=[{'use_intra_process_comms': False, 'use_sim_time': True}]), + parameters=[{'use_sim_time': True}], + extra_arguments=[{'use_intra_process_comms': False}]), ComposableNode( package='framestore', plugin='mobility::FrameStore', name='framestore', - extra_arguments=[{'use_intra_process_comms': False, 'use_sim_time': True}]), + parameters=[{'use_sim_time': True}], + extra_arguments=[{'use_intra_process_comms': False}]), ] ), ComposableNodeContainer( @@ -370,7 +380,8 @@ def generate_launch_description(): ComposableNode( package='arm', plugin='arm::ArmComponent', - name='arm'), + name='arm', + parameters=[{'use_sim_time': True}] ), ] ), ComposableNodeContainer( @@ -382,8 +393,10 @@ def generate_launch_description(): ComposableNode( package='dock', plugin='dock::DockComponent', - name='dock'), - #extra_arguments=[{'use_intra_process_comms': True}]), + name='dock', + parameters=[{'use_sim_time': True}] + #extra_arguments=[{'use_intra_process_comms': False}] + ), ] ), ComposableNodeContainer( @@ -395,8 +408,10 @@ def generate_launch_description(): ComposableNode( package='perch', plugin='perch::PerchComponent', - name='perch'), - #extra_arguments=[{'use_intra_process_comms': True}]), + name='perch', + parameters=[{'use_sim_time': True}], + #extra_arguments=[{'use_intra_process_comms': True}] + ), ] ), ComposableNodeContainer( @@ -423,7 +438,8 @@ def generate_launch_description(): package='states', plugin='states::StatesComponent', name='states', - extra_arguments=[{'use_intra_process_comms': False, 'use_sim_time': True}]), + parameters=[{'use_sim_time': True}], + extra_arguments=[{'use_intra_process_comms': False}]), ] ), ]) diff --git a/astrobee/resources/rviz/iss_ros2.rviz b/astrobee/resources/rviz/iss_ros2.rviz index e3b59c5ba1..ea1d4f419b 100644 --- a/astrobee/resources/rviz/iss_ros2.rviz +++ b/astrobee/resources/rviz/iss_ros2.rviz @@ -7,8 +7,9 @@ Panels: - /Global Options1 - /Status1 - /Debug1 + - /Dock Cam1 Splitter Ratio: 0.5 - Tree Height: 564 + Tree Height: 433 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -35,81 +36,81 @@ Visualization Manager: Frame Timeout: 15 Frames: All Enabled: true - ar_tag: + bumble/ar_tag: Value: true - body: + bumble/body: Value: true - dock/berth1: + bumble/dock_cam: Value: true - dock/berth1/approach: + bumble/flashlight_aft: Value: true - dock/berth1/complete: + bumble/flashlight_front: Value: true - dock/berth2: + bumble/handrail/approach: Value: true - dock/berth2/approach: + bumble/handrail/body: Value: true - dock/berth2/complete: + bumble/handrail/complete: Value: true - dock/body: + bumble/haz_cam: Value: true - dock_cam: + bumble/imu: Value: true - flashlight_aft: + bumble/inertial_link: Value: true - flashlight_front: + bumble/laser: Value: true - granite/body: + bumble/nav_cam: Value: true - handrail/approach: + bumble/payload/bottom_aft: Value: true - handrail/body: + bumble/payload/bottom_front: Value: true - handrail/complete: + bumble/payload/top_aft: Value: true - haz_cam: + bumble/payload/top_front: Value: true - imu: + bumble/perch_cam: Value: true - inertial_link: + bumble/sci_cam: Value: true - iss/body: + bumble/top_aft: Value: true - laser: + bumble/top_aft_arm_distal_link: Value: true - nav_cam: + bumble/top_aft_arm_proximal_link: Value: true - payload/bottom_aft: + bumble/top_aft_gripper_left_distal_link: Value: true - payload/bottom_front: + bumble/top_aft_gripper_left_proximal_link: Value: true - payload/top_aft: + bumble/top_aft_gripper_right_distal_link: Value: true - payload/top_front: + bumble/top_aft_gripper_right_proximal_link: Value: true - perch/body: + bumble/truth: Value: true - perch_cam: + dock/berth1: Value: true - rviz: + dock/berth1/approach: Value: true - sci_cam: + dock/berth1/complete: Value: true - top_aft: + dock/berth2: Value: true - top_aft_arm_distal_link: + dock/berth2/approach: Value: true - top_aft_arm_proximal_link: + dock/berth2/complete: Value: true - top_aft_gripper_left_distal_link: + dock/body: Value: true - top_aft_gripper_left_proximal_link: + granite/body: Value: true - top_aft_gripper_right_distal_link: + iss/body: Value: true - top_aft_gripper_right_proximal_link: + perch/body: Value: true - truth: + rviz: Value: true world: Value: true @@ -120,6 +121,48 @@ Visualization Manager: Show Names: false Tree: world: + bumble/body: + bumble/ar_tag: + {} + bumble/dock_cam: + {} + bumble/flashlight_aft: + {} + bumble/flashlight_front: + {} + bumble/haz_cam: + {} + bumble/imu: + {} + bumble/inertial_link: + {} + bumble/laser: + {} + bumble/nav_cam: + {} + bumble/payload/bottom_aft: + {} + bumble/payload/bottom_front: + {} + bumble/payload/top_aft: + {} + bumble/payload/top_front: + {} + bumble/perch_cam: + {} + bumble/sci_cam: + {} + bumble/top_aft: + bumble/top_aft_arm_proximal_link: + bumble/top_aft_arm_distal_link: + bumble/top_aft_gripper_left_proximal_link: + bumble/top_aft_gripper_left_distal_link: + {} + bumble/top_aft_gripper_right_proximal_link: + bumble/top_aft_gripper_right_distal_link: + {} + bumble/truth: + {} dock/body: dock/berth1: dock/berth1/approach: @@ -139,8 +182,6 @@ Visualization Manager: {} rviz: {} - truth: - {} Update Interval: 0 Value: true - Class: rviz_common/Group @@ -281,25 +322,20 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /handrail_21_5/robot_description - Enabled: true + Enabled: false Links: All Links Enabled: true Expand Joint Details: false Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - body: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true Mass Properties: Inertia: false Mass: false Name: Handrail TF Prefix: handrail Update Interval: 0 - Value: true + Value: false Visual Enabled: true - Alpha: 1 Class: rviz_default_plugins/RobotModel @@ -311,65 +347,21 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /robot_description - Enabled: true + Value: /honey/robot_description + Enabled: false Links: All Links Enabled: true Expand Joint Details: false Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - body: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - inertial_link: - Alpha: 1 - Show Axes: false - Show Trail: false - top_aft: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - top_aft_arm_distal_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - top_aft_arm_proximal_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - top_aft_gripper_left_distal_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - top_aft_gripper_left_proximal_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - top_aft_gripper_right_distal_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - top_aft_gripper_right_proximal_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true Mass Properties: Inertia: false Mass: false Name: Honey - TF Prefix: honey + TF Prefix: "" Update Interval: 0 - Value: true + Value: false Visual Enabled: true - Alpha: 1 Class: rviz_default_plugins/RobotModel @@ -381,65 +373,21 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /robot_description - Enabled: true + Value: /bumble/robot_description + Enabled: false Links: All Links Enabled: true Expand Joint Details: false Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - body: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - inertial_link: - Alpha: 1 - Show Axes: false - Show Trail: false - top_aft: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - top_aft_arm_distal_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - top_aft_arm_proximal_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - top_aft_gripper_left_distal_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - top_aft_gripper_left_proximal_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - top_aft_gripper_right_distal_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - top_aft_gripper_right_proximal_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true Mass Properties: Inertia: false Mass: false Name: Bumble - TF Prefix: bumble + TF Prefix: "" Update Interval: 0 - Value: true + Value: false Visual Enabled: true - Alpha: 1 Class: rviz_default_plugins/RobotModel @@ -451,65 +399,21 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /robot_description - Enabled: true + Value: /queen/robot_description + Enabled: false Links: All Links Enabled: true Expand Joint Details: false Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - body: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - inertial_link: - Alpha: 1 - Show Axes: false - Show Trail: false - top_aft: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - top_aft_arm_distal_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - top_aft_arm_proximal_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - top_aft_gripper_left_distal_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - top_aft_gripper_left_proximal_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - top_aft_gripper_right_distal_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - top_aft_gripper_right_proximal_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true Mass Properties: Inertia: false Mass: false Name: Queen - TF Prefix: queen + TF Prefix: "" Update Interval: 0 - Value: true + Value: false Visual Enabled: true - Class: rviz_common/Group Displays: @@ -524,64 +428,20 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /robot_description - Enabled: true + Enabled: false Links: All Links Enabled: true Expand Joint Details: false Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - body: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - inertial_link: - Alpha: 1 - Show Axes: false - Show Trail: false - top_aft: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - top_aft_arm_distal_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - top_aft_arm_proximal_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - top_aft_gripper_left_distal_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - top_aft_gripper_left_proximal_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - top_aft_gripper_right_distal_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - top_aft_gripper_right_proximal_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true Mass Properties: Inertia: false Mass: false Name: / TF Prefix: "" Update Interval: 0 - Value: true + Value: false Visual Enabled: true - Class: rviz_common/Group Displays: ~ @@ -675,16 +535,16 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 2.0104706287384033 + Distance: 2.5802009105682373 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 9.056347846984863 - Y: 10.248156547546387 - Z: -2.718514919281006 + X: 8.70544147491455 + Y: 8.735550880432129 + Z: -2.141327142715454 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false @@ -700,12 +560,12 @@ Window Geometry: collapsed: false Dock Cam: collapsed: false - Height: 1245 + Height: 1043 Hide Left Dock: false Hide Right Dock: false Nav Cam: collapsed: false - QMainWindow State: 000000ff00000000fd00000004000000000000025a00000443fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002bd000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000100044006f0063006b002000430061006d01000002fe000000a80000002800fffffffb0000000e004e00610076002000430061006d01000003ac000000d20000002800ffffff000000010000010f00000443fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b00000443000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000008700000003efc0100000002fb0000000800540069006d00650100000000000008700000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000004fb0000044300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000016700000379fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000023a000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000100044006f0063006b002000430061006d010000027b000000890000002800fffffffb0000000e004e00610076002000430061006d010000030a000000aa0000002800ffffff000000010000010f00000379fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b00000379000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000004fe0000037900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -714,6 +574,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 2160 - X: 241 - Y: 152 + Width: 1920 + X: 0 + Y: 0 diff --git a/behaviors/dock/src/dock_component.cc b/behaviors/dock/src/dock_component.cc index 14c538de36..63a344845c 100644 --- a/behaviors/dock/src/dock_component.cc +++ b/behaviors/dock/src/dock_component.cc @@ -778,7 +778,12 @@ class DockComponent : public ff_util::FreeFlyerComponent { // Package up the desired end pose geometry_msgs::PoseStamped msg; msg.header.stamp = GetTimeNow(); - ff_util::ConfigClient cfg(node_, NODE_CHOREOGRAPHER); + + // ConfigClient blocks when Setting parameters. Creating a new node prevent this + std::string platform = this->GetPlatform(); + std::shared_ptr temp_node = std::make_shared("temporal_dock_node", platform, rclcpp::NodeOptions().use_global_arguments(false)); + ff_util::ConfigClient cfg(temp_node, NODE_CHOREOGRAPHER); + // Set parameters for the choreographer cfg.Set("enable_collision_checking", false); cfg.Set("enable_validation", false); diff --git a/localization/ground_truth_localizer/include/ground_truth_localizer/ground_truth_localizer_component.h b/localization/ground_truth_localizer/include/ground_truth_localizer/ground_truth_localizer_component.h index a029966f19..4a1b185c95 100644 --- a/localization/ground_truth_localizer/include/ground_truth_localizer/ground_truth_localizer_component.h +++ b/localization/ground_truth_localizer/include/ground_truth_localizer/ground_truth_localizer_component.h @@ -64,7 +64,7 @@ class GroundTruthLocalizerComponent : public ff_util::FreeFlyerComponent { private: void Initialize(NodeHandle &nh); - void SubscribeAndAdvertise(NodeHandle &nh); + void SubscribeAndAdvertise(); bool SetMode(const std::shared_ptr req, std::shared_ptr res); @@ -78,7 +78,6 @@ class GroundTruthLocalizerComponent : public ff_util::FreeFlyerComponent { void PublishLocState(const localization_common::Time& timestamp); - NodeHandle node_; std::string platform_name_; rclcpp::Time last_time_; boost::optional pose_; diff --git a/localization/ground_truth_localizer/src/ground_truth_localizer_component.cc b/localization/ground_truth_localizer/src/ground_truth_localizer_component.cc index 7dbaef2d1c..43dcf82834 100644 --- a/localization/ground_truth_localizer/src/ground_truth_localizer_component.cc +++ b/localization/ground_truth_localizer/src/ground_truth_localizer_component.cc @@ -34,36 +34,35 @@ GroundTruthLocalizerComponent::GroundTruthLocalizerComponent(const rclcpp::NodeO void GroundTruthLocalizerComponent::Initialize(NodeHandle &nh) { platform_name_ = GetPlatform(); platform_name_ = (platform_name_.empty() ? "" : platform_name_ + "/"); - SubscribeAndAdvertise(nh); + SubscribeAndAdvertise(); // Initialize the transform broadcaster transform_pub_ = std::make_unique(*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); - state_pub_ = FF_CREATE_PUBLISHER(nh, ff_msgs::EkfState, TOPIC_GNC_EKF, 1); - heartbeat_pub_ = FF_CREATE_PUBLISHER(nh, ff_msgs::Heartbeat, TOPIC_HEARTBEAT, 5); - reset_pub_ = FF_CREATE_PUBLISHER(nh, std_msgs::Empty, TOPIC_GNC_EKF_RESET, 10); +void GroundTruthLocalizerComponent::SubscribeAndAdvertise() { + pose_pub_ = FF_CREATE_PUBLISHER(node_, geometry_msgs::PoseStamped, TOPIC_LOCALIZATION_POSE, 1); + twist_pub_ = FF_CREATE_PUBLISHER(node_, geometry_msgs::TwistStamped, TOPIC_LOCALIZATION_TWIST, 1); + state_pub_ = FF_CREATE_PUBLISHER(node_, ff_msgs::EkfState, TOPIC_GNC_EKF, 1); + heartbeat_pub_ = FF_CREATE_PUBLISHER(node_, ff_msgs::Heartbeat, TOPIC_HEARTBEAT, 5); + reset_pub_ = FF_CREATE_PUBLISHER(node_, std_msgs::Empty, TOPIC_GNC_EKF_RESET, 10); - pose_sub_ = FF_CREATE_SUBSCRIBER(nh, geometry_msgs::PoseStamped, + pose_sub_ = FF_CREATE_SUBSCRIBER(node_, geometry_msgs::PoseStamped, TOPIC_LOCALIZATION_TRUTH, 1, std::bind(&GroundTruthLocalizerComponent::PoseCallback, this, std::placeholders::_1)); - twist_sub_ = FF_CREATE_SUBSCRIBER(nh, geometry_msgs::TwistStamped, + twist_sub_ = FF_CREATE_SUBSCRIBER(node_, geometry_msgs::TwistStamped, TOPIC_LOCALIZATION_TRUTH_TWIST, 1, std::bind(&GroundTruthLocalizerComponent::TwistCallback, this, std::placeholders::_1)); - input_mode_srv_ = nh->create_service( + input_mode_srv_ = node_->create_service( SERVICE_GNC_EKF_SET_INPUT, 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, + bias_srv_ = FF_CREATE_SERVICE(node_, std_srvs::Empty, SERVICE_GNC_EKF_INIT_BIAS, std::bind(&GroundTruthLocalizerComponent::DefaultServiceResponse, this, std::placeholders::_1, std::placeholders::_2)); - bias_from_file_srv_ = FF_CREATE_SERVICE(nh, std_srvs::Empty, SERVICE_GNC_EKF_INIT_BIAS_FROM_FILE, + bias_from_file_srv_ = FF_CREATE_SERVICE(node_, std_srvs::Empty, SERVICE_GNC_EKF_INIT_BIAS_FROM_FILE, std::bind(&GroundTruthLocalizerComponent::DefaultServiceResponse, this, std::placeholders::_1, std::placeholders::_2)); - reset_srv_ = FF_CREATE_SERVICE(nh, std_srvs::Empty, SERVICE_GNC_EKF_RESET, + reset_srv_ = FF_CREATE_SERVICE(node_, std_srvs::Empty, SERVICE_GNC_EKF_RESET, std::bind(&GroundTruthLocalizerComponent::DefaultServiceResponse, this, std::placeholders::_1, std::placeholders::_2)); } diff --git a/localization/localization_common/include/localization_common/time.h b/localization/localization_common/include/localization_common/time.h index 6e1c62d83e..d01d7717c4 100644 --- a/localization/localization_common/include/localization_common/time.h +++ b/localization/localization_common/include/localization_common/time.h @@ -19,9 +19,11 @@ #ifndef LOCALIZATION_COMMON_TIME_H_ #define LOCALIZATION_COMMON_TIME_H_ +#include + namespace localization_common { using Time = double; -Time GetTime(const int seconds, const int nanoseconds); +Time GetTime(const int seconds, const unsigned int nanoseconds); } // namespace localization_common #endif // LOCALIZATION_COMMON_TIME_H_ diff --git a/localization/localization_common/src/time.cc b/localization/localization_common/src/time.cc index faae1fab89..ff7d517bdb 100644 --- a/localization/localization_common/src/time.cc +++ b/localization/localization_common/src/time.cc @@ -19,5 +19,5 @@ #include namespace localization_common { -Time GetTime(const int seconds, const int nanoseconds) { return seconds + 1e-9 * nanoseconds; } +Time GetTime(const int seconds, const unsigned int nanoseconds) { return (Time) seconds + 1e-9 * (Time) nanoseconds; } } // namespace localization_common diff --git a/localization/localization_common/src/utilities.cc b/localization/localization_common/src/utilities.cc index 157db93275..705063c39c 100644 --- a/localization/localization_common/src/utilities.cc +++ b/localization/localization_common/src/utilities.cc @@ -80,7 +80,12 @@ Time TimeFromHeader(const std_msgs::Header& header) { return GetTime(header.stam Time TimeFromRosTime(const rclcpp::Time& time) { return GetTime(time.seconds(), time.nanoseconds()); } -void TimeToHeader(const Time timestamp, std_msgs::Header& header) { header.stamp = rclcpp::Time(timestamp); } +rclcpp::Time TimeToRosTime(const Time timestamp) { + int sec = (int)(floor(timestamp)); int nanosec = (timestamp - floor(timestamp))*1e9; + return rclcpp::Time(sec, nanosec, RCL_ROS_TIME); +} + +void TimeToHeader(const Time timestamp, std_msgs::Header& header) { header.stamp = TimeToRosTime(timestamp); } gtsam::Pose3 PoseFromMsg(const geometry_msgs::PoseStamped& msg) { return PoseFromMsg(msg.pose); } diff --git a/management/access_control/CMakeLists.txt b/management/access_control/CMakeLists.txt index 9994424934..1a8da8bdd4 100644 --- a/management/access_control/CMakeLists.txt +++ b/management/access_control/CMakeLists.txt @@ -23,7 +23,7 @@ if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 14) endif() -set( CMAKE_CXX_FLAGS "${CMAKE_CXXX_FLAGS} -Wall -Werror -O3 -fPIC" ) +set( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Werror -O3 -fPIC" ) ## Find ament and libraries find_package(ament_cmake REQUIRED) diff --git a/management/executive/CMakeLists.txt b/management/executive/CMakeLists.txt index 78722e80d1..3640ddd792 100644 --- a/management/executive/CMakeLists.txt +++ b/management/executive/CMakeLists.txt @@ -15,45 +15,33 @@ # 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(executive) ## Compile as C++14, supported in ROS Kinetic and newer +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() add_compile_options(-std=c++14) -add_definitions(-DROS1) - -## Find catkin macros and libraries -find_package(catkin2 REQUIRED COMPONENTS - roscpp - ff_msgs - ff_hw_msgs - nodelet - config_reader - ff_util - jsonloader -) +set( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Werror -O3 -fPIC" ) + +## Find ament and libraries +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(ff_msgs REQUIRED) +find_package(ff_hw_msgs REQUIRED) +find_package(config_reader REQUIRED) +find_package(ff_util REQUIRED) +find_package(jsonloader REQUIRED) # System dependencies are found with CMake's conventions -find_package(cmake_modules REQUIRED) find_package(Boost REQUIRED COMPONENTS system iostreams) # Find jsoncpp LIST(APPEND CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/../../cmake") find_package(JsonCpp REQUIRED) -catkin_package( - LIBRARIES - executive - sequencer - CATKIN_DEPENDS - roscpp - ff_msgs - ff_hw_msgs - nodelet - ff_util - jsonloader -) - ########### ## Build ## ########### @@ -61,21 +49,20 @@ catkin_package( # Specify additional locations of header files include_directories( include - ${catkin_INCLUDE_DIRS} ${JSONCPP_INCLUDE_DIR} ) # Declare C++ libraries -add_library(sequencer +add_library(sequencer SHARED src/utils/sequencer/command_conversion.cc src/utils/sequencer/plan_io.cc src/utils/sequencer/sequencer.cc ) -add_dependencies(sequencer ${catkin_EXPORTED_TARGETS}) -target_link_libraries(sequencer ${Boost_IOSTREAMS_LIBRARY} ${catkin_LIBRARIES}) +target_link_libraries(sequencer ${Boost_LIBRARIES}) +ament_target_dependencies(sequencer rclcpp rclcpp_components ff_common ff_msgs ff_hw_msgs config_reader ff_util jsonloader Boost) # Declare C++ libraries -add_library(executive +add_library(executive SHARED src/executive.cc src/op_state.cc src/op_state_auto_return.cc @@ -85,100 +72,94 @@ add_library(executive src/op_state_repo.cc src/op_state_teleop.cc ) -add_dependencies(executive ${catkin_EXPORTED_TARGETS}) -target_link_libraries(executive sequencer ${Boost_SYSTEM_LIBRARY} ${catkin_LIBRARIES}) - +target_link_libraries(executive sequencer) +target_compile_definitions(executive PRIVATE "COMPOSITION_BUILDING_DLL") +ament_target_dependencies(executive rclcpp rclcpp_components ff_common ff_msgs ff_hw_msgs config_reader ff_util jsonloader) +rclcpp_components_register_nodes(executive "executive::Executive") ## Declare a C++ executable: data_to_disk_pub add_executable(data_to_disk_pub tools/data_to_disk_pub.cc) -add_dependencies(data_to_disk_pub ${catkin_EXPORTED_TARGETS}) -target_link_libraries(data_to_disk_pub - executive gflags ${catkin_LIBRARIES}) - -## Declare a C++ executable: ekf_switch_mux -add_executable(ekf_switch_mux tools/ekf_switch_mux.cc) -add_dependencies(ekf_switch_mux ${catkin_EXPORTED_TARGETS}) -target_link_libraries(ekf_switch_mux - executive gflags ${catkin_LIBRARIES}) +ament_target_dependencies(data_to_disk_pub rclcpp ff_common ff_msgs Boost) +target_link_libraries(data_to_disk_pub executive gflags) ## Declare a C++ executable: plan_pub add_executable(plan_pub tools/plan_pub.cc) -add_dependencies(plan_pub ${catkin_EXPORTED_TARGETS}) -target_link_libraries(plan_pub - executive gflags ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +ament_target_dependencies(plan_pub rclcpp ff_common ff_msgs Boost) +target_link_libraries(plan_pub executive gflags) ## Declare a C++ executable: simple_move add_executable(simple_move tools/simple_move.cc) -add_dependencies(simple_move ${catkin_EXPORTED_TARGETS}) -target_link_libraries(simple_move - executive gflags ${catkin_LIBRARIES}) +ament_target_dependencies(simple_move rclcpp ff_common ff_msgs Boost) +target_link_libraries(simple_move executive gflags) ## Declare a C++ executable: teleop_tool add_executable(teleop_tool tools/teleop_tool.cc) -add_dependencies(teleop_tool ${catkin_EXPORTED_TARGETS}) -target_link_libraries(teleop_tool - executive gflags ${catkin_LIBRARIES}) +ament_target_dependencies(teleop_tool rclcpp ff_common ff_msgs Boost) +target_link_libraries(teleop_tool executive gflags) ## Declare a C++ executable: zones_pub add_executable(zones_pub tools/zones_pub.cc) -add_dependencies(zones_pub ${catkin_EXPORTED_TARGETS}) -target_link_libraries(zones_pub - executive gflags ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +ament_target_dependencies(zones_pub rclcpp ff_common ff_msgs Boost) +target_link_libraries(zones_pub executive gflags) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ros_testing REQUIRED) -if(CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) # Executive initialization fault tester - add_rostest_gtest(test_init_executive - test/test_init_executive.test - test/test_init_executive.cc - ) - - target_link_libraries(test_init_executive - ${catkin_LIBRARIES} glog - ) +# ament_add_gtest_executable(test_init_executive +# test/test_init_executive.cc +# ) +# target_link_libraries(test_init_executive execute) +# add_ros_test(test/test_init_executive.py TIMEOUT "30" ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") endif() ############# ## Install ## ############# +ament_export_include_directories(include) + # Mark libraries for installation install(TARGETS sequencer - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -) -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} + EXPORT sequencer + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include ) -# Mark nodelet_plugin for installation -install(FILES nodelet_plugins.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +install(TARGETS ${PROJECT_NAME} + EXPORT executive + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include ) # Install C++ executables install(TARGETS data_to_disk_pub DESTINATION bin) -install(TARGETS ekf_switch_mux DESTINATION bin) install(TARGETS plan_pub DESTINATION bin) -install(TARGETS simple_move DESTINATION bin) -install(TARGETS teleop_tool DESTINATION bin) -install(TARGETS zones_pub DESTINATION bin) -install(CODE "execute_process( - COMMAND ln -s ../../bin/data_to_disk_pub share/${PROJECT_NAME} - COMMAND ln -s ../../bin/ekf_switch_mux share/${PROJECT_NAME} - COMMAND ln -s ../../bin/plan_pub share/${PROJECT_NAME} - COMMAND ln -s ../../bin/simple_move share/${PROJECT_NAME} - COMMAND ln -s ../../bin/teleop_tool share/${PROJECT_NAME} - COMMAND ln -s ../../bin/zones_pub share/${PROJECT_NAME} - WORKING_DIRECTORY ${CMAKE_INSTALL_PREFIX} - OUTPUT_QUIET - ERROR_QUIET - )") +install(TARGETS simple_move DESTINATION lib/${PROJECT_NAME}) +install(TARGETS teleop_tool RUNTIME DESTINATION lib/${PROJECT_NAME}) +#install(TARGETS zones_pub DESTINATION bin) +#install(CODE "execute_process( +# COMMAND ln -s ../../bin/data_to_disk_pub share/${PROJECT_NAME} +# COMMAND ln -s ../../bin/ekf_switch_mux share/${PROJECT_NAME} +# COMMAND ln -s ../../bin/plan_pub share/${PROJECT_NAME} +# COMMAND ln -s ../../bin/simple_move share/${PROJECT_NAME} +# COMMAND ln -s ../../bin/teleop_tool share/${PROJECT_NAME} +# COMMAND ln -s ../../bin/zones_pub share/${PROJECT_NAME} +# WORKING_DIRECTORY ${CMAKE_INSTALL_PREFIX} +# OUTPUT_QUIET +# ERROR_QUIET +# )") # Mark launch files for installation -install(DIRECTORY launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch - PATTERN ".svn" EXCLUDE) +install(DIRECTORY include/${PROJECT_NAME} + DESTINATION include + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) + +ament_package() diff --git a/management/executive/COLCON_IGNORE b/management/executive/COLCON_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/management/executive/include/executive/executive.h b/management/executive/include/executive/executive.h index 9d0ccbe338..633266737d 100644 --- a/management/executive/include/executive/executive.h +++ b/management/executive/include/executive/executive.h @@ -23,54 +23,54 @@ #include #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + #include +#include #include #include #include -#include +#include #include -#include -#include -#include -#include -#include +#include #include #include @@ -80,7 +80,8 @@ #include #include -using ff_msgs::CommandConstants; +using ff_msgs::msg::CommandConstants; +using ff_util::FreeFlyerServiceClient; namespace executive { class OpState; @@ -95,74 +96,78 @@ class OpState; * ref: state & mediator design pattern * */ -class Executive : public ff_util::FreeFlyerNodelet { +class Executive : public ff_util::FreeFlyerComponent { public: - Executive(); + explicit Executive(const rclcpp::NodeOptions& options); ~Executive(); // Message and timeout callbacks - void CameraStatesCallback(ff_msgs::CameraStatesStampedConstPtr const& state); - void CmdCallback(ff_msgs::CommandStampedPtr const& cmd); - void DataToDiskCallback(ff_msgs::CompressedFileConstPtr const& data); - void DockStateCallback(ff_msgs::DockStateConstPtr const& state); - void FaultStateCallback(ff_msgs::FaultStateConstPtr const& state); - void GuestScienceAckCallback(ff_msgs::AckStampedConstPtr const& ack); - void GuestScienceConfigCallback(ff_msgs::GuestScienceConfigConstPtr const& - config); - void GuestScienceStateCallback(ff_msgs::GuestScienceStateConstPtr const& - state); - void GuestScienceCustomCmdTimeoutCallback(ros::TimerEvent const& te); - void GuestScienceStartStopRestartCmdTimeoutCallback(ros::TimerEvent const& - te); - void InertiaCallback(geometry_msgs::InertiaStampedConstPtr const& inertia); + void CameraStatesCallback( + ff_msgs::msg::CameraStatesStamped::SharedPtr const state); + void CmdCallback(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + void DataToDiskCallback(ff_msgs::msg::CompressedFile::SharedPtr const data); + void DockStateCallback(ff_msgs::msg::DockState::SharedPtr const state); + void FaultStateCallback(ff_msgs::msg::FaultState::SharedPtr const state); + void GuestScienceAckCallback(ff_msgs::msg::AckStamped::SharedPtr const ack); + void GuestScienceConfigCallback( + ff_msgs::msg::GuestScienceConfig::SharedPtr const config); + void GuestScienceStateCallback( + ff_msgs::msg::GuestScienceState::SharedPtr const state); + void GuestScienceCustomCmdTimeoutCallback(); + void GuestScienceStartStopRestartCmdTimeoutCallback(); + void InertiaCallback( + geometry_msgs::msg::InertiaStamped::SharedPtr const inertia); void LedConnectedCallback(); - void MotionStateCallback(ff_msgs::MotionStatePtr const& state); - void PerchStateCallback(ff_msgs::PerchStateConstPtr const& state); - void PlanCallback(ff_msgs::CompressedFileConstPtr const& plan); - void SysMonitorHeartbeatCallback(ff_msgs::HeartbeatConstPtr const& heartbeat); - void SysMonitorTimeoutCallback(ros::TimerEvent const& te); - void WaitCallback(ros::TimerEvent const& te); - void ZonesCallback(ff_msgs::CompressedFileConstPtr const& zones); + void MotionStateCallback(ff_msgs::msg::MotionState::SharedPtr const state); + void PerchStateCallback(ff_msgs::msg::PerchState::SharedPtr const state); + void PlanCallback(ff_msgs::msg::CompressedFile::SharedPtr const plan); + void SysMonitorHeartbeatCallback( + ff_msgs::msg::Heartbeat::SharedPtr const heartbeat); + void SysMonitorTimeoutCallback(); + void WaitCallback(); + void ZonesCallback(ff_msgs::msg::CompressedFile::SharedPtr const zones); // Action based commands bool AreActionsRunning(); void CancelAction(Action action, std::string cmd); - bool FillArmGoal(ff_msgs::CommandStampedPtr const& cmd); - bool FillDockGoal(ff_msgs::CommandStampedPtr const& cmd, bool return_to_dock); + bool FillArmGoal(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool FillDockGoal(ff_msgs::msg::CommandStamped::SharedPtr const cmd, + bool return_to_dock); bool FillMotionGoal(Action action, - ff_msgs::CommandStampedPtr const& cmd = nullptr); + ff_msgs::msg::CommandStamped::SharedPtr const cmd = nullptr); bool IsActionRunning(Action action); bool StartAction(Action action, std::string const& cmd_id); bool RemoveAction(Action action); // Action callbacks void ArmResultCallback(ff_util::FreeFlyerActionState::Enum const& state, - ff_msgs::ArmResultConstPtr const& result); + std::shared_ptr result); void DockResultCallback(ff_util::FreeFlyerActionState::Enum const& state, - ff_msgs::DockResultConstPtr const& result); + std::shared_ptr const result); void LocalizationResultCallback( - ff_util::FreeFlyerActionState::Enum const& state, - ff_msgs::LocalizationResultConstPtr const& result); + ff_util::FreeFlyerActionState::Enum const& state, + std::shared_ptr result); - void MotionFeedbackCallback(ff_msgs::MotionFeedbackConstPtr const& feedback); + void MotionFeedbackCallback( + std::shared_ptr feedback); void MotionResultCallback(ff_util::FreeFlyerActionState::Enum const& state, - ff_msgs::MotionResultConstPtr const& result); + std::shared_ptr result); void PerchResultCallback(ff_util::FreeFlyerActionState::Enum const& state, - ff_msgs::PerchResultConstPtr const& result); + std::shared_ptr result); // Publishers void PublishCmdAck(std::string const& cmd_id, - uint8_t completed_status = ff_msgs::AckCompletedStatus::OK, - std::string const& message = "", - uint8_t status = ff_msgs::AckStatus::COMPLETED); + uint8_t completed_status = ff_msgs::msg::AckCompletedStatus::OK, + std::string const& message = "", + uint8_t status = ff_msgs::msg::AckStatus::COMPLETED); void PublishPlan(); void PublishPlanStatus(uint8_t status); // Getters - ff_msgs::MobilityState GetMobilityState(); + ff_msgs::msg::MobilityState GetMobilityState(); uint8_t GetPlanExecState(); std::string GetRunPlanCmdId(); @@ -174,159 +179,202 @@ class Executive : public ff_util::FreeFlyerNodelet { void SetRunPlanCmdId(std::string cmd_id); // Helper functions - void AckMobilityStateIssue(ff_msgs::CommandStampedPtr const& cmd, + void AckMobilityStateIssue(ff_msgs::msg::CommandStamped::SharedPtr const cmd, std::string const& current_mobility_state, std::string const& accepted_mobility_state = ""); - bool ArmControl(ff_msgs::CommandStampedPtr const& cmd); - bool CheckServiceExists(ros::ServiceClient& serviceIn, + bool ArmControl(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool CheckServiceExists(bool serviceExists, std::string const& serviceName, std::string const& cmd_in); bool CheckStoppedOrDrifting(std::string const& cmd_id, std::string const& cmd_name); - bool ConfigureLed(ff_hw_msgs::ConfigureSystemLeds& led_srv); + bool ConfigureLed( + ff_util::FreeFlyerService& led_srv); bool ConfigureMobility(bool move_to_start, std::string& err_msg); - bool FailCommandIfMoving(ff_msgs::CommandStampedPtr const& cmd); - bool LoadUnloadNodelet(ff_msgs::CommandStampedPtr const& cmd); - ros::Time MsToSec(std::string timestamp); - bool PowerItem(ff_msgs::CommandStampedPtr const& cmd, bool on); - bool ProcessGuestScienceCommand(ff_msgs::CommandStampedPtr const& cmd); + bool FailCommandIfMoving(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool LoadUnloadNodelet(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + rclcpp::Time MsToSec(std::string timestamp); + bool PowerItem(ff_msgs::msg::CommandStamped::SharedPtr const cmd, bool on); + bool ProcessGuestScienceCommand( + ff_msgs::msg::CommandStamped::SharedPtr const cmd); bool ResetEkf(std::string const& cmd_id); - void StartWaitTimer(float duration); + void StartWaitTimer(double duration); void StopWaitTimer(); + // Output functions + void Debug(std::string output); + void Error(std::string output); + void Info(std::string output); + void Warn(std::string output); + // Plan related functions bool AckCurrentPlanItem(); sequencer::ItemType GetCurrentPlanItemType(); - ff_msgs::CommandStampedPtr GetPlanCommand(); + ff_msgs::msg::CommandStamped::SharedPtr GetPlanCommand(); bool GetSetPlanInertia(std::string const& cmd_id); void GetSetPlanOperatingLimits(); // Commands - bool ArmPanAndTilt(ff_msgs::CommandStampedPtr const& cmd); - bool AutoReturn(ff_msgs::CommandStampedPtr const& cmd); - bool CustomGuestScience(ff_msgs::CommandStampedPtr const& cmd); - bool DeployArm(ff_msgs::CommandStampedPtr const& cmd); - bool Dock(ff_msgs::CommandStampedPtr const& cmd); - bool EnableAstrobeeIntercomms(ff_msgs::CommandStampedPtr const& cmd); - bool Fault(ff_msgs::CommandStampedPtr const& cmd); - bool GripperControl(ff_msgs::CommandStampedPtr const& cmd); - bool IdlePropulsion(ff_msgs::CommandStampedPtr const& cmd); - bool InitializeBias(ff_msgs::CommandStampedPtr const& cmd); - bool LoadNodelet(ff_msgs::CommandStampedPtr const& cmd); - bool NoOp(ff_msgs::CommandStampedPtr const& cmd); - bool PausePlan(ff_msgs::CommandStampedPtr const& cmd); - bool Perch(ff_msgs::CommandStampedPtr const& cmd); - bool PowerItemOff(ff_msgs::CommandStampedPtr const& cmd); - bool PowerItemOn(ff_msgs::CommandStampedPtr const& cmd); - bool Prepare(ff_msgs::CommandStampedPtr const& cmd); - bool ReacquirePosition(ff_msgs::CommandStampedPtr const& cmd); - bool ResetEkf(ff_msgs::CommandStampedPtr const& cmd); - bool RestartGuestScience(ff_msgs::CommandStampedPtr const& cmd); - bool RunPlan(ff_msgs::CommandStampedPtr const& cmd); - bool SetCamera(ff_msgs::CommandStampedPtr const& cmd); - bool SetCameraRecording(ff_msgs::CommandStampedPtr const& cmd); - bool SetCameraStreaming(ff_msgs::CommandStampedPtr const& cmd); - bool SetCheckObstacles(ff_msgs::CommandStampedPtr const& cmd); - bool SetCheckZones(ff_msgs::CommandStampedPtr const& cmd); - bool SetDataToDisk(ff_msgs::CommandStampedPtr const& cmd); - bool SetEnableAutoReturn(ff_msgs::CommandStampedPtr const& cmd); - bool SetEnableImmediate(ff_msgs::CommandStampedPtr const& cmd); - bool SetEnableReplan(ff_msgs::CommandStampedPtr const& cmd); - bool SetFlashlightBrightness(ff_msgs::CommandStampedPtr const& cmd); - bool SetHolonomicMode(ff_msgs::CommandStampedPtr const& cmd); - bool SetInertia(ff_msgs::CommandStampedPtr const& cmd); - bool SetOperatingLimits(ff_msgs::CommandStampedPtr const& cmd); - bool SetPlan(ff_msgs::CommandStampedPtr const& cmd); - bool SetPlanner(ff_msgs::CommandStampedPtr const& cmd); - bool SetTelemetryRate(ff_msgs::CommandStampedPtr const& cmd); - bool SetZones(ff_msgs::CommandStampedPtr const& cmd); - bool SkipPlanStep(ff_msgs::CommandStampedPtr const& cmd); - bool StartGuestScience(ff_msgs::CommandStampedPtr const& cmd); - bool StartRecording(ff_msgs::CommandStampedPtr const& cmd); - bool StopAllMotion(ff_msgs::CommandStampedPtr const& cmd); - bool StopArm(ff_msgs::CommandStampedPtr const& cmd); - bool StopRecording(ff_msgs::CommandStampedPtr const& cmd); - bool StopGuestScience(ff_msgs::CommandStampedPtr const& cmd); - bool StowArm(ff_msgs::CommandStampedPtr const& cmd); - bool SwitchLocalization(ff_msgs::CommandStampedPtr const& cmd); - bool Undock(ff_msgs::CommandStampedPtr const& cmd); - bool UnloadNodelet(ff_msgs::CommandStampedPtr const& cmd); - bool Unperch(ff_msgs::CommandStampedPtr const& cmd); - bool Unterminate(ff_msgs::CommandStampedPtr const& cmd); - bool Wait(ff_msgs::CommandStampedPtr const& cmd); + bool ArmPanAndTilt(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool AutoReturn(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool CustomGuestScience(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool DeployArm(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool Dock(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool EnableAstrobeeIntercomms( + ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool Fault(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool GripperControl(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool IdlePropulsion(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool InitializeBias(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool LoadNodelet(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool NoOp(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool PausePlan(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool Perch(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool PowerItemOff(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool PowerItemOn(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool Prepare(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool ReacquirePosition(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool ResetEkf(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool RestartGuestScience(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool RunPlan(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool SetCamera(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool SetCameraRecording(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool SetCameraStreaming(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool SetCheckObstacles(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool SetCheckZones(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool SetDataToDisk(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool SetEnableAutoReturn(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool SetEnableImmediate(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool SetEnableReplan(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool SetFlashlightBrightness( + ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool SetHolonomicMode(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool SetInertia(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool SetOperatingLimits(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool SetPlan(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool SetPlanner(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool SetTelemetryRate(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool SetZones(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool SkipPlanStep(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool StartGuestScience(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool StartRecording(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool StopAllMotion(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool StopArm(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool StopGuestScience(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool StopRecording(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool StowArm(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool SwitchLocalization(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool Undock(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool UnloadNodelet(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool Unperch(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool Unterminate(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + bool Wait(ff_msgs::msg::CommandStamped::SharedPtr const cmd); protected: - virtual void Initialize(ros::NodeHandle *nh); + virtual void Initialize(NodeHandle &nh); bool ReadParams(); bool ReadMapperParams(); bool ReadCommand(config_reader::ConfigReader::Table *response, - ff_msgs::CommandStampedPtr cmd); + ff_msgs::msg::CommandStamped::SharedPtr cmd); void PublishAgentState(); OpState* state_; - ExecutiveActionClient arm_ac_; - ExecutiveActionClient dock_ac_; - ExecutiveActionClient localization_ac_; - ExecutiveActionClient motion_ac_; - ExecutiveActionClient perch_ac_; + ExecutiveActionClient arm_ac_; + ExecutiveActionClient dock_ac_; + ExecutiveActionClient localization_ac_; + ExecutiveActionClient motion_ac_; + ExecutiveActionClient perch_ac_; config_reader::ConfigReader config_params_, mapper_config_params_; - ff_msgs::AgentStateStamped agent_state_; - - ff_msgs::AckStamped ack_; - - ff_msgs::CommandStampedPtr sys_monitor_init_fault_response_; - ff_msgs::CommandStampedPtr sys_monitor_heartbeat_fault_response_; - - ff_msgs::CompressedFileAck cf_ack_; - ff_msgs::CompressedFileConstPtr plan_, zones_, data_to_disk_; - - ff_msgs::CameraStatesStamped camera_states_; - ff_msgs::DockStateConstPtr dock_state_; - ff_msgs::FaultStateConstPtr fault_state_; - ff_msgs::GuestScienceConfigConstPtr guest_science_config_; - ff_msgs::MotionStatePtr motion_state_; - ff_msgs::PerchStateConstPtr perch_state_; - - ff_msgs::ArmGoal arm_goal_; - ff_msgs::DockGoal dock_goal_; - ff_msgs::LocalizationGoal localization_goal_; - ff_msgs::MotionGoal motion_goal_; - ff_msgs::PerchGoal perch_goal_; - - ff_util::FreeFlyerServiceClient led_client_; - - geometry_msgs::InertiaStampedConstPtr current_inertia_; - - ros::NodeHandle nh_; - - ros::Publisher agent_state_pub_, cmd_ack_pub_, plan_pub_, plan_status_pub_; - ros::Publisher cf_ack_pub_, gs_cmd_pub_; - - ros::ServiceClient zones_client_, laser_enable_client_; - ros::ServiceClient front_flashlight_client_, back_flashlight_client_; - ros::ServiceClient dock_cam_config_client_, dock_cam_enable_client_; - ros::ServiceClient haz_cam_config_client_, haz_cam_enable_client_; - ros::ServiceClient nav_cam_config_client_, nav_cam_enable_client_; - ros::ServiceClient perch_cam_config_client_, perch_cam_enable_client_; - ros::ServiceClient sci_cam_config_client_, sci_cam_enable_client_; - ros::ServiceClient payload_power_client_, pmc_enable_client_; - ros::ServiceClient set_inertia_client_, set_rate_client_; - ros::ServiceClient set_data_client_, enable_recording_client_; - ros::ServiceClient eps_terminate_client_; - ros::ServiceClient enable_astrobee_intercommunication_client_; - ros::ServiceClient unload_load_nodelet_client_; - ros::ServiceClient set_collision_distance_client_; - - ros::Subscriber cmd_sub_, dock_state_sub_, fault_state_sub_, gs_ack_sub_; - ros::Subscriber heartbeat_sub_, motion_sub_, plan_sub_, zones_sub_, data_sub_; - ros::Subscriber gs_config_sub_, gs_state_sub_, camera_state_sub_; - ros::Subscriber perch_state_sub_, inertia_sub_; - - ros::Timer gs_start_stop_restart_command_timer_, gs_custom_command_timer_; - ros::Timer reload_params_timer_, wait_timer_, sys_monitor_heartbeat_timer_; - ros::Timer sys_monitor_startup_timer_; + ff_msgs::msg::AgentStateStamped agent_state_; + + ff_msgs::msg::AckStamped ack_; + + ff_msgs::msg::CommandStamped::SharedPtr sys_monitor_init_fault_response_; + ff_msgs::msg::CommandStamped::SharedPtr sys_monitor_heartbeat_fault_response_; + + ff_msgs::msg::CompressedFileAck::SharedPtr cf_ack_; + ff_msgs::msg::CompressedFile::SharedPtr plan_, zones_, data_to_disk_; + + ff_msgs::msg::CameraStatesStamped::SharedPtr camera_states_; + ff_msgs::msg::DockState::SharedPtr dock_state_; + ff_msgs::msg::FaultState::SharedPtr fault_state_; + ff_msgs::msg::GuestScienceConfig::SharedPtr guest_science_config_; + ff_msgs::msg::MotionState::SharedPtr motion_state_; + ff_msgs::msg::PerchState::SharedPtr perch_state_; + + ff_msgs::action::Arm::Goal arm_goal_; + ff_msgs::action::Dock::Goal dock_goal_; + ff_msgs::action::Localization::Goal localization_goal_; + ff_msgs::action::Motion::Goal motion_goal_; + ff_msgs::action::Perch::Goal perch_goal_; + + geometry_msgs::msg::InertiaStamped::SharedPtr current_inertia_; + + NodeHandle nh_; + std::shared_ptr cfg_node_; + + Publisher agent_state_pub_; + Publisher cmd_ack_pub_; + Publisher plan_pub_; + Publisher plan_status_pub_; + Publisher cf_ack_pub_; + Publisher gs_cmd_pub_; + + FreeFlyerServiceClient zones_client_; + FreeFlyerServiceClient laser_enable_client_; + FreeFlyerServiceClient + front_flashlight_client_; + FreeFlyerServiceClient + back_flashlight_client_; + FreeFlyerServiceClient dock_cam_config_client_; + FreeFlyerServiceClient dock_cam_enable_client_; + FreeFlyerServiceClient haz_cam_config_client_; + FreeFlyerServiceClient haz_cam_enable_client_; + FreeFlyerServiceClient nav_cam_config_client_; + FreeFlyerServiceClient nav_cam_enable_client_; + FreeFlyerServiceClient perch_cam_config_client_; + FreeFlyerServiceClient perch_cam_enable_client_; + FreeFlyerServiceClient sci_cam_config_client_; + FreeFlyerServiceClient sci_cam_enable_client_; + FreeFlyerServiceClient + payload_power_client_; + FreeFlyerServiceClient pmc_enable_client_; + FreeFlyerServiceClient set_inertia_client_; + FreeFlyerServiceClient set_rate_client_; + FreeFlyerServiceClient set_data_client_; + FreeFlyerServiceClient + enable_recording_client_; + FreeFlyerServiceClient eps_terminate_client_; + FreeFlyerServiceClient + enable_astrobee_intercommunication_client_; + FreeFlyerServiceClient unload_load_nodelet_client_; + FreeFlyerServiceClient set_collision_distance_client_; + FreeFlyerServiceClient led_client_; + + + Subscriber camera_state_sub_; + Subscriber cmd_sub_; + Subscriber data_sub_; + Subscriber dock_state_sub_; + Subscriber fault_state_sub_; + Subscriber gs_ack_sub_; + Subscriber gs_config_sub_; + Subscriber gs_state_sub_; + Subscriber heartbeat_sub_; + Subscriber inertia_sub_; + Subscriber motion_sub_; + Subscriber perch_state_sub_; + Subscriber plan_sub_; + Subscriber zones_sub_; + + ff_util::FreeFlyerTimer gs_start_stop_restart_command_timer_; + ff_util::FreeFlyerTimer gs_custom_command_timer_; + ff_util::FreeFlyerTimer reload_params_timer_; + ff_util::FreeFlyerTimer wait_timer_; + ff_util::FreeFlyerTimer sys_monitor_heartbeat_timer_; + ff_util::FreeFlyerTimer sys_monitor_startup_timer_; sequencer::Sequencer sequencer_; diff --git a/management/executive/include/executive/executive_action_client.h b/management/executive/include/executive/executive_action_client.h index d0531fd6c7..3ecd1c9385 100644 --- a/management/executive/include/executive/executive_action_client.h +++ b/management/executive/include/executive/executive_action_client.h @@ -20,7 +20,7 @@ #ifndef EXECUTIVE_EXECUTIVE_ACTION_CLIENT_H_ #define EXECUTIVE_EXECUTIVE_ACTION_CLIENT_H_ -#include +#include #include #include diff --git a/management/executive/include/executive/op_state.h b/management/executive/include/executive/op_state.h index dc4c5383c6..c96f2188a5 100644 --- a/management/executive/include/executive/op_state.h +++ b/management/executive/include/executive/op_state.h @@ -19,21 +19,19 @@ #ifndef EXECUTIVE_OP_STATE_H_ #define EXECUTIVE_OP_STATE_H_ -#include - #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include -using ff_msgs::CommandConstants; +using ff_msgs::msg::CommandConstants; namespace executive { class Executive; @@ -46,8 +44,8 @@ class OpState { public: virtual ~OpState() {} virtual OpState* StartupState(std::string const& cmd_id = ""); - virtual OpState* HandleCmd(ff_msgs::CommandStampedPtr const& cmd); - OpState* HandleCmd(ff_msgs::CommandStampedPtr const& cmd, + virtual OpState* HandleCmd(ff_msgs::msg::CommandStamped::SharedPtr const cmd); + OpState* HandleCmd(ff_msgs::msg::CommandStamped::SharedPtr const cmd, bool& completed, bool& successful); @@ -60,12 +58,12 @@ class OpState { virtual OpState* HandleWaitCallback(); virtual OpState* HandleGuestScienceAck( - ff_msgs::AckStampedConstPtr const& ack); + ff_msgs::msg::AckStamped::SharedPtr const ack); virtual void AckCmd(std::string const& cmd_id, - uint8_t completed_status = ff_msgs::AckCompletedStatus::OK, - std::string const& message = "", - uint8_t status = ff_msgs::AckStatus::COMPLETED); + uint8_t completed_status = ff_msgs::msg::AckCompletedStatus::OK, + std::string const& message = "", + uint8_t status = ff_msgs::msg::AckStatus::COMPLETED); std::string GenerateActionFailedMsg( ff_util::FreeFlyerActionState::Enum const& state, @@ -74,10 +72,9 @@ class OpState { std::string GetActionString(Action const& action); - virtual bool PausePlan(ff_msgs::CommandStampedPtr const& cmd); + virtual bool PausePlan(ff_msgs::msg::CommandStamped::SharedPtr const cmd); OpState* TransitionToState(unsigned char id); - std::string const& name() const {return name_;} unsigned char const& id() const {return id_;} diff --git a/management/executive/include/executive/op_state_auto_return.h b/management/executive/include/executive/op_state_auto_return.h index a243602253..33d87e3b5a 100644 --- a/management/executive/include/executive/op_state_auto_return.h +++ b/management/executive/include/executive/op_state_auto_return.h @@ -28,7 +28,7 @@ class OpStateAutoReturn : public OpState { public: ~OpStateAutoReturn() {} - OpState* HandleCmd(ff_msgs::CommandStampedPtr const& cmd); + OpState* HandleCmd(ff_msgs::msg::CommandStamped::SharedPtr const cmd); protected: explicit OpStateAutoReturn(std::string const& name, unsigned char id) : diff --git a/management/executive/include/executive/op_state_fault.h b/management/executive/include/executive/op_state_fault.h index dfb39a92a1..9e50507769 100644 --- a/management/executive/include/executive/op_state_fault.h +++ b/management/executive/include/executive/op_state_fault.h @@ -27,10 +27,10 @@ class OpStateFault : public OpState { public: ~OpStateFault() {} - OpState* HandleCmd(ff_msgs::CommandStampedPtr const& cmd); + OpState* HandleCmd(ff_msgs::msg::CommandStamped::SharedPtr const cmd); // Guest Science Ack - OpState* HandleGuestScienceAck(ff_msgs::AckStampedConstPtr const& ack); + OpState* HandleGuestScienceAck(ff_msgs::msg::AckStamped::SharedPtr const ack); OpState* HandleResult(ff_util::FreeFlyerActionState::Enum const& state, std::string const& result_response, diff --git a/management/executive/include/executive/op_state_plan_exec.h b/management/executive/include/executive/op_state_plan_exec.h index 6e85f86cf4..9aa0a29dc9 100644 --- a/management/executive/include/executive/op_state_plan_exec.h +++ b/management/executive/include/executive/op_state_plan_exec.h @@ -22,7 +22,7 @@ #include #include "executive/op_state.h" #include "executive/utils/sequencer/sequencer.h" -#include "ff_msgs/AckCompletedStatus.h" +#include "ff_msgs/msg/ack_completed_status.hpp" namespace executive { class OpStatePlanExec : public OpState { @@ -30,7 +30,7 @@ class OpStatePlanExec : public OpState { ~OpStatePlanExec() {} OpState* StartupState(std::string const& cmd_id); - OpState* HandleCmd(ff_msgs::CommandStampedPtr const& cmd); + OpState* HandleCmd(ff_msgs::msg::CommandStamped::SharedPtr const cmd); OpState* HandleResult(ff_util::FreeFlyerActionState::Enum const& state, std::string const& result_response, @@ -39,16 +39,16 @@ class OpStatePlanExec : public OpState { OpState* HandleWaitCallback(); - OpState* HandleGuestScienceAck(ff_msgs::AckStampedConstPtr const& ack); + OpState* HandleGuestScienceAck(ff_msgs::msg::AckStamped::SharedPtr const ack); void AckCmd(std::string const& cmd_id, - uint8_t completed_status = ff_msgs::AckCompletedStatus::OK, + uint8_t completed_status = ff_msgs::msg::AckCompletedStatus::OK, std::string const& message = "", - uint8_t status = ff_msgs::AckStatus::COMPLETED); + uint8_t status = ff_msgs::msg::AckStatus::COMPLETED); void AckPlanCmdFailed(uint8_t completed_status, std::string const& message); - bool PausePlan(ff_msgs::CommandStampedPtr const& cmd); + bool PausePlan(ff_msgs::msg::CommandStamped::SharedPtr const cmd); protected: explicit OpStatePlanExec(std::string const& name, unsigned char id) : diff --git a/management/executive/include/executive/op_state_ready.h b/management/executive/include/executive/op_state_ready.h index f56c3d0fe8..61fa0e9012 100644 --- a/management/executive/include/executive/op_state_ready.h +++ b/management/executive/include/executive/op_state_ready.h @@ -28,10 +28,10 @@ class OpStateReady : public OpState { public: ~OpStateReady() {} - OpState* HandleCmd(ff_msgs::CommandStampedPtr const& cmd); + OpState* HandleCmd(ff_msgs::msg::CommandStamped::SharedPtr const cmd); // Guest Science Ack - OpState* HandleGuestScienceAck(ff_msgs::AckStampedConstPtr const& ack); + OpState* HandleGuestScienceAck(ff_msgs::msg::AckStamped::SharedPtr const ack); protected: explicit OpStateReady(std::string const& name, unsigned char id) : diff --git a/management/executive/include/executive/op_state_repo.h b/management/executive/include/executive/op_state_repo.h index 11f929dda2..83f47511b2 100644 --- a/management/executive/include/executive/op_state_repo.h +++ b/management/executive/include/executive/op_state_repo.h @@ -19,8 +19,6 @@ #ifndef EXECUTIVE_OP_STATE_REPO_H_ #define EXECUTIVE_OP_STATE_REPO_H_ -#include - #include "executive/op_state.h" #include "executive/op_state_auto_return.h" #include "executive/op_state_fault.h" @@ -28,7 +26,7 @@ #include "executive/op_state_ready.h" #include "executive/op_state_teleop.h" -#include "ff_msgs/OpState.h" +#include "ff_msgs/msg/op_state.hpp" namespace executive { /** diff --git a/management/executive/include/executive/op_state_teleop.h b/management/executive/include/executive/op_state_teleop.h index caaf8d6a6a..e322ce0bd2 100644 --- a/management/executive/include/executive/op_state_teleop.h +++ b/management/executive/include/executive/op_state_teleop.h @@ -28,7 +28,7 @@ class OpStateTeleop : public OpState { public: ~OpStateTeleop() {} - OpState* HandleCmd(ff_msgs::CommandStampedPtr const& cmd); + OpState* HandleCmd(ff_msgs::msg::CommandStamped::SharedPtr const cmd); OpState* HandleResult(ff_util::FreeFlyerActionState::Enum const& state, std::string const& result_response, @@ -42,7 +42,7 @@ class OpStateTeleop : public OpState { private: // allow creation only by repo friend class OpStateRepo; - ff_msgs::CommandStampedPtr move_cmd_; + ff_msgs::msg::CommandStamped::SharedPtr move_cmd_; }; } // namespace executive #endif // EXECUTIVE_OP_STATE_TELEOP_H_ diff --git a/management/executive/include/executive/utils/sequencer/command_conversion.h b/management/executive/include/executive/utils/sequencer/command_conversion.h index 14e2e6011b..df8c3da359 100644 --- a/management/executive/include/executive/utils/sequencer/command_conversion.h +++ b/management/executive/include/executive/utils/sequencer/command_conversion.h @@ -20,6 +20,8 @@ #ifndef EXECUTIVE_UTILS_SEQUENCER_COMMAND_CONVERSION_H_ #define EXECUTIVE_UTILS_SEQUENCER_COMMAND_CONVERSION_H_ +#include + #include #include #include @@ -44,7 +46,7 @@ namespace internal { // a function that takes plan command and generates a DDS-based command // if the conversion is successful, return true. otherwise... don't. using GenerateFn = bool (*)(const jsonloader::Command * plan_cmd, - ff_msgs::CommandStamped * dds_cmd); + ff_msgs::msg::CommandStamped * dds_cmd); struct CommandInfo { std::string name; diff --git a/management/executive/include/executive/utils/sequencer/plan_io.h b/management/executive/include/executive/utils/sequencer/plan_io.h index 1213db29bf..f343806a92 100644 --- a/management/executive/include/executive/utils/sequencer/plan_io.h +++ b/management/executive/include/executive/utils/sequencer/plan_io.h @@ -21,7 +21,7 @@ #ifndef EXECUTIVE_UTILS_SEQUENCER_PLAN_IO_H_ #define EXECUTIVE_UTILS_SEQUENCER_PLAN_IO_H_ -#include +#include #include #include diff --git a/management/executive/include/executive/utils/sequencer/sequencer.h b/management/executive/include/executive/utils/sequencer/sequencer.h index 71b71d4142..6e95116cb4 100644 --- a/management/executive/include/executive/utils/sequencer/sequencer.h +++ b/management/executive/include/executive/utils/sequencer/sequencer.h @@ -20,17 +20,17 @@ #ifndef EXECUTIVE_UTILS_SEQUENCER_SEQUENCER_H_ #define EXECUTIVE_UTILS_SEQUENCER_SEQUENCER_H_ -#include -#include +#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include -#include +#include #include #include @@ -52,20 +52,20 @@ class Sequencer { // because unions can't even :/ OMG. ItemType CurrentType(bool reset_time = true) noexcept; - ff_msgs::CommandStamped::Ptr CurrentCommand() noexcept; + ff_msgs::msg::CommandStamped::SharedPtr CurrentCommand() noexcept; jsonloader::Segment CurrentSegment() noexcept; // give feedback about the end of the current item (command/segment) // this advances the current item if it is a successful ack. // // returns true if there are more command/segments in the plan. - bool Feedback(ff_msgs::AckCompletedStatus const& ack) noexcept; + bool Feedback(ff_msgs::msg::AckCompletedStatus const& ack) noexcept; // give feedback about an index in the current segment - void Feedback(ff_msgs::ControlFeedback const& progress) noexcept; + void Feedback(ff_msgs::msg::ControlFeedback const& progress) noexcept; // get the current plan status - ff_msgs::PlanStatusStamped const& plan_status() noexcept; + ff_msgs::msg::PlanStatusStamped const& plan_status() noexcept; // I can haz validity? bool valid() const noexcept; @@ -76,24 +76,28 @@ class Sequencer { bool HaveOperatingLimits() const noexcept; // get the goods - geometry_msgs::InertiaStamped GetInertia() const noexcept; - bool GetOperatingLimits(ff_msgs::AgentStateStamped &state) const noexcept; + geometry_msgs::msg::InertiaStamped GetInertia() const noexcept; + bool GetOperatingLimits(ff_msgs::msg::AgentStateStamped &state) const noexcept; + + void SetNodeHandle(NodeHandle nh); private: - int AppendStatus(ff_msgs::Status const& msg) noexcept; + int AppendStatus(ff_msgs::msg::Status const& msg) noexcept; void Reset() noexcept; - friend bool LoadPlan(ff_msgs::CompressedFile::ConstPtr const& cf, + friend bool LoadPlan(ff_msgs::msg::CompressedFile::SharedPtr const& cf, Sequencer * seq); bool valid_; jsonloader::Plan plan_; - ff_msgs::PlanStatusStamped status_; + ff_msgs::msg::PlanStatusStamped status_; + + NodeHandle nh_; // when we started the current item - ros::Time start_; + rclcpp::Time start_; // milestone is a station or segment within a plan int current_milestone_; @@ -111,9 +115,9 @@ class Sequencer { // load a plan from a compressed file. // returns true if everything be cool, otherwise not. -bool LoadPlan(ff_msgs::CompressedFile::ConstPtr const& cf, Sequencer *seq); +bool LoadPlan(ff_msgs::msg::CompressedFile::SharedPtr const& cf, Sequencer *seq); -std::vector +std::vector Segment2Trajectory(jsonloader::Segment const& segment); } // end namespace sequencer diff --git a/management/executive/launch/executive.launch b/management/executive/launch/executive.launch deleted file mode 100644 index dbf6574d5d..0000000000 --- a/management/executive/launch/executive.launch +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/management/executive/nodelet_plugins.xml b/management/executive/nodelet_plugins.xml deleted file mode 100644 index a88893902c..0000000000 --- a/management/executive/nodelet_plugins.xml +++ /dev/null @@ -1,10 +0,0 @@ - - - - Nodelet to keep track of the state of the robot and decide which commands - to execute. - - - diff --git a/management/executive/package.xml b/management/executive/package.xml index 4428a2275c..3671ca0997 100644 --- a/management/executive/package.xml +++ b/management/executive/package.xml @@ -1,5 +1,5 @@ - + executive 0.0.0 @@ -14,22 +14,23 @@ Astrobee Flight Software - catkin - roscpp + ament_cmake + rclcpp + rclcpp_component ff_msgs ff_hw_msgs - nodelet config_reader ff_util jsonloader - roscpp - ff_msgs - ff_hw_msgs - nodelet - config_reader - ff_util - jsonloader + + rclcpp + rclcpp_component + ff_msgs + ff_hw_msgs + config_reader + ff_util + jsonloader - + ament_cmake diff --git a/management/executive/src/executive.cc b/management/executive/src/executive.cc index b958aba475..9af1afad89 100644 --- a/management/executive/src/executive.cc +++ b/management/executive/src/executive.cc @@ -22,13 +22,16 @@ #include "executive/op_state.h" #include "executive/op_state_repo.h" +FF_DEFINE_LOGGER("executive"); + namespace executive { -Executive::Executive() : - ff_util::FreeFlyerNodelet(NODE_EXECUTIVE, true), +Executive::Executive(const rclcpp::NodeOptions& options) : + ff_util::FreeFlyerComponent(options, NODE_EXECUTIVE, true), state_(OpStateRepo::Instance()->ready()), - sys_monitor_init_fault_response_(new ff_msgs::CommandStamped()), - sys_monitor_heartbeat_fault_response_(new ff_msgs::CommandStamped()), + sys_monitor_init_fault_response_(new ff_msgs::msg::CommandStamped()), + sys_monitor_heartbeat_fault_response_(new ff_msgs::msg::CommandStamped()), + camera_states_(new ff_msgs::msg::CameraStatesStamped()), dock_state_(NULL), fault_state_(NULL), guest_science_config_(NULL), @@ -58,44 +61,45 @@ Executive::~Executive() { } /************************ Message and timeout callbacks ***********************/ -void Executive::CameraStatesCallback(ff_msgs::CameraStatesStampedConstPtr const& - state) { +void Executive::CameraStatesCallback( + ff_msgs::msg::CameraStatesStamped::SharedPtr const state) { unsigned int i, j; bool streaming = false; - ff_hw_msgs::ConfigureSystemLeds led_srv; + ff_util::FreeFlyerService led_srv; // State array is only one array and the camera states array is at most 5 // elements so this doesn't waste too much time // Don't care about cameras not in the camera states array for (i = 0; i < state->states.size(); i++) { - for (j = 0; j < camera_states_.states.size(); j++) { + for (j = 0; j < camera_states_->states.size(); j++) { if (state->states[i].camera_name == - camera_states_.states[j].camera_name) { - camera_states_.states[j].streaming = state->states[i].streaming; + camera_states_->states[j].camera_name) { + camera_states_->states[j].streaming = state->states[i].streaming; } } } // The state message usually only contains one camera so we have to go through // all the camera states to see if any are streaming - for (i = 0; i < camera_states_.states.size(); i++) { - streaming |= camera_states_.states[i].streaming; + for (i = 0; i < camera_states_->states.size(); i++) { + streaming |= camera_states_->states[i].streaming; } if (streaming && !live_led_on_) { - led_srv.request.live = ff_hw_msgs::ConfigureSystemLeds::Request::ON; + led_srv.request->live = ff_hw_msgs::srv::ConfigureSystemLeds::Request::ON; if (ConfigureLed(led_srv)) { live_led_on_ = true; } } else if (!streaming && live_led_on_) { - led_srv.request.live = ff_hw_msgs::ConfigureSystemLeds::Request::OFF; + led_srv.request->live = ff_hw_msgs::srv::ConfigureSystemLeds::Request::OFF; if (ConfigureLed(led_srv)) { live_led_on_ = false; } } } -void Executive::CmdCallback(ff_msgs::CommandStampedPtr const& cmd) { + +void Executive::CmdCallback(ff_msgs::msg::CommandStamped::SharedPtr const cmd) { // Check to see if the command came from a guest science apk. If it did, // make sure a primary apk is running if (cmd->cmd_origin == "guest_science") { @@ -104,84 +108,87 @@ void Executive::CmdCallback(ff_msgs::CommandStampedPtr const& cmd) { // running if (primary_apk_running_ == "None") { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, "Can't run a gs command when a primary apk not running."); return; } } // TODO(Katie) add more checks - SetOpState(state_->HandleCmd(cmd)); } -void Executive::DataToDiskCallback(ff_msgs::CompressedFileConstPtr const& - data) { +void Executive::DataToDiskCallback( + ff_msgs::msg::CompressedFile::SharedPtr const data) { data_to_disk_ = data; - cf_ack_.header.stamp = ros::Time::now(); - cf_ack_.id = data_to_disk_->id; - cf_ack_pub_.publish(cf_ack_); + cf_ack_->header.stamp = GetTimeNow(); + cf_ack_->id = data_to_disk_->id; + cf_ack_pub_->publish(*cf_ack_); } -void Executive::DockStateCallback(ff_msgs::DockStateConstPtr const& state) { +void Executive::DockStateCallback( + ff_msgs::msg::DockState::SharedPtr const state) { dock_state_ = state; // Check to see if the dock state is docking/docked/undocking. If it is, we // can change the mobility state to docking/docked. // Docking max state signifies that we are docking - if (dock_state_->state <= ff_msgs::DockState::DOCKING_MAX_STATE && - dock_state_->state > ff_msgs::DockState::UNDOCKED) { - SetMobilityState(ff_msgs::MobilityState::DOCKING, dock_state_->state); + if (dock_state_->state <= ff_msgs::msg::DockState::DOCKING_MAX_STATE && + dock_state_->state > ff_msgs::msg::DockState::UNDOCKED) { + SetMobilityState(ff_msgs::msg::MobilityState::DOCKING, dock_state_->state); } // If the dock state is undocked, the mobility state needs to be set to // whatever the motion state is. - if (dock_state_->state == ff_msgs::DockState::UNDOCKED) { + if (dock_state_->state == ff_msgs::msg::DockState::UNDOCKED) { SetMobilityState(); } } -void Executive::FaultStateCallback(ff_msgs::FaultStateConstPtr const& state) { - ff_hw_msgs::ConfigureSystemLeds led_srv; +void Executive::FaultStateCallback( + ff_msgs::msg::FaultState::SharedPtr const state) { + ff_util::FreeFlyerService led_srv; fault_state_ = state; // Check if we are in the fault state - if (state_->id() == ff_msgs::OpState::FAULT) { + if (state_->id() == ff_msgs::msg::OpState::FAULT) { // Check if the blocked fault is cleared - if (state->state != ff_msgs::FaultState::BLOCKED) { + if (state->state != ff_msgs::msg::FaultState::BLOCKED) { // Turn a2 led off so the astronauts can see there is no longer a fault - led_srv.request.status_a2 = ff_hw_msgs::ConfigureSystemLeds::Request::OFF; + led_srv.request->status_a2 = + ff_hw_msgs::srv::ConfigureSystemLeds::Request::OFF; ConfigureLed(led_srv); // Check if an action is in progress, if so transition to teleop // Otherwise transition to ready if (AreActionsRunning()) { - SetOpState(state_->TransitionToState(ff_msgs::OpState::TELEOPERATION)); + SetOpState( + state_->TransitionToState(ff_msgs::msg::OpState::TELEOPERATION)); } else { - SetOpState(state_->TransitionToState(ff_msgs::OpState::READY)); + SetOpState(state_->TransitionToState(ff_msgs::msg::OpState::READY)); } } } else { // Check if a blocking fault is occurring - if (state->state == ff_msgs::FaultState::BLOCKED) { + if (state->state == ff_msgs::msg::FaultState::BLOCKED) { // Turn a2 led on and blinking so the astronauts can see there is a fault - led_srv.request.status_a2 = - ff_hw_msgs::ConfigureSystemLeds::Request::FAST; + led_srv.request->status_a2 = + ff_hw_msgs::srv::ConfigureSystemLeds::Request::FAST; ConfigureLed(led_srv); // If so, transiton to fault state - SetOpState(state_->TransitionToState(ff_msgs::OpState::FAULT)); + SetOpState(state_->TransitionToState(ff_msgs::msg::OpState::FAULT)); } } } -void Executive::GuestScienceAckCallback(ff_msgs::AckStampedConstPtr const& - ack) { +void Executive::GuestScienceAckCallback( + ff_msgs::msg::AckStamped::SharedPtr const ack) { if (ack->cmd_id == "plan") { SetOpState(state_->HandleGuestScienceAck(ack)); } else { - cmd_ack_pub_.publish(ack); + cmd_ack_pub_->publish(*ack); } // Clear guest science command timers @@ -194,13 +201,13 @@ void Executive::GuestScienceAckCallback(ff_msgs::AckStampedConstPtr const& } } -void Executive::GuestScienceConfigCallback(ff_msgs::GuestScienceConfigConstPtr - const& config) { +void Executive::GuestScienceConfigCallback( + ff_msgs::msg::GuestScienceConfig::SharedPtr const config) { guest_science_config_ = config; } -void Executive::GuestScienceStateCallback(ff_msgs::GuestScienceStateConstPtr - const& state) { +void Executive::GuestScienceStateCallback( + ff_msgs::msg::GuestScienceState::SharedPtr const state) { unsigned int i = 0; std::string primary_apk = "None"; @@ -215,26 +222,26 @@ void Executive::GuestScienceStateCallback(ff_msgs::GuestScienceStateConstPtr // Currently, this shouldn't happen since we only poll for the current apks on // startup. if (guest_science_config_->serial != state->serial) { - NODELET_WARN("Guest science state and config serial doesn't match."); + FF_WARN("Guest science state and config serial doesn't match."); return; } // Also check that the state and config are the same size because it would be // really bad if they weren't. - if (state->runningApks.size() != guest_science_config_->apks.size()) { - NODELET_ERROR("Guest science apk array size doesn't match but serial does"); + if (state->running_apks.size() != guest_science_config_->apks.size()) { + FF_ERROR("Guest science apk array size doesn't match but serial does"); return; } // Check to see if any apks are running - for (i = 0; i < state->runningApks.size(); i++) { - if (state->runningApks[i]) { + for (i = 0; i < state->running_apks.size(); i++) { + if (state->running_apks[i]) { // Check if primary if (guest_science_config_->apks[i].primary) { if (primary_apk == "None") { primary_apk = guest_science_config_->apks[i].apk_name; } else { - NODELET_ERROR("More than 1 primary apk running in gs state."); + FF_ERROR("More than 1 primary apk running in gs state."); } } } @@ -243,31 +250,29 @@ void Executive::GuestScienceStateCallback(ff_msgs::GuestScienceStateConstPtr primary_apk_running_ = primary_apk; if (primary_apk_running_ != "None") { - agent_state_.guest_science_state.state = ff_msgs::ExecState::EXECUTING; + agent_state_.guest_science_state.state = ff_msgs::msg::ExecState::EXECUTING; } else { - agent_state_.guest_science_state.state = ff_msgs::ExecState::IDLE; + agent_state_.guest_science_state.state = ff_msgs::msg::ExecState::IDLE; } } -void Executive::GuestScienceCustomCmdTimeoutCallback( - ros::TimerEvent const& te) { +void Executive::GuestScienceCustomCmdTimeoutCallback() { std::string err_msg = "GS manager didn't return an ack for the custom guest "; err_msg += "science command in the timeout specified. The GS manager may not"; err_msg += " have started or it may have died."; PublishCmdAck(gs_custom_cmd_id_, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, err_msg); // Don't need to stop timer because it is a one shot timer gs_custom_cmd_id_ = ""; } -void Executive::GuestScienceStartStopRestartCmdTimeoutCallback( - ros::TimerEvent const& te) { +void Executive::GuestScienceStartStopRestartCmdTimeoutCallback() { std::string err_msg = "GS manager didn't return an ack for the start/stop "; err_msg += "guest science command in the timeout specified. The GS manager "; err_msg += "may not have started or it may have died."; PublishCmdAck(gs_start_stop_restart_cmd_id_, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, err_msg); // Don't need to stop timer because it is a one shot timer gs_start_stop_restart_cmd_id_ = ""; @@ -275,56 +280,59 @@ void Executive::GuestScienceStartStopRestartCmdTimeoutCallback( void Executive::InertiaCallback( - geometry_msgs::InertiaStampedConstPtr const& inertia) { + geometry_msgs::msg::InertiaStamped::SharedPtr const inertia) { current_inertia_ = inertia; } void Executive::LedConnectedCallback() { - ff_hw_msgs::ConfigureSystemLeds led_srv; + ff_util::FreeFlyerService led_srv; // Set video light since this means the fsw started - led_srv.request.video = ff_hw_msgs::ConfigureSystemLeds::Request::ON; + led_srv.request->video = ff_hw_msgs::srv::ConfigureSystemLeds::Request::ON; // Check if we are in the fault state - if (state_->id() == ff_msgs::OpState::FAULT) { + if (state_->id() == ff_msgs::msg::OpState::FAULT) { // If so, turn a2 led on and blinking so astronauts can see there is a fault - led_srv.request.status_a2 = ff_hw_msgs::ConfigureSystemLeds::Request::FAST; + led_srv.request->status_a2 = + ff_hw_msgs::srv::ConfigureSystemLeds::Request::FAST; ConfigureLed(led_srv); } else { // Turn a2 led off to signify that the executive is ready. - led_srv.request.status_a2 = ff_hw_msgs::ConfigureSystemLeds::Request::OFF; + led_srv.request->status_a2 = + ff_hw_msgs::srv::ConfigureSystemLeds::Request::OFF; ConfigureLed(led_srv); } } -void Executive::MotionStateCallback(ff_msgs::MotionStatePtr const& state) { +void Executive::MotionStateCallback( + ff_msgs::msg::MotionState::SharedPtr const state) { if (motion_state_ == NULL) { motion_state_ = state; } // Check the current motion state to see if it maps to our mobility state. If // it does, set the mobility state and our stored motion state. - if (state->state == ff_msgs::MotionState::INITIALIZING) { + if (state->state == ff_msgs::msg::MotionState::INITIALIZING) { // Set motion state to idle a.k.a mobility state to drifting when the // mobility subsystem is initializing - motion_state_->state = ff_msgs::MotionState::IDLE; - } else if (state->state == ff_msgs::MotionState::IDLE || - state->state == ff_msgs::MotionState::IDLING || - state->state == ff_msgs::MotionState::STOPPED || - state->state == ff_msgs::MotionState::STOPPING || - state->state == ff_msgs::MotionState::CONTROLLING || - state->state == ff_msgs::MotionState::BOOTSTRAPPING) { + motion_state_->state = ff_msgs::msg::MotionState::IDLE; + } else if (state->state == ff_msgs::msg::MotionState::IDLE || + state->state == ff_msgs::msg::MotionState::IDLING || + state->state == ff_msgs::msg::MotionState::STOPPED || + state->state == ff_msgs::msg::MotionState::STOPPING || + state->state == ff_msgs::msg::MotionState::CONTROLLING || + state->state == ff_msgs::msg::MotionState::BOOTSTRAPPING) { motion_state_->state = state->state; } // Check to see if we are docking or undocking. If we are, don't use the // motion state as our mobility state. if (dock_state_ != NULL) { - if (dock_state_->state > ff_msgs::DockState::UNDOCKED) { + if (dock_state_->state > ff_msgs::msg::DockState::UNDOCKED) { // If dock state is unknown or initializing, use motion state to set // mobility state - if (dock_state_->state != ff_msgs::DockState::UNKNOWN && - dock_state_->state != ff_msgs::DockState::INITIALIZING) { + if (dock_state_->state != ff_msgs::msg::DockState::UNKNOWN && + dock_state_->state != ff_msgs::msg::DockState::INITIALIZING) { return; } } @@ -333,11 +341,11 @@ void Executive::MotionStateCallback(ff_msgs::MotionStatePtr const& state) { // Check to see if we are perching or unperching. If we are, don't use the // motion state as our mobility state. if (perch_state_ != NULL) { - if (perch_state_->state > ff_msgs::PerchState::UNPERCHED) { + if (perch_state_->state > ff_msgs::msg::PerchState::UNPERCHED) { // If perching state is unknown or initializing, use motion state to set // mobility state - if (perch_state_->state != ff_msgs::PerchState::UNKNOWN && - perch_state_->state != ff_msgs::PerchState::INITIALIZING) { + if (perch_state_->state != ff_msgs::msg::PerchState::UNKNOWN && + perch_state_->state != ff_msgs::msg::PerchState::INITIALIZING) { return; } } @@ -347,15 +355,17 @@ void Executive::MotionStateCallback(ff_msgs::MotionStatePtr const& state) { SetMobilityState(); } -void Executive::PerchStateCallback(ff_msgs::PerchStateConstPtr const& state) { +void Executive::PerchStateCallback( + ff_msgs::msg::PerchState::SharedPtr const state) { perch_state_ = state; // Check to see if the perch state is perching/perched/unperching. If it is, // we can change the mobility state to perching/perched. // Perching max state signifies that we are perching - if (perch_state_->state <= ff_msgs::PerchState::PERCHING_MAX_STATE && - perch_state_->state > ff_msgs::PerchState::UNPERCHED) { - SetMobilityState(ff_msgs::MobilityState::PERCHING, perch_state_->state); + if (perch_state_->state <= ff_msgs::msg::PerchState::PERCHING_MAX_STATE && + perch_state_->state > ff_msgs::msg::PerchState::UNPERCHED) { + SetMobilityState(ff_msgs::msg::MobilityState::PERCHING, + perch_state_->state); } // Check to see if we are docked. If we are, don't use the motion state @@ -364,28 +374,29 @@ void Executive::PerchStateCallback(ff_msgs::PerchStateConstPtr const& state) { // already docked when FSW starts. // More docking and undocking states might be added to the check if needed if (dock_state_ != NULL) { - if (dock_state_->state == ff_msgs::DockState::DOCKED) { + if (dock_state_->state == ff_msgs::msg::DockState::DOCKED) { return; } } // If the perch state is unperched, the mobility state needs to be set to // whatever the motion state is. - if (perch_state_->state == ff_msgs::PerchState::UNPERCHED) { + if (perch_state_->state == ff_msgs::msg::PerchState::UNPERCHED) { SetMobilityState(); } } -void Executive::PlanCallback(ff_msgs::CompressedFileConstPtr const& plan) { +void Executive::PlanCallback( + ff_msgs::msg::CompressedFile::SharedPtr const plan) { plan_ = plan; - cf_ack_.header.stamp = ros::Time::now(); - cf_ack_.id = plan_->id; - cf_ack_pub_.publish(cf_ack_); + cf_ack_->header.stamp = GetTimeNow(); + cf_ack_->id = plan_->id; + cf_ack_pub_->publish(*cf_ack_); } void Executive::SysMonitorHeartbeatCallback( - ff_msgs::HeartbeatConstPtr const& heartbeat) { + ff_msgs::msg::Heartbeat::SharedPtr const heartbeat) { sys_monitor_heartbeat_timer_.stop(); // Stop the startup timer everytime since it isn't an expensive operation @@ -398,12 +409,12 @@ void Executive::SysMonitorHeartbeatCallback( // Check fault state before transitioning to ready because we need to make // sure there aren't any other blocking faults occurring if (fault_state_ != NULL) { - if (fault_state_->state != ff_msgs::FaultState::BLOCKED) { + if (fault_state_->state != ff_msgs::msg::FaultState::BLOCKED) { if (AreActionsRunning()) { SetOpState(state_->TransitionToState( - ff_msgs::OpState::TELEOPERATION)); + ff_msgs::msg::OpState::TELEOPERATION)); } else { - SetOpState(state_->TransitionToState(ff_msgs::OpState::READY)); + SetOpState(state_->TransitionToState(ff_msgs::msg::OpState::READY)); } } } @@ -414,14 +425,14 @@ void Executive::SysMonitorHeartbeatCallback( // if there is a fault in the fault array, the executive needs to trigger the // system monitor initialization fault. if (heartbeat->faults.size() > 0 && !sys_monitor_init_fault_occurring_) { - NODELET_ERROR("System monitor initalization fault detected in executive."); + FF_ERROR("System monitor initalization fault detected in executive."); sys_monitor_init_fault_occurring_ = true; sys_monitor_init_fault_response_->cmd_id = "executive" + - std::to_string(ros::Time::now().sec); + std::to_string(GetTimeNow().seconds()); CmdCallback(sys_monitor_init_fault_response_); // If fault is blocking, transition to fault state if (sys_monitor_init_fault_blocking_) { - SetOpState(state_->TransitionToState(ff_msgs::OpState::FAULT)); + SetOpState(state_->TransitionToState(ff_msgs::msg::OpState::FAULT)); } return; // Check initialiization fault went away @@ -432,12 +443,12 @@ void Executive::SysMonitorHeartbeatCallback( // Check fault state before transitioning to ready because we need to // make sure there aren't any other blocking faults occurring if (fault_state_ != NULL) { - if (fault_state_->state != ff_msgs::FaultState::BLOCKED) { + if (fault_state_->state != ff_msgs::msg::FaultState::BLOCKED) { if (AreActionsRunning()) { SetOpState(state_->TransitionToState( - ff_msgs::OpState::TELEOPERATION)); + ff_msgs::msg::OpState::TELEOPERATION)); } else { - SetOpState(state_->TransitionToState(ff_msgs::OpState::READY)); + SetOpState(state_->TransitionToState(ff_msgs::msg::OpState::READY)); } } } @@ -447,32 +458,33 @@ void Executive::SysMonitorHeartbeatCallback( sys_monitor_heartbeat_timer_.start(); } -void Executive::SysMonitorTimeoutCallback(ros::TimerEvent const& te) { - NODELET_ERROR("System monitor heartbeat fault detected in executive."); +void Executive::SysMonitorTimeoutCallback() { + FF_ERROR("System monitor heartbeat fault detected in executive."); // If the executive doesn't receive a heartbeat from the system monitor, it // needs to trigger the system monitor heartbeat missed fault. sys_monitor_heartbeat_fault_occurring_ = true; sys_monitor_heartbeat_fault_response_->cmd_id = "executive" + - std::to_string(ros::Time::now().sec); + std::to_string(GetTimeNow().seconds()); CmdCallback(sys_monitor_heartbeat_fault_response_); // If fault is blocking, transition to fault state if (sys_monitor_heartbeat_fault_blocking_) { - SetOpState(state_->TransitionToState(ff_msgs::OpState::FAULT)); + SetOpState(state_->TransitionToState(ff_msgs::msg::OpState::FAULT)); } sys_monitor_heartbeat_timer_.stop(); } -void Executive::WaitCallback(ros::TimerEvent const& te) { +void Executive::WaitCallback() { wait_timer_.stop(); SetOpState(state_->HandleWaitCallback()); } -void Executive::ZonesCallback(ff_msgs::CompressedFileConstPtr const& zones) { +void Executive::ZonesCallback( + ff_msgs::msg::CompressedFile::SharedPtr const zones) { zones_ = zones; - cf_ack_.header.stamp = ros::Time::now(); - cf_ack_.id = zones_->id; - cf_ack_pub_.publish(cf_ack_); + cf_ack_->header.stamp = GetTimeNow(); + cf_ack_->id = zones_->id; + cf_ack_pub_->publish(*cf_ack_); } /************************ Action based commands *******************************/ @@ -494,7 +506,7 @@ void Executive::CancelAction(Action action, std::string cmd) { arm_ac_.CancelGoal(); err_msg = "Arm action was canceled due to a " + cmd + " command."; state_->AckCmd(arm_ac_.cmd_id(), - ff_msgs::AckCompletedStatus::CANCELED, + ff_msgs::msg::AckCompletedStatus::CANCELED, err_msg); arm_ac_.SetCmdInfo(NONE, ""); break; @@ -503,7 +515,7 @@ void Executive::CancelAction(Action action, std::string cmd) { dock_ac_.CancelGoal(); err_msg = "Dock action was canceled due to a " + cmd + " command."; state_->AckCmd(dock_ac_.cmd_id(), - ff_msgs::AckCompletedStatus::CANCELED, + ff_msgs::msg::AckCompletedStatus::CANCELED, err_msg); dock_ac_.SetCmdInfo(NONE, ""); break; @@ -513,7 +525,7 @@ void Executive::CancelAction(Action action, std::string cmd) { motion_ac_.CancelGoal(); err_msg = "Motion action was canceled due to a " + cmd + " command."; state_->AckCmd(motion_ac_.cmd_id(), - ff_msgs::AckCompletedStatus::CANCELED, + ff_msgs::msg::AckCompletedStatus::CANCELED, err_msg); motion_ac_.SetCmdInfo(NONE, ""); break; @@ -522,18 +534,18 @@ void Executive::CancelAction(Action action, std::string cmd) { perch_ac_.CancelGoal(); err_msg = "Perch action was canceled due to a " + cmd + " command."; state_->AckCmd(perch_ac_.cmd_id(), - ff_msgs::AckCompletedStatus::CANCELED, + ff_msgs::msg::AckCompletedStatus::CANCELED, err_msg); perch_ac_.SetCmdInfo(NONE, ""); break; default: - NODELET_ERROR("Action to cancel not recognized!"); + FF_ERROR("Action to cancel not recognized!"); return; } } // TODO(Katie) Add stow check -bool Executive::FillArmGoal(ff_msgs::CommandStampedPtr const& cmd) { +bool Executive::FillArmGoal(ff_msgs::msg::CommandStamped::SharedPtr const cmd) { bool successful = true; std::string err_msg; if (cmd->cmd_name == CommandConstants::CMD_NAME_ARM_PAN_AND_TILT) { @@ -541,9 +553,9 @@ bool Executive::FillArmGoal(ff_msgs::CommandStampedPtr const& cmd) { // tilt value, and the last is a string specifying whether to pan, tilt or // both if (cmd->args.size() != 3 || - cmd->args[0].data_type != ff_msgs::CommandArg::DATA_TYPE_FLOAT || - cmd->args[1].data_type != ff_msgs::CommandArg::DATA_TYPE_FLOAT || - cmd->args[2].data_type != ff_msgs::CommandArg::DATA_TYPE_STRING) { + cmd->args[0].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_FLOAT || + cmd->args[1].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_FLOAT || + cmd->args[2].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_STRING) { successful = false; err_msg = "Malformed arguments for pan and tilt command!"; } else { @@ -553,11 +565,11 @@ bool Executive::FillArmGoal(ff_msgs::CommandStampedPtr const& cmd) { arm_goal_.pan = cmd->args[0].f; arm_goal_.tilt = cmd->args[1].f; if (cmd->args[2].s == "Pan" || cmd->args[2].s == "pan") { - arm_goal_.command = ff_msgs::ArmGoal::ARM_PAN; + arm_goal_.command = ff_msgs::action::Arm::Goal::ARM_PAN; } else if (cmd->args[2].s == "Tilt" || cmd->args[2].s == "tilt") { - arm_goal_.command = ff_msgs::ArmGoal::ARM_TILT; + arm_goal_.command = ff_msgs::action::Arm::Goal::ARM_TILT; } else if (cmd->args[2].s == "Both" || cmd->args[2].s == "both") { - arm_goal_.command = ff_msgs::ArmGoal::ARM_MOVE; + arm_goal_.command = ff_msgs::action::Arm::Goal::ARM_MOVE; } else { successful = false; err_msg = "Unrecognized which parameter in pan and tilt command. Got: " @@ -565,41 +577,41 @@ bool Executive::FillArmGoal(ff_msgs::CommandStampedPtr const& cmd) { } } } else if (cmd->cmd_name == CommandConstants::CMD_NAME_DEPLOY_ARM) { - arm_goal_.command = ff_msgs::ArmGoal::ARM_DEPLOY; + arm_goal_.command = ff_msgs::action::Arm::Goal::ARM_DEPLOY; } else if (cmd->cmd_name == CommandConstants::CMD_NAME_GRIPPER_CONTROL) { // Gripper control has one argument which is a booleanused to specify // whether to open or close the arm if (cmd->args.size() != 1 || - cmd->args[0].data_type != ff_msgs::CommandArg::DATA_TYPE_BOOL) { + cmd->args[0].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_BOOL) { successful = false; err_msg = "Malformed arguments for gripper control command!"; } else { // True means open if (cmd->args[0].b) { - arm_goal_.command = ff_msgs::ArmGoal::GRIPPER_OPEN; + arm_goal_.command = ff_msgs::action::Arm::Goal::GRIPPER_OPEN; } else { - arm_goal_.command = ff_msgs::ArmGoal::GRIPPER_CLOSE; + arm_goal_.command = ff_msgs::action::Arm::Goal::GRIPPER_CLOSE; } } } else if (cmd->cmd_name == CommandConstants::CMD_NAME_STOW_ARM) { - arm_goal_.command = ff_msgs::ArmGoal::ARM_STOW; + arm_goal_.command = ff_msgs::action::Arm::Goal::ARM_STOW; } else if (cmd->cmd_name == CommandConstants::CMD_NAME_STOP_ARM) { - arm_goal_.command = ff_msgs::ArmGoal::ARM_STOP; + arm_goal_.command = ff_msgs::action::Arm::Goal::ARM_STOP; } else { successful = false; err_msg = "Arm command not recognized in fill arm goal."; } if (!successful) { - NODELET_ERROR("%s", err_msg.c_str()); + FF_ERROR("%s", err_msg.c_str()); state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::BAD_SYNTAX, + ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX, err_msg); } return successful; } -bool Executive::FillDockGoal(ff_msgs::CommandStampedPtr const& cmd, +bool Executive::FillDockGoal(ff_msgs::msg::CommandStamped::SharedPtr const cmd, bool return_to_dock) { bool successful = true; std::string err_msg; @@ -609,33 +621,33 @@ bool Executive::FillDockGoal(ff_msgs::CommandStampedPtr const& cmd, if (cmd->cmd_name == CommandConstants::CMD_NAME_DOCK) { if (cmd->args.size() != 1 || - cmd->args[0].data_type != ff_msgs::CommandArg::DATA_TYPE_INT) { + cmd->args[0].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_INT) { successful = false; err_msg = "Malformed argument for dock command in plan."; } - dock_goal_.command = ff_msgs::DockGoal::DOCK; + dock_goal_.command = ff_msgs::action::Dock::Goal::DOCK; if (cmd->args[0].i == 1) { - dock_goal_.berth = ff_msgs::DockGoal::BERTH_1; + dock_goal_.berth = ff_msgs::action::Dock::Goal::BERTH_1; } else if (cmd->args[0].i == 2) { - dock_goal_.berth = ff_msgs::DockGoal::BERTH_2; + dock_goal_.berth = ff_msgs::action::Dock::Goal::BERTH_2; } else { successful = false; err_msg = "Berth must be 1 or 2 not " + std::to_string(cmd->args[0].i); } } else if (cmd->cmd_name == CommandConstants::CMD_NAME_UNDOCK) { - dock_goal_.command = ff_msgs::DockGoal::UNDOCK; + dock_goal_.command = ff_msgs::action::Dock::Goal::UNDOCK; // We don't need a berth to undock - dock_goal_.berth = ff_msgs::DockGoal::BERTH_UNKNOWN; + dock_goal_.berth = ff_msgs::action::Dock::Goal::BERTH_UNKNOWN; } else { successful = false; err_msg = "Dock command not recognized in fill dock goal."; } if (!successful) { - NODELET_ERROR("%s", err_msg.c_str()); + FF_ERROR("%s", err_msg.c_str()); state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::BAD_SYNTAX, + ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX, err_msg); } @@ -643,7 +655,7 @@ bool Executive::FillDockGoal(ff_msgs::CommandStampedPtr const& cmd, } bool Executive::FillMotionGoal(Action action, - ff_msgs::CommandStampedPtr const& cmd) { + ff_msgs::msg::CommandStamped::SharedPtr const cmd) { jsonloader::Segment segment; // Flight mode needs to be set for all motion actions motion_goal_.flight_mode = agent_state_.flight_mode; @@ -652,7 +664,7 @@ bool Executive::FillMotionGoal(Action action, case EXECUTE: segment = sequencer_.CurrentSegment(); - motion_goal_.command = ff_msgs::MotionGoal::EXEC; + motion_goal_.command = ff_msgs::action::Motion::Goal::EXEC; // Convert JSON to a segment type motion_goal_.segment = sequencer::Segment2Trajectory(sequencer_.CurrentSegment()); @@ -661,35 +673,35 @@ bool Executive::FillMotionGoal(Action action, break; case IDLE: // Need to set flight mode to off so the PMCs shutdown - motion_goal_.flight_mode = ff_msgs::MotionGoal::OFF; - motion_goal_.command = ff_msgs::MotionGoal::IDLE; + motion_goal_.flight_mode = ff_msgs::action::Motion::Goal::OFF; + motion_goal_.command = ff_msgs::action::Motion::Goal::IDLE; break; case STOP: - motion_goal_.command = ff_msgs::MotionGoal::STOP; + motion_goal_.command = ff_msgs::action::Motion::Goal::STOP; break; case MOVE: if (cmd == nullptr) { - NODELET_ERROR("Executive: move cmd is null in fill motion goal."); + FF_ERROR("Executive: move cmd is null in fill motion goal."); return false; } if (cmd->args.size() != 4 || - cmd->args[0].data_type != ff_msgs::CommandArg::DATA_TYPE_STRING || - cmd->args[1].data_type != ff_msgs::CommandArg::DATA_TYPE_VEC3d || - cmd->args[2].data_type != ff_msgs::CommandArg::DATA_TYPE_VEC3d || - cmd->args[3].data_type != ff_msgs::CommandArg::DATA_TYPE_MAT33f) { + cmd->args[0].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_STRING || + cmd->args[1].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_VEC3D || + cmd->args[2].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_VEC3D || + cmd->args[3].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_MAT33F) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::BAD_SYNTAX, + ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX, "Malformed arguments for simple move 6dof command!"); return false; } - motion_goal_.command = ff_msgs::MotionGoal::MOVE; + motion_goal_.command = ff_msgs::action::Motion::Goal::MOVE; if (motion_goal_.states.size() != 1) { motion_goal_.states.resize(1); } - motion_goal_.states[0].header.stamp = ros::Time::now(); + motion_goal_.states[0].header.stamp = GetTimeNow(); // Copying the reference frame to the goal if (cmd->args[0].s == "ISS") { @@ -714,7 +726,7 @@ bool Executive::FillMotionGoal(Action action, default: if (cmd != nullptr) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, "Command isn't a mobility action in fill motion goal!"); } return false; @@ -740,7 +752,7 @@ bool Executive::StartAction(Action action, std::string const& cmd_id) { if (arm_ac_.IsConnected()) { arm_ac_.SetCmdInfo(action, cmd_id); arm_ac_.SendGoal(arm_goal_); - NODELET_DEBUG("Arm action goal sent/started."); + FF_DEBUG("Arm action goal sent/started."); } else { successful = false; err_msg = "Arm action server not connected! Arm node may have died!"; @@ -751,7 +763,7 @@ bool Executive::StartAction(Action action, std::string const& cmd_id) { if (dock_ac_.IsConnected()) { dock_ac_.SetCmdInfo(action, cmd_id); dock_ac_.SendGoal(dock_goal_); - NODELET_DEBUG("Dock action goal sent/started."); + FF_DEBUG("Dock action goal sent/started."); } else { successful = false; err_msg = "Dock action server not connected! Dock node may have died!"; @@ -764,8 +776,7 @@ bool Executive::StartAction(Action action, std::string const& cmd_id) { if (motion_ac_.IsConnected()) { motion_ac_.SetCmdInfo(action, cmd_id); motion_ac_.SendGoal(motion_goal_); - NODELET_DEBUG("Motion action %i goal sent/started.", - motion_goal_.command); + FF_DEBUG("Motion action %i goal sent/started.", motion_goal_.command); } else { successful = false; err_msg = "Motion action server not connected! Node may have died!"; @@ -776,7 +787,7 @@ bool Executive::StartAction(Action action, std::string const& cmd_id) { if (localization_ac_.IsConnected()) { localization_ac_.SetCmdInfo(action, cmd_id); localization_ac_.SendGoal(localization_goal_); - NODELET_DEBUG("Localization action goal sent/started."); + FF_DEBUG("Localization action goal sent/started."); } else { successful = false; err_msg = "Localization action server not connected. Node may be dead"; @@ -787,7 +798,7 @@ bool Executive::StartAction(Action action, std::string const& cmd_id) { if (perch_ac_.IsConnected()) { perch_ac_.SetCmdInfo(action, cmd_id); perch_ac_.SendGoal(perch_goal_); - NODELET_DEBUG("Perch action goal sent/started."); + FF_DEBUG("Perch action goal sent/started."); } else { successful = false; err_msg = "Perch action server not connected. Node may have died!"; @@ -799,12 +810,14 @@ bool Executive::StartAction(Action action, std::string const& cmd_id) { } if (!successful) { - state_->AckCmd(cmd_id, ff_msgs::AckCompletedStatus::EXEC_FAILED, err_msg); + state_->AckCmd(cmd_id, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, + err_msg); } else { state_->AckCmd(cmd_id, - ff_msgs::AckCompletedStatus::NOT, + ff_msgs::msg::AckCompletedStatus::NOT, "", - ff_msgs::AckStatus::EXECUTING); + ff_msgs::msg::AckStatus::EXECUTING); } if (successful) { @@ -829,16 +842,15 @@ bool Executive::RemoveAction(Action action) { if (found) { running_actions_.erase(running_actions_.begin() + i); } else { - NODELET_ERROR_STREAM("Action " << action << - " not in running actions vector!"); + FF_ERROR_STREAM("Action " << action << " not in running actions vector!"); } return found; } /************************ Action callbacks ************************************/ void Executive::ArmResultCallback( - ff_util::FreeFlyerActionState::Enum const& state, - ff_msgs::ArmResultConstPtr const& result) { + ff_util::FreeFlyerActionState::Enum const& state, + std::shared_ptr result) { std::string response = ""; Action current_action = arm_ac_.action(); std::string cmd_id = arm_ac_.cmd_id(); @@ -863,8 +875,8 @@ void Executive::ArmResultCallback( } void Executive::DockResultCallback( - ff_util::FreeFlyerActionState::Enum const& state, - ff_msgs::DockResultConstPtr const& result) { + ff_util::FreeFlyerActionState::Enum const& state, + std::shared_ptr result) { std::string response = ""; Action current_action = dock_ac_.action(); std::string cmd_id = dock_ac_.cmd_id(); @@ -889,8 +901,8 @@ void Executive::DockResultCallback( } void Executive::LocalizationResultCallback( - ff_util::FreeFlyerActionState::Enum const& state, - ff_msgs::LocalizationResultConstPtr const& result) { + ff_util::FreeFlyerActionState::Enum const& state, + std::shared_ptr result) { std::string response = ""; Action current_action = localization_ac_.action(); std::string cmd_id = localization_ac_.cmd_id(); @@ -919,7 +931,7 @@ void Executive::LocalizationResultCallback( } void Executive::MotionFeedbackCallback( - ff_msgs::MotionFeedbackConstPtr const& feedback) { + std::shared_ptr feedback) { // The only feedback used from the motion action is the execute feedback and // it goes to the sequencer. Otherwise there isn't much to with the feedback if (motion_ac_.action() == EXECUTE) { @@ -928,8 +940,8 @@ void Executive::MotionFeedbackCallback( } void Executive::MotionResultCallback( - ff_util::FreeFlyerActionState::Enum const& state, - ff_msgs::MotionResultConstPtr const& result) { + ff_util::FreeFlyerActionState::Enum const& state, + std::shared_ptr result) { std::string response = ""; Action current_action = motion_ac_.action(); std::string cmd_id = motion_ac_.cmd_id(); @@ -954,8 +966,8 @@ void Executive::MotionResultCallback( } void Executive::PerchResultCallback( - ff_util::FreeFlyerActionState::Enum const& state, - ff_msgs::PerchResultConstPtr const& result) { + ff_util::FreeFlyerActionState::Enum const& state, + std::shared_ptr result) { std::string response = ""; Action current_action = perch_ac_.action(); std::string cmd_id = perch_ac_.cmd_id(); @@ -985,36 +997,35 @@ void Executive::PublishCmdAck(std::string const& cmd_id, std::string const& message, uint8_t status) { // Output if the command failed - if (completed_status != ff_msgs::AckCompletedStatus::OK && - completed_status != ff_msgs::AckCompletedStatus::NOT && - completed_status != ff_msgs::AckCompletedStatus::CANCELED) { - NODELET_ERROR("Executive: Command failed with message: %s", - message.c_str()); + if (completed_status != ff_msgs::msg::AckCompletedStatus::OK && + completed_status != ff_msgs::msg::AckCompletedStatus::NOT && + completed_status != ff_msgs::msg::AckCompletedStatus::CANCELED) { + FF_ERROR("Executive: Command failed with message: %s", message.c_str()); } - ack_.header.stamp = ros::Time::now(); + ack_.header.stamp = GetTimeNow(); ack_.cmd_id = cmd_id; ack_.status.status = status; ack_.completed_status.status = completed_status; ack_.message = message; - cmd_ack_pub_.publish(ack_); + cmd_ack_pub_->publish(ack_); } void Executive::PublishPlan() { - plan_pub_.publish(plan_); + plan_pub_->publish(*plan_); } void Executive::PublishPlanStatus(uint8_t status) { // The sequencer sets the plan status to executing for every plan status but // since the executive has knowledge of if the plan has just started, // is paused, or is executing, the executive sets the status. - ff_msgs::PlanStatusStamped plan_status = sequencer_.plan_status(); - plan_status.header.stamp = ros::Time::now(); + ff_msgs::msg::PlanStatusStamped plan_status = sequencer_.plan_status(); + plan_status.header.stamp = GetTimeNow(); plan_status.status.status = status; - plan_status_pub_.publish(plan_status); + plan_status_pub_->publish(plan_status); } /************************ Getters *********************************************/ -ff_msgs::MobilityState Executive::GetMobilityState() { +ff_msgs::msg::MobilityState Executive::GetMobilityState() { return agent_state_.mobility_state; } @@ -1033,21 +1044,21 @@ void Executive::SetMobilityState() { return; } - if (motion_state_->state == ff_msgs::MotionState::IDLE) { - agent_state_.mobility_state.state = ff_msgs::MobilityState::DRIFTING; + if (motion_state_->state == ff_msgs::msg::MotionState::IDLE) { + agent_state_.mobility_state.state = ff_msgs::msg::MobilityState::DRIFTING; agent_state_.mobility_state.sub_state = 0; - } else if (motion_state_->state == ff_msgs::MotionState::IDLING) { - agent_state_.mobility_state.state = ff_msgs::MobilityState::DRIFTING; + } else if (motion_state_->state == ff_msgs::msg::MotionState::IDLING) { + agent_state_.mobility_state.state = ff_msgs::msg::MobilityState::DRIFTING; agent_state_.mobility_state.sub_state = 1; - } else if (motion_state_->state == ff_msgs::MotionState::STOPPED) { - agent_state_.mobility_state.state = ff_msgs::MobilityState::STOPPING; + } else if (motion_state_->state == ff_msgs::msg::MotionState::STOPPED) { + agent_state_.mobility_state.state = ff_msgs::msg::MobilityState::STOPPING; agent_state_.mobility_state.sub_state = 0; - } else if (motion_state_->state == ff_msgs::MotionState::STOPPING) { - agent_state_.mobility_state.state = ff_msgs::MobilityState::STOPPING; + } else if (motion_state_->state == ff_msgs::msg::MotionState::STOPPING) { + agent_state_.mobility_state.state = ff_msgs::msg::MobilityState::STOPPING; agent_state_.mobility_state.sub_state = 1; - } else if (motion_state_->state == ff_msgs::MotionState::CONTROLLING || - motion_state_->state == ff_msgs::MotionState::BOOTSTRAPPING) { - agent_state_.mobility_state.state = ff_msgs::MobilityState::FLYING; + } else if (motion_state_->state == ff_msgs::msg::MotionState::CONTROLLING || + motion_state_->state == ff_msgs::msg::MotionState::BOOTSTRAPPING) { + agent_state_.mobility_state.state = ff_msgs::msg::MobilityState::FLYING; agent_state_.mobility_state.sub_state = 0; } @@ -1062,7 +1073,7 @@ void Executive::SetMobilityState(uint8_t state, uint32_t sub_state) { void Executive::SetOpState(OpState* state) { if (state_->id() != state->id()) { - NODELET_INFO("Executive state changing from [%s(%i)] to [%s(%i)].", + FF_INFO("Executive state changing from [%s(%i)] to [%s(%i)].", state_->name().c_str(), state_->id(), state->name().c_str(), state->id()); agent_state_.operating_state.state = state->id(); @@ -1083,9 +1094,10 @@ void Executive::SetRunPlanCmdId(std::string cmd_id) { /************************ Helper functions ************************************/ // Used as a helper function to send a failed ack when the command is not // accepted in the current mobility state -void Executive::AckMobilityStateIssue(ff_msgs::CommandStampedPtr const& cmd, - std::string const& current_mobility_state, - std::string const& accepted_mobility_state) { +void Executive::AckMobilityStateIssue( + ff_msgs::msg::CommandStamped::SharedPtr const cmd, + std::string const& current_mobility_state, + std::string const& accepted_mobility_state) { std::string err_msg = cmd->cmd_name + " not accepted while " + current_mobility_state + "!"; if (accepted_mobility_state != "") { @@ -1093,22 +1105,23 @@ void Executive::AckMobilityStateIssue(ff_msgs::CommandStampedPtr const& cmd, } state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, err_msg); } -bool Executive::ArmControl(ff_msgs::CommandStampedPtr const& cmd) { +bool Executive::ArmControl(ff_msgs::msg::CommandStamped::SharedPtr const cmd) { // Check to make sure we aren't trying to dock or perch - if (agent_state_.mobility_state.state == ff_msgs::MobilityState::DOCKING) { + if (agent_state_.mobility_state.state == + ff_msgs::msg::MobilityState::DOCKING) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, "Cannot move arm while (un)docking or docked!"); return false; } else if (agent_state_.mobility_state.state == - ff_msgs::MobilityState::PERCHING && + ff_msgs::msg::MobilityState::PERCHING && agent_state_.mobility_state.sub_state != 0) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, "Cannot move arm while (un)perching!"); return false; } @@ -1116,7 +1129,7 @@ bool Executive::ArmControl(ff_msgs::CommandStampedPtr const& cmd) { // Check to make sure another arm command isn't being executed if (IsActionRunning(ARM)) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, "Arm command already being executed!"); return false; } @@ -1125,20 +1138,22 @@ bool Executive::ArmControl(ff_msgs::CommandStampedPtr const& cmd) { return false; } - NODELET_INFO("Executing arm command."); + FF_INFO("Executing arm command."); if (!StartAction(ARM, cmd->cmd_id)) { return false; } return true; } -bool Executive::CheckServiceExists(ros::ServiceClient& serviceIn, +bool Executive::CheckServiceExists(bool serviceExists, std::string const& serviceName, std::string const& cmd_id) { std::string err_msg = ""; - if (!serviceIn.exists()) { + if (!serviceExists) { err_msg = serviceName + " service isn't running! Node may have died!"; - state_->AckCmd(cmd_id, ff_msgs::AckCompletedStatus::EXEC_FAILED, err_msg); + state_->AckCmd(cmd_id, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, + err_msg); return false; } return true; @@ -1146,27 +1161,30 @@ bool Executive::CheckServiceExists(ros::ServiceClient& serviceIn, bool Executive::CheckStoppedOrDrifting(std::string const& cmd_id, std::string const& cmd_name) { - if ((agent_state_.mobility_state.state == ff_msgs::MobilityState::STOPPING && + if ((agent_state_.mobility_state.state == + ff_msgs::msg::MobilityState::STOPPING && agent_state_.mobility_state.sub_state == 0) || - agent_state_.mobility_state.state == ff_msgs::MobilityState::DRIFTING) { + agent_state_.mobility_state.state == + ff_msgs::msg::MobilityState::DRIFTING) { return true; } state_->AckCmd(cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, ("Must be stopped or drifting before " + cmd_name + ".")); return false; } -bool Executive::ConfigureLed(ff_hw_msgs::ConfigureSystemLeds& led_srv) { +bool Executive::ConfigureLed( + ff_util::FreeFlyerService& led_srv) { if (!led_client_.Call(led_srv)) { - NODELET_ERROR("Configure system leds service not running!"); + FF_ERROR("Configure system leds service not running!"); return false; } - if (!led_srv.response.success) { - NODELET_ERROR("Configure system leds failed with message %s.", - led_srv.response.status.c_str()); + if (!led_srv.response->success) { + FF_ERROR("Configure system leds failed with message %s.", + led_srv.response->status.c_str()); return false; } @@ -1178,8 +1196,10 @@ bool Executive::ConfigureLed(ff_hw_msgs::ConfigureSystemLeds& led_srv) { bool Executive::ConfigureMobility(bool move_to_start, std::string& err_msg) { // Initialize choreographer config client if it hasn't been initialized if (!choreographer_cfg_) { + std::string platform = this->GetPlatform(); + cfg_node_ = std::make_shared("executive_cfg_node", platform, rclcpp::NodeOptions().use_global_arguments(false)); choreographer_cfg_ = - std::make_shared(&nh_, NODE_CHOREOGRAPHER); + std::make_shared(cfg_node_, NODE_CHOREOGRAPHER); } // Set values for configuring, these values will persist until changed @@ -1213,8 +1233,8 @@ bool Executive::ConfigureMobility(bool move_to_start, std::string& err_msg) { } // Set the collision distance in the mapper - ff_msgs::SetFloat collision_distance_srv; - collision_distance_srv.request.data = agent_state_.collision_distance; + ff_util::FreeFlyerService collision_distance_srv; + collision_distance_srv.request->data = agent_state_.collision_distance; // Check to make sure the service is valid and running // Don't use the check service exists function since we don't want to @@ -1230,7 +1250,7 @@ bool Executive::ConfigureMobility(bool move_to_start, std::string& err_msg) { return false; } - if (!collision_distance_srv.response.success) { + if (!collision_distance_srv.response->success) { err_msg = "Set collision distance service was not successful."; return false; } @@ -1241,11 +1261,13 @@ bool Executive::ConfigureMobility(bool move_to_start, std::string& err_msg) { // Used to check the mobility state for commands that can only be executed when // the astrobee is in some sort of stopped state. Send a failed execution ack // and return false if mobility state is flying, docking, perching, or stopping. -bool Executive::FailCommandIfMoving(ff_msgs::CommandStampedPtr const& cmd) { - if (agent_state_.mobility_state.state == ff_msgs::MobilityState::FLYING) { +bool Executive::FailCommandIfMoving( + ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + if (agent_state_.mobility_state.state == + ff_msgs::msg::MobilityState::FLYING) { AckMobilityStateIssue(cmd, "flying"); } else if (agent_state_.mobility_state.state == - ff_msgs::MobilityState::DOCKING && + ff_msgs::msg::MobilityState::DOCKING && agent_state_.mobility_state.sub_state != 0) { // Check if astrobee is docking vs. undocking if (agent_state_.mobility_state.sub_state > 0) { @@ -1254,7 +1276,7 @@ bool Executive::FailCommandIfMoving(ff_msgs::CommandStampedPtr const& cmd) { AckMobilityStateIssue(cmd, "undocking", "stopped"); } } else if (agent_state_.mobility_state.state == - ff_msgs::MobilityState::PERCHING && + ff_msgs::msg::MobilityState::PERCHING && agent_state_.mobility_state.sub_state != 0) { // Check if astrobee is perching vs. unperching if (agent_state_.mobility_state.sub_state > 0) { @@ -1263,7 +1285,7 @@ bool Executive::FailCommandIfMoving(ff_msgs::CommandStampedPtr const& cmd) { AckMobilityStateIssue(cmd, "unperching", "stopped"); } } else if (agent_state_.mobility_state.state == - ff_msgs::MobilityState::STOPPING && + ff_msgs::msg::MobilityState::STOPPING && agent_state_.mobility_state.sub_state == 1) { AckMobilityStateIssue(cmd, "stopping", "stopped"); } else { @@ -1272,7 +1294,8 @@ bool Executive::FailCommandIfMoving(ff_msgs::CommandStampedPtr const& cmd) { return false; } -bool Executive::LoadUnloadNodelet(ff_msgs::CommandStampedPtr const& cmd) { +bool Executive::LoadUnloadNodelet( + ff_msgs::msg::CommandStamped::SharedPtr const cmd) { bool load = true; std::string which = "Load"; int num_args = cmd->args.size(); @@ -1282,15 +1305,16 @@ bool Executive::LoadUnloadNodelet(ff_msgs::CommandStampedPtr const& cmd) { which = "Unload"; } - ff_msgs::UnloadLoadNodelet unload_load_nodelet_srv; - unload_load_nodelet_srv.request.load = load; + ff_util::FreeFlyerService + unload_load_nodelet_srv; + unload_load_nodelet_srv.request->load = load; // Don't load/unload a nodelet while moving if (FailCommandIfMoving(cmd)) { // Only one argument is required for load/unload nodelet, the nodelet name if (num_args < 1) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::BAD_SYNTAX, + ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX, (which + " nodelet must have one argument.")); return false; } @@ -1298,7 +1322,7 @@ bool Executive::LoadUnloadNodelet(ff_msgs::CommandStampedPtr const& cmd) { // The unload nodelet command takes only 1 or 2 arguments if (num_args > 2 && !load) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::BAD_SYNTAX, + ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX, (which + " nodelet takes no more than two arguments.")); return false; } @@ -1306,33 +1330,34 @@ bool Executive::LoadUnloadNodelet(ff_msgs::CommandStampedPtr const& cmd) { // The load nodelet command takes 1 or up to 4 arguments if (num_args > 4 && load) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::BAD_SYNTAX, + ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX, (which + " nodelet takes no more than four arguments.")); return false; } // Extract arguments for (int i = 0; i < num_args; i++) { - if (cmd->args[i].data_type != ff_msgs::CommandArg::DATA_TYPE_STRING) { + if (cmd->args[i].data_type != + ff_msgs::msg::CommandArg::DATA_TYPE_STRING) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::BAD_SYNTAX, + ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX, (which + " nodelet parameters must be strings.")); return false; } if (i == 0) { - unload_load_nodelet_srv.request.name = cmd->args[0].s; + unload_load_nodelet_srv.request->name = cmd->args[0].s; } else if (i == 1) { - unload_load_nodelet_srv.request.manager_name = cmd->args[1].s; + unload_load_nodelet_srv.request->manager_name = cmd->args[1].s; } else if (i == 2) { - unload_load_nodelet_srv.request.type = cmd->args[2].s; + unload_load_nodelet_srv.request->type = cmd->args[2].s; } else { - unload_load_nodelet_srv.request.bond_id = cmd->args[3].s; + unload_load_nodelet_srv.request->bond_id = cmd->args[3].s; } } // Check if the load/unload nodelet service is running - if (!CheckServiceExists(unload_load_nodelet_client_, + if (!CheckServiceExists(unload_load_nodelet_client_.exists(), "Load/unload nodelet", cmd->cmd_id)) { return false; @@ -1341,17 +1366,17 @@ bool Executive::LoadUnloadNodelet(ff_msgs::CommandStampedPtr const& cmd) { // Call the unload load nodelet service if (!unload_load_nodelet_client_.call(unload_load_nodelet_srv)) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, "Unload load nodelet service returned false."); return false; } - if (unload_load_nodelet_srv.response.result != - ff_msgs::UnloadLoadNodelet::Response::SUCCESSFUL) { + if (unload_load_nodelet_srv.response->result != + ff_msgs::srv::UnloadLoadNodelet::Response::SUCCESSFUL) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, (which + " nodelet failed with result " + - std::to_string(unload_load_nodelet_srv.response.result))); + std::to_string(unload_load_nodelet_srv.response->result))); return false; } @@ -1362,43 +1387,44 @@ bool Executive::LoadUnloadNodelet(ff_msgs::CommandStampedPtr const& cmd) { return false; } -ros::Time Executive::MsToSec(std::string timestamp) { +rclcpp::Time Executive::MsToSec(std::string timestamp) { uint64_t time, secs, nsecs; time = std::stoull(timestamp); secs = time/1000; nsecs = (time % 1000) * 1000000; - return ros::Time(secs, nsecs); + return rclcpp::Time(secs, nsecs); } -bool Executive::PowerItem(ff_msgs::CommandStampedPtr const& cmd, bool on) { - uint8_t completed_status = ff_msgs::AckCompletedStatus::OK; +bool Executive::PowerItem(ff_msgs::msg::CommandStamped::SharedPtr const cmd, + bool on) { + uint8_t completed_status = ff_msgs::msg::AckCompletedStatus::OK; std::string err_msg = ""; bool success; // Check to make sure command is formatted as expected if (cmd->args.size() != 1 || - cmd->args[0].data_type != ff_msgs::CommandArg::DATA_TYPE_STRING) { + cmd->args[0].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_STRING) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::BAD_SYNTAX, + ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX, "Malformed arguments for power item command!"); return false; } - NODELET_INFO("Item %s is being powered on/off!", cmd->args[0].s.c_str()); + FF_INFO("Item %s is being powered on/off!", cmd->args[0].s.c_str()); // Handle pmcs and laser the same since they use the same service message if (cmd->args[0].s == CommandConstants::PARAM_NAME_POWERED_COMPONENT_LASER_POINTER || cmd->args[0].s == CommandConstants::PARAM_NAME_POWERED_COMPONENT_PMCS_AND_SIGNAL_LIGHTS) { - ff_hw_msgs::SetEnabled enable_srv; - enable_srv.request.enabled = on; + ff_util::FreeFlyerService enable_srv; + enable_srv.request->enabled = on; if (cmd->args[0].s == CommandConstants::PARAM_NAME_POWERED_COMPONENT_LASER_POINTER) { // Check to make sure the laser service is valid and running - if (!CheckServiceExists(laser_enable_client_, + if (!CheckServiceExists(laser_enable_client_.exists(), "Enable laser", cmd->cmd_id)) { return false; @@ -1406,7 +1432,9 @@ bool Executive::PowerItem(ff_msgs::CommandStampedPtr const& cmd, bool on) { success = laser_enable_client_.call(enable_srv); } else { // PMCS // Check to make sure the pmc service is valid and running - if (!CheckServiceExists(pmc_enable_client_, "Enable PMC", cmd->cmd_id)) { + if (!CheckServiceExists(pmc_enable_client_.exists(), + "Enable PMC", + cmd->cmd_id)) { return false; } success = pmc_enable_client_.call(enable_srv); @@ -1415,43 +1443,44 @@ bool Executive::PowerItem(ff_msgs::CommandStampedPtr const& cmd, bool on) { // Check to see if the service was successfully enabled/disabled if (!success) { err_msg = "Service returned false."; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; - } else if (!enable_srv.response.success) { - err_msg = enable_srv.response.status_message; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; + } else if (!enable_srv.response->success) { + err_msg = enable_srv.response->status_message; + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; } } else { // Item is probably a payload - ff_hw_msgs::ConfigurePayloadPower config_srv; - config_srv.request.top_front = config_srv.request.PERSIST; - config_srv.request.bottom_front = config_srv.request.PERSIST; - config_srv.request.top_aft = config_srv.request.PERSIST; - config_srv.request.bottom_aft = config_srv.request.PERSIST; + ff_util::FreeFlyerService + config_srv; + config_srv.request->top_front = config_srv.request->PERSIST; + config_srv.request->bottom_front = config_srv.request->PERSIST; + config_srv.request->top_aft = config_srv.request->PERSIST; + config_srv.request->bottom_aft = config_srv.request->PERSIST; uint8_t power; if (on) { - power = ff_hw_msgs::ConfigurePayloadPower::Request::ON; + power = ff_hw_msgs::srv::ConfigurePayloadPower::Request::ON; } else { - power = ff_hw_msgs::ConfigurePayloadPower::Request::OFF; + power = ff_hw_msgs::srv::ConfigurePayloadPower::Request::OFF; } if (cmd->args[0].s == CommandConstants::PARAM_NAME_POWERED_COMPONENT_PAYLOAD_TOP_AFT) { - config_srv.request.top_aft = power; + config_srv.request->top_aft = power; } else if (cmd->args[0].s == CommandConstants::PARAM_NAME_POWERED_COMPONENT_PAYLOAD_BOTTOM_AFT) { - config_srv.request.bottom_aft = power; + config_srv.request->bottom_aft = power; } else if (cmd->args[0].s == CommandConstants::PARAM_NAME_POWERED_COMPONENT_PAYLOAD_BOTTOM_FRONT) { - config_srv.request.bottom_front = power; + config_srv.request->bottom_front = power; } else { // Item wasn't recognized err_msg = "Item " + cmd->args[0].s + " not recognized in power item."; state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, err_msg); return false; } - if (!CheckServiceExists(payload_power_client_, + if (!CheckServiceExists(payload_power_client_.exists(), "Power payload", cmd->cmd_id)) { return false; @@ -1462,10 +1491,10 @@ bool Executive::PowerItem(ff_msgs::CommandStampedPtr const& cmd, bool on) { // Check to see if the payload was successfully enabled/disabled if (!success) { err_msg = "Power payload service returned false."; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; - } else if (!config_srv.response.success) { - err_msg = config_srv.response.status; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; + } else if (!config_srv.response->success) { + err_msg = config_srv.response->status; + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; } } @@ -1473,20 +1502,20 @@ bool Executive::PowerItem(ff_msgs::CommandStampedPtr const& cmd, bool on) { return true; } -bool Executive::ProcessGuestScienceCommand(ff_msgs::CommandStampedPtr - const& cmd) { +bool Executive::ProcessGuestScienceCommand( + ff_msgs::msg::CommandStamped::SharedPtr const cmd) { int gs_command_timeout_mod = gs_command_timeout_; // Check if command is restart. If so, need to extract time to add to the // timeout if (cmd->cmd_name == - ff_msgs::CommandConstants::CMD_NAME_RESTART_GUEST_SCIENCE) { + ff_msgs::msg::CommandConstants::CMD_NAME_RESTART_GUEST_SCIENCE) { // Check command arguments are correct before sending to the guest science // manager if (cmd->args.size() != 2 || - cmd->args[0].data_type != ff_msgs::CommandArg::DATA_TYPE_STRING || - cmd->args[1].data_type != ff_msgs::CommandArg::DATA_TYPE_INT) { + cmd->args[0].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_STRING || + cmd->args[1].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_INT) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::BAD_SYNTAX, + ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX, "Malformed arguments for restart guest science command."); return false; } @@ -1496,9 +1525,9 @@ bool Executive::ProcessGuestScienceCommand(ff_msgs::CommandStampedPtr // Check command arguments are correct before sending to the guest science // manager if (cmd->args.size() != 1 || - cmd->args[0].data_type != ff_msgs::CommandArg::DATA_TYPE_STRING) { + cmd->args[0].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_STRING) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::BAD_SYNTAX, + ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX, "Malformed arguments for guest science command."); return false; } @@ -1508,19 +1537,21 @@ bool Executive::ProcessGuestScienceCommand(ff_msgs::CommandStampedPtr std::string msg = "Already executing a start or stop guest science command"; msg += ". Please wait for it to finish before issuing another start guest "; msg += "science command."; - state_->AckCmd(cmd->cmd_id, ff_msgs::AckCompletedStatus::EXEC_FAILED, msg); + state_->AckCmd(cmd->cmd_id, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, + msg); return false; } // If starting an apk, check to see if it is primary. If it is, make sure // a primary apk isn't already running. if (cmd->cmd_name == - ff_msgs::CommandConstants::CMD_NAME_START_GUEST_SCIENCE) { + ff_msgs::msg::CommandConstants::CMD_NAME_START_GUEST_SCIENCE) { // Make sure the executive has received the guest science config message. // This is needed to check if the apk is primary. if (guest_science_config_ == NULL) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, "Executive never got GS config. GS manager may have died"); return false; } @@ -1531,7 +1562,7 @@ bool Executive::ProcessGuestScienceCommand(ff_msgs::CommandStampedPtr // Cannot start a primary apk if another primary apk is running. if (primary_apk_running_ != "None") { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, "Can't start primary apk when one is already running"); return false; } @@ -1541,24 +1572,24 @@ bool Executive::ProcessGuestScienceCommand(ff_msgs::CommandStampedPtr } } - gs_cmd_pub_.publish(cmd); - gs_start_stop_restart_command_timer_.setPeriod( - ros::Duration(gs_command_timeout_mod)); + gs_cmd_pub_->publish(*cmd); + gs_start_stop_restart_command_timer_.setPeriod(gs_command_timeout_mod); gs_start_stop_restart_command_timer_.start(); gs_start_stop_restart_cmd_id_ = cmd->cmd_id; return true; } bool Executive::ResetEkf(std::string const& cmd_id) { - localization_goal_.command = ff_msgs::LocalizationGoal::COMMAND_RESET_FILTER; + localization_goal_.command = + ff_msgs::action::Localization::Goal::COMMAND_RESET_FILTER; // Don't need to specify a pipeline for reset but clear it just in case localization_goal_.pipeline = ""; return StartAction(LOCALIZATION, cmd_id); } -void Executive::StartWaitTimer(float duration) { - wait_timer_.setPeriod(ros::Duration(duration)); +void Executive::StartWaitTimer(double duration) { + wait_timer_.setPeriod(duration); wait_timer_.start(); } @@ -1566,11 +1597,27 @@ void Executive::StopWaitTimer() { wait_timer_.stop(); } +/****************************** Output functions ******************************/ +void Executive::Debug(std::string output) { + FF_DEBUG_STREAM(output); +} +void Executive::Error(std::string output) { + FF_ERROR_STREAM(output); +} + +void Executive::Info(std::string output) { + FF_INFO_STREAM(output); +} + +void Executive::Warn(std::string output) { + FF_WARN_STREAM(output); +} + /************************ Plan related functions ******************************/ // Returns false if there are no more command/segments in the plan bool Executive::AckCurrentPlanItem() { - ff_msgs::AckCompletedStatus ack; - ack.status = ff_msgs::AckCompletedStatus::OK; + ff_msgs::msg::AckCompletedStatus ack; + ack.status = ff_msgs::msg::AckCompletedStatus::OK; return sequencer_.Feedback(ack); } @@ -1578,33 +1625,35 @@ sequencer::ItemType Executive::GetCurrentPlanItemType() { return sequencer_.CurrentType(); } -ff_msgs::CommandStampedPtr Executive::GetPlanCommand() { +ff_msgs::msg::CommandStamped::SharedPtr Executive::GetPlanCommand() { return sequencer_.CurrentCommand(); } bool Executive::GetSetPlanInertia(std::string const& cmd_id) { // If the plan has inertia, set it. Otherwise, leave the inertia the way it is if (sequencer_.HaveInertia()) { - ff_msgs::SetInertia inertia_srv; - inertia_srv.request.inertia = sequencer_.GetInertia(); + ff_util::FreeFlyerService inertia_srv; + inertia_srv.request->inertia = sequencer_.GetInertia(); // Plan header doesn't contain the center of mass so we need to get the // current center of mass - inertia_srv.request.inertia.inertia.com = current_inertia_->inertia.com; + inertia_srv.request->inertia.inertia.com = current_inertia_->inertia.com; - if (!CheckServiceExists(set_inertia_client_, "Set inertia", cmd_id)) { + if (!CheckServiceExists(set_inertia_client_.exists(), + "Set inertia", + cmd_id)) { return false; } if (!set_inertia_client_.call(inertia_srv)) { state_->AckCmd(cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, "Set inertia service returned false for plan inertia."); return false; } - if (!inertia_srv.response.success) { + if (!inertia_srv.response->success) { state_->AckCmd(cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, "Set inertia srv returned unsuccessful for plan inertia"); return false; } @@ -1623,21 +1672,23 @@ void Executive::GetSetPlanOperatingLimits() { } /************************ Commands ********************************************/ -bool Executive::ArmPanAndTilt(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing arm pan and tilt command!"); +bool Executive::ArmPanAndTilt( + ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing arm pan and tilt command!"); return ArmControl(cmd); } -bool Executive::AutoReturn(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing auto return command!"); +bool Executive::AutoReturn(ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing auto return command!"); bool successful = false; std::string err_msg; // Astrobee needs to be either stopped or drifting - if (agent_state_.mobility_state.state == ff_msgs::MobilityState::DOCKING) { + if (agent_state_.mobility_state.state == + ff_msgs::msg::MobilityState::DOCKING) { err_msg = "Already docked!"; } else if (agent_state_.mobility_state.state == - ff_msgs::MobilityState::PERCHING) { + ff_msgs::msg::MobilityState::PERCHING) { err_msg = "Astrobee cannot attempt to dock while it is perched(ing)."; } else { // TODO(Katie) Currently this is just the dock 1 command with return to dock @@ -1645,7 +1696,7 @@ bool Executive::AutoReturn(ff_msgs::CommandStampedPtr const& cmd) { successful = true; cmd->cmd_name = "dock"; cmd->args.resize(1); - cmd->args[0].data_type = ff_msgs::CommandArg::DATA_TYPE_INT; + cmd->args[0].data_type = ff_msgs::msg::CommandArg::DATA_TYPE_INT; cmd->args[0].i = 1; if (!FillDockGoal(cmd, true)) { return false; @@ -1659,21 +1710,22 @@ bool Executive::AutoReturn(ff_msgs::CommandStampedPtr const& cmd) { // Ack command as failed if we are already docked or perched if (!successful) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, err_msg); } return successful; } -bool Executive::CustomGuestScience(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing custom guest science command!"); +bool Executive::CustomGuestScience( + ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing custom guest science command!"); // Check command arguments are correcy before sending to the guest science // manager if (cmd->args.size() != 2 || - cmd->args[0].data_type != ff_msgs::CommandArg::DATA_TYPE_STRING || - cmd->args[1].data_type != ff_msgs::CommandArg::DATA_TYPE_STRING) { + cmd->args[0].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_STRING || + cmd->args[1].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_STRING) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::BAD_SYNTAX, + ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX, "Malformed arguments for custom guest science command."); return false; } @@ -1681,23 +1733,26 @@ bool Executive::CustomGuestScience(ff_msgs::CommandStampedPtr const& cmd) { if (gs_custom_cmd_id_ != "") { std::string msg = "Already executing a custom gs command. Please wait for "; msg += "it to finish before issuing another custom gs command."; - state_->AckCmd(cmd->cmd_id, ff_msgs::AckCompletedStatus::EXEC_FAILED, msg); + state_->AckCmd(cmd->cmd_id, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, + msg); return false; } - gs_cmd_pub_.publish(cmd); - gs_custom_command_timer_.setPeriod(ros::Duration(gs_command_timeout_)); + gs_cmd_pub_->publish(*cmd); + gs_custom_command_timer_.setPeriod(gs_command_timeout_); gs_custom_command_timer_.start(); gs_custom_cmd_id_ = cmd->cmd_id; return true; } -bool Executive::DeployArm(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing deploy arm command!"); +bool Executive::DeployArm(ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing deploy arm command!"); // Check if Astrobee is perching/perched. Arm control will check the rest. - if (agent_state_.mobility_state.state == ff_msgs::MobilityState::PERCHING) { + if (agent_state_.mobility_state.state == + ff_msgs::msg::MobilityState::PERCHING) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, "Can't deploy arm while perched or (un)perching!"); return false; } @@ -1705,18 +1760,19 @@ bool Executive::DeployArm(ff_msgs::CommandStampedPtr const& cmd) { return ArmControl(cmd); } -bool Executive::Dock(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing dock command!"); +bool Executive::Dock(ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing dock command!"); bool successful = false; std::string err_msg; // Make sure robot is stopped before attempting to dock. Only accept dock in // ready op state so perched and docked are the only mobility states we need // to check for - if (agent_state_.mobility_state.state == ff_msgs::MobilityState::DOCKING) { + if (agent_state_.mobility_state.state == + ff_msgs::msg::MobilityState::DOCKING) { err_msg = "Already docked."; } else if (agent_state_.mobility_state.state == - ff_msgs::MobilityState::PERCHING) { + ff_msgs::msg::MobilityState::PERCHING) { err_msg = "Astrobee cannot attempt to dock while it is perched."; } else { successful = true; @@ -1733,19 +1789,20 @@ bool Executive::Dock(ff_msgs::CommandStampedPtr const& cmd) { // need to fail an ack if we aren't stopped and thus cannot dock if (!successful) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, err_msg); } return successful; } -bool Executive::EnableAstrobeeIntercomms(ff_msgs::CommandStampedPtr const& - cmd) { - NODELET_INFO("Executive executing enable astrobee intercomms command!"); +bool Executive::EnableAstrobeeIntercomms( + ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing enable astrobee intercomms command!"); - ff_msgs::ResponseOnly enable_astrobee_intercomms_srv; + ff_util::FreeFlyerService + enable_astrobee_intercomms_srv; - if (!CheckServiceExists(enable_astrobee_intercommunication_client_, + if (!CheckServiceExists(enable_astrobee_intercommunication_client_.exists(), "Enable astrobee intercommunication", cmd->cmd_id)) { return false; @@ -1754,16 +1811,16 @@ bool Executive::EnableAstrobeeIntercomms(ff_msgs::CommandStampedPtr const& if (!enable_astrobee_intercommunication_client_.call( enable_astrobee_intercomms_srv)) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, "Enable astrobee intercommunication service returned false"); return false; } - if (!enable_astrobee_intercomms_srv.response.success) { + if (!enable_astrobee_intercomms_srv.response->success) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, ("Enable astrobee intercommunication failed with result: " + - enable_astrobee_intercomms_srv.response.status)); + enable_astrobee_intercomms_srv.response->status)); return false; } @@ -1771,8 +1828,8 @@ bool Executive::EnableAstrobeeIntercomms(ff_msgs::CommandStampedPtr const& return true; } -bool Executive::Fault(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing fault command!"); +bool Executive::Fault(ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing fault command!"); // Only transition to the fault state if the fault command came from the // system monitor or executive. The only way to transition out of the fault @@ -1781,25 +1838,28 @@ bool Executive::Fault(ff_msgs::CommandStampedPtr const& cmd) { // transition out of the fault state if (cmd->cmd_src == "sys_monitor" || cmd->cmd_src == "executive") { // Configure leds to be blinking since there is a fault - ff_hw_msgs::ConfigureSystemLeds led_srv; - led_srv.request.status_a2 = ff_hw_msgs::ConfigureSystemLeds::Request::FAST; + ff_util::FreeFlyerService led_srv; + led_srv.request->status_a2 = + ff_hw_msgs::srv::ConfigureSystemLeds::Request::FAST; ConfigureLed(led_srv); state_->AckCmd(cmd->cmd_id); return true; } state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, "The fault command can only come from the system monitor."); return false; } -bool Executive::GripperControl(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing gripper control command!"); +bool Executive::GripperControl( + ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing gripper control command!"); return ArmControl(cmd); } -bool Executive::IdlePropulsion(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing idle propulsion command!"); +bool Executive::IdlePropulsion( + ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing idle propulsion command!"); // Cancel any motion actions being executed include the arm unsigned int i = 0; @@ -1816,7 +1876,7 @@ bool Executive::IdlePropulsion(ff_msgs::CommandStampedPtr const& cmd) { } else if (running_actions_[i] == IDLE) { // Don't try to idle if we are already trying to idle state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, "Can't idle, already trying to idle"); return false; } else if (running_actions_[i] == MOVE) { @@ -1844,11 +1904,13 @@ bool Executive::IdlePropulsion(ff_msgs::CommandStampedPtr const& cmd) { return StartAction(IDLE, cmd->cmd_id); } -bool Executive::InitializeBias(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing initialize bias command!"); +bool Executive::InitializeBias( + ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing initialize bias command!"); // We don't want to initialize the bias when stopped because we will not be // completely still - if (agent_state_.mobility_state.state == ff_msgs::MobilityState::STOPPING) { + if (agent_state_.mobility_state.state == + ff_msgs::msg::MobilityState::STOPPING) { AckMobilityStateIssue(cmd, "stopped", "docked, idle, or perched"); return false; } @@ -1856,7 +1918,7 @@ bool Executive::InitializeBias(ff_msgs::CommandStampedPtr const& cmd) { // We also cannot be moving when we initialize the bias if (FailCommandIfMoving(cmd)) { localization_goal_.command = - ff_msgs::LocalizationGoal::COMMAND_ESTIMATE_BIAS; + ff_msgs::action::Localization::Goal::COMMAND_ESTIMATE_BIAS; // Don't need to specify a pipeline for init bias but clear it just in case localization_goal_.pipeline = ""; return StartAction(LOCALIZATION, cmd->cmd_id); @@ -1864,38 +1926,39 @@ bool Executive::InitializeBias(ff_msgs::CommandStampedPtr const& cmd) { return false; } -bool Executive::LoadNodelet(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing load nodelet command!"); +bool Executive::LoadNodelet(ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing load nodelet command!"); return LoadUnloadNodelet(cmd); } -bool Executive::NoOp(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing noop command!"); +bool Executive::NoOp(ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing noop command!"); state_->AckCmd(cmd->cmd_id); return true; } -bool Executive::PausePlan(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing pause plan command!"); +bool Executive::PausePlan(ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing pause plan command!"); return state_->PausePlan(cmd); } -bool Executive::Perch(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing perch command!"); +bool Executive::Perch(ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing perch command!"); bool successful = false; std::string err_msg; // Make sure robot is stopped before attempting to perch. Only accept perch in // ready op state so perched and docked are the only mobility states we need // to check for - if (agent_state_.mobility_state.state == ff_msgs::MobilityState::DOCKING) { + if (agent_state_.mobility_state.state == + ff_msgs::msg::MobilityState::DOCKING) { err_msg = "Astrobee cannot attempt to perch while it is perched."; } else if (agent_state_.mobility_state.state == - ff_msgs::MobilityState::PERCHING) { + ff_msgs::msg::MobilityState::PERCHING) { err_msg = "Already perched."; } else { successful = true; - perch_goal_.command = ff_msgs::PerchGoal::PERCH; + perch_goal_.command = ff_msgs::action::Perch::Goal::PERCH; if (!StartAction(PERCH, cmd->cmd_id)) { return false; @@ -1906,67 +1969,71 @@ bool Executive::Perch(ff_msgs::CommandStampedPtr const& cmd) { // if we aren't stopped and thus cannot perch if (!successful) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, err_msg); } return successful; } -bool Executive::PowerItemOff(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing power item off command!"); +bool Executive::PowerItemOff( + ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing power item off command!"); return PowerItem(cmd, false); } -bool Executive::PowerItemOn(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing power item on command!"); +bool Executive::PowerItemOn(ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing power item on command!"); return PowerItem(cmd, true); } -bool Executive::Prepare(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing prepare command!"); +bool Executive::Prepare(ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing prepare command!"); // TODO(Katie) Stub, change to be actual code // Astrobee needs to be either stopped or drifting if (CheckStoppedOrDrifting(cmd->cmd_id, "preparing")) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, "Prepare not implemented yet! Stay tune!"); return false; } return false; } -bool Executive::ReacquirePosition(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing reacquire position command!"); +bool Executive::ReacquirePosition( + ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing reacquire position command!"); if (FailCommandIfMoving(cmd)) { // Reacquire position tries to get astrobee localizing again with mapped // landmarks localization_goal_.command = - ff_msgs::LocalizationGoal::COMMAND_SWITCH_PIPELINE; + ff_msgs::action::Localization::Goal::COMMAND_SWITCH_PIPELINE; localization_goal_.pipeline = - ff_msgs::LocalizationGoal::PIPELINE_MAP_LANDMARKS; + ff_msgs::action::Localization::Goal::PIPELINE_MAP_LANDMARKS; return StartAction(REACQUIRE, cmd->cmd_id); } return false; } -bool Executive::ResetEkf(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing reset ekf command!"); +bool Executive::ResetEkf(ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing reset ekf command!"); if (FailCommandIfMoving(cmd)) { return ResetEkf(cmd->cmd_id); } return false; } -bool Executive::RestartGuestScience(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing restart guest science command!"); +bool Executive::RestartGuestScience( + ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing restart guest science command!"); return ProcessGuestScienceCommand(cmd); } -bool Executive::RunPlan(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing run plan command!"); - if (agent_state_.plan_execution_state.state != ff_msgs::ExecState::PAUSED) { +bool Executive::RunPlan(ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing run plan command!"); + if (agent_state_.plan_execution_state.state != + ff_msgs::msg::ExecState::PAUSED) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, "Got run plan cmd but plan status is not paused!"); return false; } @@ -1975,34 +2042,34 @@ bool Executive::RunPlan(ff_msgs::CommandStampedPtr const& cmd) { // Also send successful ack because technically we didn't fail, the plan // was just empty if (GetCurrentPlanItemType() == sequencer::ItemType::NONE) { - SetPlanExecState(ff_msgs::ExecState::IDLE); + SetPlanExecState(ff_msgs::msg::ExecState::IDLE); state_->AckCmd(cmd->cmd_id); return false; } // We can start the plan! state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::NOT, + ff_msgs::msg::AckCompletedStatus::NOT, "", - ff_msgs::AckStatus::EXECUTING); + ff_msgs::msg::AckStatus::EXECUTING); return true; } -bool Executive::SetCamera(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing set camera command!"); +bool Executive::SetCamera(ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing set camera command!"); std::string err_msg = ""; - uint8_t completed_status = ff_msgs::AckCompletedStatus::OK; + uint8_t completed_status = ff_msgs::msg::AckCompletedStatus::OK; bool successful = true; // Check to make sure command is formatted as expected if (cmd->args.size() != 5 || - cmd->args[0].data_type != ff_msgs::CommandArg::DATA_TYPE_STRING || - cmd->args[1].data_type != ff_msgs::CommandArg::DATA_TYPE_STRING || - cmd->args[2].data_type != ff_msgs::CommandArg::DATA_TYPE_STRING || - cmd->args[3].data_type != ff_msgs::CommandArg::DATA_TYPE_FLOAT || - cmd->args[4].data_type != ff_msgs::CommandArg::DATA_TYPE_FLOAT) { + cmd->args[0].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_STRING || + cmd->args[1].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_STRING || + cmd->args[2].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_STRING || + cmd->args[3].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_FLOAT || + cmd->args[4].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_FLOAT) { successful = false; err_msg = "Malformed arguments for set camera command!"; - completed_status = ff_msgs::AckCompletedStatus::BAD_SYNTAX; + completed_status = ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX; } else { // Third argument is a string specifing the width and height, need to // parse it @@ -2017,42 +2084,42 @@ bool Executive::SetCamera(ff_msgs::CommandStampedPtr const& cmd) { } else { successful = false; err_msg = "Camera resolution needs format w_h, wXh, or wxh!"; - completed_status = ff_msgs::AckCompletedStatus::BAD_SYNTAX; + completed_status = ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX; } // Need to ensure mode is valid int mode; if (cmd->args[1].s == CommandConstants::PARAM_NAME_CAMERA_MODE_BOTH) { - mode = ff_msgs::ConfigureCamera::Request::BOTH; + mode = ff_msgs::srv::ConfigureCamera::Request::BOTH; } else if (cmd->args[1].s == CommandConstants::PARAM_NAME_CAMERA_MODE_RECORDING) { - mode = ff_msgs::ConfigureCamera::Request::RECORDING; + mode = ff_msgs::srv::ConfigureCamera::Request::RECORDING; } else if (cmd->args[1].s == CommandConstants::PARAM_NAME_CAMERA_MODE_STREAMING) { - mode = ff_msgs::ConfigureCamera::Request::STREAMING; + mode = ff_msgs::srv::ConfigureCamera::Request::STREAMING; } else { successful = false; err_msg = "Camera mode invalid. Options are Both, Streaming, Recording"; - completed_status = ff_msgs::AckCompletedStatus::BAD_SYNTAX; + completed_status = ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX; } if (successful) { width = cmd->args[2].s.substr(0, pos); height = cmd->args[2].s.substr((pos + 1)); - ff_msgs::ConfigureCamera config_img_srv; - config_img_srv.request.mode = mode; - config_img_srv.request.rate = cmd->args[3].f; - config_img_srv.request.width = std::stoi(width); - config_img_srv.request.height = std::stoi(height); - config_img_srv.request.bitrate = cmd->args[4].f; + ff_util::FreeFlyerService config_img_srv; + config_img_srv.request->mode = mode; + config_img_srv.request->rate = cmd->args[3].f; + config_img_srv.request->width = std::stoi(width); + config_img_srv.request->height = std::stoi(height); + config_img_srv.request->bitrate = cmd->args[4].f; if (cmd->args[0].s == CommandConstants::PARAM_NAME_CAMERA_NAME_DOCK) { // Check to make sure the dock cam service is valid if (!dock_cam_config_client_.exists()) { successful = false; err_msg = "Dock cam config service not running! Node may have died"; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; } else { // Check to see if the dock cam was successfully configured if (!dock_cam_config_client_.call(config_img_srv)) { @@ -2060,7 +2127,7 @@ bool Executive::SetCamera(ff_msgs::CommandStampedPtr const& cmd) { // equal to 0 successful = false; err_msg = "Height, width, and/or rate was invalid for dock camera."; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; } } } else if (cmd->args[0].s == @@ -2069,7 +2136,7 @@ bool Executive::SetCamera(ff_msgs::CommandStampedPtr const& cmd) { if (!haz_cam_config_client_.exists()) { successful = false; err_msg = "Haz cam config service not running! Node may have died"; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; } else { // Check to see if the haz cam was successfully configured if (!haz_cam_config_client_.call(config_img_srv)) { @@ -2077,7 +2144,7 @@ bool Executive::SetCamera(ff_msgs::CommandStampedPtr const& cmd) { // equal to 0 successful = false; err_msg = "Height, width, and/or rate was invalid for haz camera."; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; } } } else if (cmd->args[0].s == @@ -2086,7 +2153,7 @@ bool Executive::SetCamera(ff_msgs::CommandStampedPtr const& cmd) { if (!nav_cam_config_client_.exists()) { successful = false; err_msg = "Nav cam configure service not running! Node may have died"; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; } else { // Check to see if the nav cam was successfully configured if (!nav_cam_config_client_.call(config_img_srv)) { @@ -2094,7 +2161,7 @@ bool Executive::SetCamera(ff_msgs::CommandStampedPtr const& cmd) { // equal to 0 successful = false; err_msg = "Height, width, and/or rate was invalid for nav camera."; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; } } } else if (cmd->args[0].s == @@ -2103,7 +2170,7 @@ bool Executive::SetCamera(ff_msgs::CommandStampedPtr const& cmd) { if (!perch_cam_config_client_.exists()) { successful = false; err_msg = "Perch cam config service not running! Node may have died"; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; } else { // Check to see if the perch cam was successfully configured if (!perch_cam_config_client_.call(config_img_srv)) { @@ -2111,7 +2178,7 @@ bool Executive::SetCamera(ff_msgs::CommandStampedPtr const& cmd) { // equal to 0 successful = false; err_msg = "Height, width, and/or rate was invalid for perch camera"; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; } } } else if (cmd->args[0].s == @@ -2120,7 +2187,7 @@ bool Executive::SetCamera(ff_msgs::CommandStampedPtr const& cmd) { if (!sci_cam_config_client_.exists()) { successful = false; err_msg = "Sci cam configure service not running! Node may have died"; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; } else { // Check to see if the sci cam was successfully configured if (!sci_cam_config_client_.call(config_img_srv)) { @@ -2129,13 +2196,13 @@ bool Executive::SetCamera(ff_msgs::CommandStampedPtr const& cmd) { successful = false; err_msg = "Height, width, rate, and/or bitrate was invalid for sci"; err_msg += " camera."; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; } } } else { successful = false; err_msg = "Camera " + cmd->args[0].s + " not recognized."; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; } } } @@ -2144,35 +2211,37 @@ bool Executive::SetCamera(ff_msgs::CommandStampedPtr const& cmd) { return successful; } -bool Executive::SetCameraRecording(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing set camera recording command!"); +bool Executive::SetCameraRecording( + ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing set camera recording command!"); bool successful = true; std::string err_msg; - uint8_t completed_status = ff_msgs::AckCompletedStatus::OK; + uint8_t completed_status = ff_msgs::msg::AckCompletedStatus::OK; // Check to make sure command is formatted as expected if (cmd->args.size() != 2 || - cmd->args[0].data_type != ff_msgs::CommandArg::DATA_TYPE_STRING || - cmd->args[1].data_type != ff_msgs::CommandArg::DATA_TYPE_BOOL) { + cmd->args[0].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_STRING || + cmd->args[1].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_BOOL) { successful = false; err_msg = "Malformed arguments for set camera recording command!"; - completed_status = ff_msgs::AckCompletedStatus::BAD_SYNTAX; + completed_status = ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX; } else { - ff_msgs::EnableCamera enable_img_srv; - enable_img_srv.request.mode = ff_msgs::EnableCamera::Request::RECORDING; - enable_img_srv.request.enable = cmd->args[1].b; + ff_util::FreeFlyerService enable_img_srv; + enable_img_srv.request->mode = + ff_msgs::srv::EnableCamera::Request::RECORDING; + enable_img_srv.request->enable = cmd->args[1].b; if (cmd->args[0].s == CommandConstants::PARAM_NAME_CAMERA_NAME_DOCK) { // Check to make sure the dock cam enable service exists if (!dock_cam_enable_client_.exists()) { successful = false; err_msg = "Dock cam enable service not running! Node may have died!"; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; } else { // Check to see if recording was set successfully if (!dock_cam_enable_client_.call(enable_img_srv)) { successful = false; err_msg = "Enable recording failed for dock cam."; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; } } } else if (cmd->args[0].s == CommandConstants::PARAM_NAME_CAMERA_NAME_HAZ) { @@ -2180,13 +2249,13 @@ bool Executive::SetCameraRecording(ff_msgs::CommandStampedPtr const& cmd) { if (!haz_cam_enable_client_.exists()) { successful = false; err_msg = "Haz cam enable service not running! Node may have died!"; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; } else { // Check to see if recording was set successfully if (!haz_cam_enable_client_.call(enable_img_srv)) { successful = false; err_msg = "Enable recording failed for haz cam."; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; } } } else if (cmd->args[0].s == CommandConstants::PARAM_NAME_CAMERA_NAME_NAV) { @@ -2194,13 +2263,13 @@ bool Executive::SetCameraRecording(ff_msgs::CommandStampedPtr const& cmd) { if (!nav_cam_enable_client_.exists()) { successful = false; err_msg = "Nav cam enable service not running! Node may have died!"; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; } else { // Check to see if recording was set successfully if (!nav_cam_enable_client_.call(enable_img_srv)) { successful = false; err_msg = "Enable recording failed for nav cam."; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; } } } else if (cmd->args[0].s == @@ -2209,13 +2278,13 @@ bool Executive::SetCameraRecording(ff_msgs::CommandStampedPtr const& cmd) { if (!perch_cam_enable_client_.exists()) { successful = false; err_msg = "Perch cam enable service not running! Node may have died!"; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; } else { // Check to see if recording was set successfully if (!perch_cam_enable_client_.call(enable_img_srv)) { successful = false; err_msg = "Enable recording failed for perch cam."; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; } } } else if (cmd->args[0].s == CommandConstants::PARAM_NAME_CAMERA_NAME_SCI) { @@ -2223,19 +2292,19 @@ bool Executive::SetCameraRecording(ff_msgs::CommandStampedPtr const& cmd) { if (!sci_cam_enable_client_.exists()) { successful = false; err_msg = "Sci cam enable service not running! Node may have died!"; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; } else { // Check to see if recording was set successfully if (!sci_cam_enable_client_.call(enable_img_srv)) { successful = false; err_msg = "Enable recording failed for sci cam."; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; } } } else { successful = false; err_msg = "Camera " + cmd->args[0].s + " not recognized."; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; } } @@ -2243,35 +2312,37 @@ bool Executive::SetCameraRecording(ff_msgs::CommandStampedPtr const& cmd) { return successful; } -bool Executive::SetCameraStreaming(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing set camera streaming command!"); +bool Executive::SetCameraStreaming( + ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing set camera streaming command!"); bool successful = true; std::string err_msg = ""; - uint8_t completed_status = ff_msgs::AckCompletedStatus::OK; + uint8_t completed_status = ff_msgs::msg::AckCompletedStatus::OK; // Check to make sure command is formatted as expected if (cmd->args.size() != 2 || - cmd->args[0].data_type != ff_msgs::CommandArg::DATA_TYPE_STRING || - cmd->args[1].data_type != ff_msgs::CommandArg::DATA_TYPE_BOOL) { + cmd->args[0].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_STRING || + cmd->args[1].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_BOOL) { successful = false; err_msg = "Malformed arguments for set camera streaming!"; - completed_status = ff_msgs::AckCompletedStatus::BAD_SYNTAX; + completed_status = ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX; } else { - ff_msgs::EnableCamera enable_img_srv; - enable_img_srv.request.mode = ff_msgs::EnableCamera::Request::STREAMING; - enable_img_srv.request.enable = cmd->args[1].b; + ff_util::FreeFlyerService enable_img_srv; + enable_img_srv.request->mode = + ff_msgs::srv::EnableCamera::Request::STREAMING; + enable_img_srv.request->enable = cmd->args[1].b; if (cmd->args[0].s == CommandConstants::PARAM_NAME_CAMERA_NAME_DOCK) { // Check to make sure the dock cam service is valid if (!dock_cam_enable_client_.exists()) { successful = false; err_msg = "Dock cam enable service not running! Node may have died!"; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; } else { // Check to see if streaming was successfully set if (!dock_cam_enable_client_.call(enable_img_srv)) { successful = false; err_msg = "Enable streaming failed for dock cam."; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; } } } else if (cmd->args[0].s == CommandConstants::PARAM_NAME_CAMERA_NAME_HAZ) { @@ -2279,13 +2350,13 @@ bool Executive::SetCameraStreaming(ff_msgs::CommandStampedPtr const& cmd) { if (!haz_cam_enable_client_.exists()) { successful = false; err_msg = "Haz cam enable service not running! Node may have died!"; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; } else { // Check to see if streaming was successfully set if (!haz_cam_enable_client_.call(enable_img_srv)) { successful = false; err_msg = "Enable streaming failed for haz cam."; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; } } } else if (cmd->args[0].s == CommandConstants::PARAM_NAME_CAMERA_NAME_NAV) { @@ -2293,13 +2364,13 @@ bool Executive::SetCameraStreaming(ff_msgs::CommandStampedPtr const& cmd) { if (!nav_cam_enable_client_.exists()) { successful = false; err_msg = "Nav cam enable service not running! Node may have died!"; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; } else { // Check to see if streaming was successfully set if (!nav_cam_enable_client_.call(enable_img_srv)) { successful = false; err_msg = "Enable streaming failed for nav cam."; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; } } } else if (cmd->args[0].s == @@ -2308,13 +2379,13 @@ bool Executive::SetCameraStreaming(ff_msgs::CommandStampedPtr const& cmd) { if (!perch_cam_enable_client_.exists()) { successful = false; err_msg = "Perch cam enable service not running! Node may have died!"; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; } else { // Check to see if streaming was successfully set if (!perch_cam_enable_client_.call(enable_img_srv)) { successful = false; err_msg = "Enable streaming failed for perch cam."; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; } } } else if (cmd->args[0].s == CommandConstants::PARAM_NAME_CAMERA_NAME_SCI) { @@ -2322,19 +2393,19 @@ bool Executive::SetCameraStreaming(ff_msgs::CommandStampedPtr const& cmd) { if (!sci_cam_enable_client_.exists()) { successful = false; err_msg = "Sci cam enable service not running! Node may have died!"; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; } else { // Check to see if streaming was successfully set if (!sci_cam_enable_client_.call(enable_img_srv)) { successful = false; err_msg = "Enable streaming failed for sci cam."; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; } } } else { successful = false; err_msg = "Camera " + cmd->args[0].s + " not recognized."; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; } } @@ -2342,15 +2413,16 @@ bool Executive::SetCameraStreaming(ff_msgs::CommandStampedPtr const& cmd) { return successful; } -bool Executive::SetCheckObstacles(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing set check obstacles command!"); +bool Executive::SetCheckObstacles( + ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing set check obstacles command!"); // Don't set whether to check obstacles when moving if (FailCommandIfMoving(cmd)) { if (cmd->args.size() != 1 || - cmd->args[0].data_type != ff_msgs::CommandArg::DATA_TYPE_BOOL) { - NODELET_ERROR("Malformed arguments for set check obstacles!"); + cmd->args[0].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_BOOL) { + FF_ERROR("Malformed arguments for set check obstacles!"); state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::BAD_SYNTAX, + ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX, "Malformed arguments for set check obstacles!"); return false; } @@ -2363,15 +2435,16 @@ bool Executive::SetCheckObstacles(ff_msgs::CommandStampedPtr const& cmd) { return false; } -bool Executive::SetCheckZones(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing set check zones command!"); +bool Executive::SetCheckZones( + ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing set check zones command!"); // Don't set whether to check zones when moving if (FailCommandIfMoving(cmd)) { if (cmd->args.size() != 1 || - cmd->args[0].data_type != ff_msgs::CommandArg::DATA_TYPE_BOOL) { - NODELET_ERROR("Malformed arguments for set check zones!"); + cmd->args[0].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_BOOL) { + FF_ERROR("Malformed arguments for set check zones!"); state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::BAD_SYNTAX, + ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX, "Malformed arguments for set check zones!"); return false; } @@ -2384,10 +2457,10 @@ bool Executive::SetCheckZones(ff_msgs::CommandStampedPtr const& cmd) { return false; } -bool Executive::SetDataToDisk(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing set data to disk command!"); +bool Executive::SetDataToDisk(ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing set data to disk command!"); if (data_to_disk_) { - ff_msgs::SetDataToDisk data_srv; + ff_util::FreeFlyerService data_srv; std::string file_contents; // Decompress file into a string @@ -2399,7 +2472,7 @@ bool Executive::SetDataToDisk(ff_msgs::CommandStampedPtr const& cmd) { // Reset data to disk so that the same file isn't reloaded data_to_disk_.reset(); state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, "Unable to decompress data to disk file."); return false; } @@ -2411,7 +2484,7 @@ bool Executive::SetDataToDisk(ff_msgs::CommandStampedPtr const& cmd) { Json::Value file_obj; if (!jsonloader::LoadData(file_contents, &file_obj)) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::BAD_SYNTAX, + ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX, "Error parsing json."); return false; } @@ -2419,30 +2492,30 @@ bool Executive::SetDataToDisk(ff_msgs::CommandStampedPtr const& cmd) { // Check to make sure the name exists in the file if (!file_obj.isMember("name") || !file_obj["name"].isString()) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::BAD_SYNTAX, + ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX, "Name of data profile doesn't exist or isn't a string."); return false; } // Get name - data_srv.request.state.name = file_obj["name"].asString(); + data_srv.request->state.name = file_obj["name"].asString(); // Check to make sure topic settings exists if (!file_obj.isMember("topicSettings") || !file_obj["topicSettings"].isArray()) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::BAD_SYNTAX, + ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX, "Topic settings doesn't exist in file or is not an array!"); return false; } - ff_msgs::SaveSettings save_settings; + ff_msgs::msg::SaveSettings save_settings; for (Json::Value const& data_obj : file_obj["topicSettings"]) { // Check to make sure topic name exists if (!data_obj.isMember("topicName") || !data_obj["topicName"].isString()) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::BAD_SYNTAX, + ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX, "Topic name for entry doesn't exist or isn't a string."); return false; } @@ -2452,18 +2525,18 @@ bool Executive::SetDataToDisk(ff_msgs::CommandStampedPtr const& cmd) { if (!data_obj.isMember("downlinkOption") || !data_obj["downlinkOption"].isString()) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::BAD_SYNTAX, + ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX, "Downlink option doesn't exist or isn't a string."); return false; } if (data_obj["downlinkOption"].asString() == "immediate") { - save_settings.downlinkOption = ff_msgs::SaveSettings::IMMEDIATE; + save_settings.downlink_option = ff_msgs::msg::SaveSettings::IMMEDIATE; } else if (data_obj["downlinkOption"].asString() == "delayed") { - save_settings.downlinkOption = ff_msgs::SaveSettings::DELAYED; + save_settings.downlink_option = ff_msgs::msg::SaveSettings::DELAYED; } else { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::BAD_SYNTAX, + ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX, "Downlink option must be either delayed or immediate."); return false; } @@ -2472,17 +2545,17 @@ bool Executive::SetDataToDisk(ff_msgs::CommandStampedPtr const& cmd) { if (!data_obj.isMember("frequency") || !data_obj["frequency"].isNumeric()) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::BAD_SYNTAX, + ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX, "Frequency for entry doesn't exist or isn't a float."); return false; } save_settings.frequency = data_obj["frequency"].asFloat(); - data_srv.request.state.topic_save_settings.push_back(save_settings); + data_srv.request->state.topic_save_settings.push_back(save_settings); } // Check to make sure the service is valid and running - if (!CheckServiceExists(set_data_client_, + if (!CheckServiceExists(set_data_client_.exists(), "Set data to disk", cmd->cmd_id)) { return false; @@ -2490,15 +2563,15 @@ bool Executive::SetDataToDisk(ff_msgs::CommandStampedPtr const& cmd) { if (!set_data_client_.call(data_srv)) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, "Set data to disk service returned false."); return false; } - if (!data_srv.response.success) { + if (!data_srv.response->success) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, - data_srv.response.status); + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, + data_srv.response->status); return false; } @@ -2509,18 +2582,19 @@ bool Executive::SetDataToDisk(ff_msgs::CommandStampedPtr const& cmd) { // Data to disk pointer is null state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, "Data to disk file not found."); return false; } -bool Executive::SetEnableAutoReturn(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing set enable auto return command!"); +bool Executive::SetEnableAutoReturn( + ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing set enable auto return command!"); if (cmd->args.size() != 1 || - cmd->args[0].data_type != ff_msgs::CommandArg::DATA_TYPE_BOOL) { - NODELET_ERROR("Malformed arguments for enable auto return command!"); + cmd->args[0].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_BOOL) { + FF_ERROR("Malformed arguments for enable auto return command!"); state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::BAD_SYNTAX, + ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX, "Malformed arguments for enable auto return command!"); return false; } @@ -2531,14 +2605,15 @@ bool Executive::SetEnableAutoReturn(ff_msgs::CommandStampedPtr const& cmd) { return true; } -bool Executive::SetEnableImmediate(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing set enable immediate command!"); +bool Executive::SetEnableImmediate( + ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing set enable immediate command!"); if (FailCommandIfMoving(cmd)) { if (cmd->args.size() != 1 || - cmd->args[0].data_type != ff_msgs::CommandArg::DATA_TYPE_BOOL) { - NODELET_ERROR("Malformed arguments for enable immediate command!"); + cmd->args[0].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_BOOL) { + FF_ERROR("Malformed arguments for enable immediate command!"); state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::BAD_SYNTAX, + ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX, "Malformed arguments for enable immediate command!"); return false; } @@ -2551,14 +2626,15 @@ bool Executive::SetEnableImmediate(ff_msgs::CommandStampedPtr const& cmd) { return false; } -bool Executive::SetEnableReplan(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing set enable replan command!"); +bool Executive::SetEnableReplan( + ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing set enable replan command!"); if (FailCommandIfMoving(cmd)) { if (cmd->args.size() != 1 || - cmd->args[0].data_type != ff_msgs::CommandArg::DATA_TYPE_BOOL) { - NODELET_ERROR("Malformed arguments for enable replan command!"); + cmd->args[0].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_BOOL) { + FF_ERROR("Malformed arguments for enable replan command!"); state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::BAD_SYNTAX, + ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX, "Malformed arguments for enable replan command!"); return false; } @@ -2571,71 +2647,72 @@ bool Executive::SetEnableReplan(ff_msgs::CommandStampedPtr const& cmd) { return false; } -bool Executive::SetFlashlightBrightness(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing set flashlight brightness command!"); +bool Executive::SetFlashlightBrightness( + ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing set flashlight brightness command!"); bool successful = true; - uint8_t completed_status = ff_msgs::AckCompletedStatus::OK; + uint8_t completed_status = ff_msgs::msg::AckCompletedStatus::OK; std::string err_msg = ""; // Check to make sure command is formatted as expected if (cmd->args.size() != 2 || - cmd->args[0].data_type != ff_msgs::CommandArg::DATA_TYPE_STRING || - cmd->args[1].data_type != ff_msgs::CommandArg::DATA_TYPE_FLOAT) { + cmd->args[0].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_STRING || + cmd->args[1].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_FLOAT) { successful = false; err_msg = "Malformed arguments for set flashlight brightness command!"; - completed_status = ff_msgs::AckCompletedStatus::BAD_SYNTAX; + completed_status = ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX; } else if (cmd->args[0].s != CommandConstants::PARAM_NAME_FLASHLIGHT_LOCATION_BACK && cmd->args[0].s != CommandConstants::PARAM_NAME_FLASHLIGHT_LOCATION_FRONT) { successful = false; err_msg = "Flashlight location not recognized. Must be Front or Back."; - completed_status = ff_msgs::AckCompletedStatus::BAD_SYNTAX; + completed_status = ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX; } else if (cmd->args[1].f < 0 || cmd->args[1].f > 1) { successful = false; err_msg = "Flashlight brightness must be a value between 0 and 1."; - completed_status = ff_msgs::AckCompletedStatus::BAD_SYNTAX; + completed_status = ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX; } else { - ff_hw_msgs::SetFlashlight flashlight_srv; + ff_util::FreeFlyerService flashlight_srv; // Flashlight brightness needs to be a value between 0 and 200 - flashlight_srv.request.brightness = 200 * cmd->args[1].f; + flashlight_srv.request->brightness = 200 * cmd->args[1].f; if (cmd->args[0].s == CommandConstants::PARAM_NAME_FLASHLIGHT_LOCATION_BACK) { if (!back_flashlight_client_.exists()) { successful = false; err_msg = "Back flashlight control service isn't running."; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; } else { if (!back_flashlight_client_.call(flashlight_srv)) { successful = false; err_msg = "Back flashlight service returned false."; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; } // Check to see if flashlight brightness was successfully set - if (successful && !flashlight_srv.response.success) { + if (successful && !flashlight_srv.response->success) { successful = false; - err_msg = flashlight_srv.response.status_message; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; + err_msg = flashlight_srv.response->status_message; + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; } } } else { if (!front_flashlight_client_.exists()) { successful = false; err_msg = "Front flashlight control service isn't running."; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; } else { if (!front_flashlight_client_.call(flashlight_srv)) { successful = false; err_msg = "Front flashlight service returned false."; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; } // Check to see if flashlight brightness was successfully set - if (successful && !flashlight_srv.response.success) { + if (successful && !flashlight_srv.response->success) { successful = false; - err_msg = flashlight_srv.response.status_message; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; + err_msg = flashlight_srv.response->status_message; + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; } } } @@ -2645,14 +2722,15 @@ bool Executive::SetFlashlightBrightness(ff_msgs::CommandStampedPtr const& cmd) { return successful; } -bool Executive::SetHolonomicMode(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing set holonomic mode command!"); +bool Executive::SetHolonomicMode( + ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing set holonomic mode command!"); if (FailCommandIfMoving(cmd)) { if (cmd->args.size() != 1 || - cmd->args[0].data_type != ff_msgs::CommandArg::DATA_TYPE_BOOL) { - NODELET_ERROR("Malformed arguments for set holonomic mode command!"); + cmd->args[0].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_BOOL) { + FF_ERROR("Malformed arguments for set holonomic mode command!"); state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::BAD_SYNTAX, + ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX, "Malformed arguments for set holonomic mode command!"); return false; } @@ -2665,54 +2743,56 @@ bool Executive::SetHolonomicMode(ff_msgs::CommandStampedPtr const& cmd) { return false; } -bool Executive::SetInertia(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing set inertia command!"); +bool Executive::SetInertia(ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing set inertia command!"); if (FailCommandIfMoving(cmd)) { if (cmd->args.size() != 4 || - cmd->args[0].data_type != ff_msgs::CommandArg::DATA_TYPE_STRING || - cmd->args[1].data_type != ff_msgs::CommandArg::DATA_TYPE_FLOAT || - cmd->args[2].data_type != ff_msgs::CommandArg::DATA_TYPE_VEC3d || - cmd->args[3].data_type != ff_msgs::CommandArg::DATA_TYPE_MAT33f) { + cmd->args[0].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_STRING || + cmd->args[1].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_FLOAT || + cmd->args[2].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_VEC3D || + cmd->args[3].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_MAT33F) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::BAD_SYNTAX, + ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX, "Malformed arguments for set inertia command!"); return false; } - ff_msgs::SetInertia inertia_srv; + ff_util::FreeFlyerService inertia_srv; // Inertia profile name - inertia_srv.request.inertia.header.frame_id = cmd->args[0].s; + inertia_srv.request->inertia.header.frame_id = cmd->args[0].s; // Mass - inertia_srv.request.inertia.inertia.m = cmd->args[1].f; + inertia_srv.request->inertia.inertia.m = cmd->args[1].f; // Center of mass - inertia_srv.request.inertia.inertia.com.x = cmd->args[2].vec3d[0]; - inertia_srv.request.inertia.inertia.com.y = cmd->args[2].vec3d[1]; - inertia_srv.request.inertia.inertia.com.z = cmd->args[2].vec3d[2]; + inertia_srv.request->inertia.inertia.com.x = cmd->args[2].vec3d[0]; + inertia_srv.request->inertia.inertia.com.y = cmd->args[2].vec3d[1]; + inertia_srv.request->inertia.inertia.com.z = cmd->args[2].vec3d[2]; // Inertia Tensor // | ixx ixy ixz | // I = | ixy iyy iyz | // | ixz iyz izz | - inertia_srv.request.inertia.inertia.ixx = cmd->args[3].mat33f[0]; - inertia_srv.request.inertia.inertia.ixy = cmd->args[3].mat33f[1]; - inertia_srv.request.inertia.inertia.ixz = cmd->args[3].mat33f[2]; - inertia_srv.request.inertia.inertia.iyy = cmd->args[3].mat33f[4]; - inertia_srv.request.inertia.inertia.iyz = cmd->args[3].mat33f[5]; - inertia_srv.request.inertia.inertia.izz = cmd->args[3].mat33f[8]; - - if (!CheckServiceExists(set_inertia_client_, "Set inertia", cmd->cmd_id)) { + inertia_srv.request->inertia.inertia.ixx = cmd->args[3].mat33f[0]; + inertia_srv.request->inertia.inertia.ixy = cmd->args[3].mat33f[1]; + inertia_srv.request->inertia.inertia.ixz = cmd->args[3].mat33f[2]; + inertia_srv.request->inertia.inertia.iyy = cmd->args[3].mat33f[4]; + inertia_srv.request->inertia.inertia.iyz = cmd->args[3].mat33f[5]; + inertia_srv.request->inertia.inertia.izz = cmd->args[3].mat33f[8]; + + if (!CheckServiceExists(set_inertia_client_.exists(), + "Set inertia", + cmd->cmd_id)) { return false; } if (!set_inertia_client_.call(inertia_srv)) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, "Set inertia service returned false!"); return false; } - if (!inertia_srv.response.success) { + if (!inertia_srv.response->success) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, "Set inertia service returned unsuccessful."); return false; } @@ -2724,29 +2804,30 @@ bool Executive::SetInertia(ff_msgs::CommandStampedPtr const& cmd) { return false; } -bool Executive::SetOperatingLimits(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing set operating limits command!"); +bool Executive::SetOperatingLimits( + ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing set operating limits command!"); if (FailCommandIfMoving(cmd)) { if (cmd->args.size() != 7 || - cmd->args[0].data_type != ff_msgs::CommandArg::DATA_TYPE_STRING || - cmd->args[1].data_type != ff_msgs::CommandArg::DATA_TYPE_STRING || - cmd->args[2].data_type != ff_msgs::CommandArg::DATA_TYPE_FLOAT || - cmd->args[3].data_type != ff_msgs::CommandArg::DATA_TYPE_FLOAT || - cmd->args[4].data_type != ff_msgs::CommandArg::DATA_TYPE_FLOAT || - cmd->args[5].data_type != ff_msgs::CommandArg::DATA_TYPE_FLOAT || - cmd->args[6].data_type != ff_msgs::CommandArg::DATA_TYPE_FLOAT) { + cmd->args[0].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_STRING || + cmd->args[1].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_STRING || + cmd->args[2].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_FLOAT || + cmd->args[3].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_FLOAT || + cmd->args[4].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_FLOAT || + cmd->args[5].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_FLOAT || + cmd->args[6].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_FLOAT) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::BAD_SYNTAX, + ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX, "Malformed arguments for set operating limits command!"); return false; } // Check to make sure the flight mode exists before setting everything - ff_msgs::FlightMode mode; + ff_msgs::msg::FlightMode mode; if (!ff_util::FlightUtil::GetFlightMode(mode, cmd->args[1].s)) { std::string err_msg = "Flight mode " + cmd->args[1].s +" doesn't exist!."; state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::BAD_SYNTAX, + ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX, err_msg); return false; } @@ -2775,18 +2856,18 @@ bool Executive::SetOperatingLimits(ff_msgs::CommandStampedPtr const& cmd) { return false; } -bool Executive::SetPlan(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing set plan command!"); +bool Executive::SetPlan(ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing set plan command!"); std::string err_msg; if (plan_) { if (sequencer::LoadPlan(plan_, &sequencer_)) { if (sequencer_.plan_status().name.size() < 128) { // Set plan execution state to paused, apparently this was the way // spheres worked - SetPlanExecState(ff_msgs::ExecState::PAUSED); + SetPlanExecState(ff_msgs::msg::ExecState::PAUSED); // Publish plan stuff for ground PublishPlan(); - PublishPlanStatus(ff_msgs::AckStatus::QUEUED); + PublishPlanStatus(ff_msgs::msg::AckStatus::QUEUED); // Clear plan so that the operator has to upload a new plan after this // plan is done running plan_.reset(); @@ -2809,22 +2890,22 @@ bool Executive::SetPlan(ff_msgs::CommandStampedPtr const& cmd) { } else { err_msg = "No plan found! Plan must have failed to upload."; } - SetPlanExecState(ff_msgs::ExecState::IDLE); + SetPlanExecState(ff_msgs::msg::ExecState::IDLE); state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, err_msg); return false; } -bool Executive::SetPlanner(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing set planner command!"); +bool Executive::SetPlanner(ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing set planner command!"); // Don't set planner when moving if (FailCommandIfMoving(cmd)) { if (cmd->args.size() != 1 || - cmd->args[0].data_type != ff_msgs::CommandArg::DATA_TYPE_STRING) { - NODELET_ERROR("Malformed arguments for set planner command!"); + cmd->args[0].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_STRING) { + FF_ERROR("Malformed arguments for set planner command!"); state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::BAD_SYNTAX, + ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX, "Malformed arguments for set planner command!"); return false; } @@ -2833,9 +2914,9 @@ bool Executive::SetPlanner(ff_msgs::CommandStampedPtr const& cmd) { if (cmd->args[0].s != CommandConstants::PARAM_NAME_PLANNER_TYPE_TRAPEZOIDAL && cmd->args[0].s != CommandConstants::PARAM_NAME_PLANNER_TYPE_QUADRATIC_PROGRAM) { - NODELET_ERROR("Planner must be either Trapezoidal or QuadraticProgram"); + FF_ERROR("Planner must be either Trapezoidal or QuadraticProgram"); state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::BAD_SYNTAX, + ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX, "Planner must be either Trapezoidal or QuadraticProgram"); return false; } @@ -2848,66 +2929,70 @@ bool Executive::SetPlanner(ff_msgs::CommandStampedPtr const& cmd) { return false; } -bool Executive::SetTelemetryRate(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing set telemetry rate command!"); +bool Executive::SetTelemetryRate( + ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing set telemetry rate command!"); if (cmd->args.size() != 2 || - cmd->args[0].data_type != ff_msgs::CommandArg::DATA_TYPE_STRING || - cmd->args[1].data_type != ff_msgs::CommandArg::DATA_TYPE_FLOAT) { + cmd->args[0].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_STRING || + cmd->args[1].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_FLOAT) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::BAD_SYNTAX, + ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX, "Malformed arguments for set telemetry rate!"); return false; } - ff_msgs::SetRate set_rate_srv; - set_rate_srv.request.rate = cmd->args[1].f; + ff_util::FreeFlyerService set_rate_srv; + set_rate_srv.request->rate = cmd->args[1].f; if (cmd->args[0].s == CommandConstants::PARAM_NAME_TELEMETRY_TYPE_COMM_STATUS) { - set_rate_srv.request.which = ff_msgs::SetRate::Request::COMM_STATUS; + set_rate_srv.request->which = ff_msgs::srv::SetRate::Request::COMM_STATUS; } else if (cmd->args[0].s == CommandConstants::PARAM_NAME_TELEMETRY_TYPE_CPU_STATE) { - set_rate_srv.request.which = ff_msgs::SetRate::Request::CPU_STATE; + set_rate_srv.request->which = ff_msgs::srv::SetRate::Request::CPU_STATE; } else if (cmd->args[0].s == CommandConstants::PARAM_NAME_TELEMETRY_TYPE_DISK_STATE) { - set_rate_srv.request.which = ff_msgs::SetRate::Request::DISK_STATE; + set_rate_srv.request->which = ff_msgs::srv::SetRate::Request::DISK_STATE; } else if (cmd->args[0].s == CommandConstants::PARAM_NAME_TELEMETRY_TYPE_EKF_STATE) { - set_rate_srv.request.which = ff_msgs::SetRate::Request::EKF_STATE; + set_rate_srv.request->which = ff_msgs::srv::SetRate::Request::EKF_STATE; } else if (cmd->args[0].s == CommandConstants::PARAM_NAME_TELEMETRY_TYPE_GNC_STATE) { - set_rate_srv.request.which = ff_msgs::SetRate::Request::GNC_STATE; + set_rate_srv.request->which = ff_msgs::srv::SetRate::Request::GNC_STATE; } else if (cmd->args[0].s == CommandConstants::PARAM_NAME_TELEMETRY_TYPE_PMC_CMD_STATE) { - set_rate_srv.request.which = ff_msgs::SetRate::Request::PMC_CMD_STATE; + set_rate_srv.request->which = ff_msgs::srv::SetRate::Request::PMC_CMD_STATE; } else if (cmd->args[0].s == CommandConstants::PARAM_NAME_TELEMETRY_TYPE_POSITION) { - set_rate_srv.request.which = ff_msgs::SetRate::Request::POSITION; + set_rate_srv.request->which = ff_msgs::srv::SetRate::Request::POSITION; } else if (cmd->args[0].s == CommandConstants::PARAM_NAME_TELEMETRY_TYPE_SPARSE_MAPPING_POSE) { - set_rate_srv.request.which = ff_msgs::SetRate::Request::SPARSE_MAPPING_POSE; + set_rate_srv.request->which = + ff_msgs::srv::SetRate::Request::SPARSE_MAPPING_POSE; } else { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::BAD_SYNTAX, + ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX, "Unknown name in set telemetry rate command!"); return false; } - if (!CheckServiceExists(set_rate_client_, "Set telem rate", cmd->cmd_id)) { + if (!CheckServiceExists(set_rate_client_.exists(), + "Set telem rate", + cmd->cmd_id)) { return false; } if (!set_rate_client_.call(set_rate_srv)) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, "Set rate service returned false."); return false; } // Check to see if the rate was set successfully - if (!set_rate_srv.response.success) { + if (!set_rate_srv.response->success) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, - set_rate_srv.response.status); + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, + set_rate_srv.response->status); return false; } @@ -2915,11 +3000,11 @@ bool Executive::SetTelemetryRate(ff_msgs::CommandStampedPtr const& cmd) { return true; } -bool Executive::SetZones(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing set zones command!"); +bool Executive::SetZones(ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing set zones command!"); if (FailCommandIfMoving(cmd)) { if (zones_) { - ff_msgs::SetZones zones_srv; + ff_util::FreeFlyerService zones_srv; std::string file_contents; // Decompress file into a string @@ -2929,7 +3014,7 @@ bool Executive::SetZones(ff_msgs::CommandStampedPtr const& cmd) { // Reset zones so that the same file isn't reloaded zones_.reset(); state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, "Unable to decompress zones file."); return false; } @@ -2941,7 +3026,7 @@ bool Executive::SetZones(ff_msgs::CommandStampedPtr const& cmd) { Json::Value file_obj; if (!jsonloader::LoadData(file_contents, &file_obj)) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::BAD_SYNTAX, + ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX, "Error parsing json."); return false; } @@ -2950,31 +3035,31 @@ bool Executive::SetZones(ff_msgs::CommandStampedPtr const& cmd) { if (!file_obj.isMember("timestamp") || !file_obj["timestamp"].isString()) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::BAD_SYNTAX, + ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX, "File timestamp doesn't exist or is not a string."); return false; } // Get timestamp in milliseconds and convert it to a number std::string timestamp = file_obj["timestamp"].asString(); - zones_srv.request.timestamp = MsToSec(timestamp); + zones_srv.request->timestamp = MsToSec(timestamp); // Check to make sure zones array exists if (!file_obj.isMember("zones") || !file_obj["zones"].isArray()) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::BAD_SYNTAX, + ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX, "Parser error: zones don't exist or are not an array!"); return false; } std::string err_msg; - ff_msgs::Zone zone; + ff_msgs::msg::Zone zone; int i = 0; for (Json::Value const& zone_obj : file_obj["zones"]) { // Check to make sure zone name exists if (!zone_obj.isMember("name") || !zone_obj["name"].isString()) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::BAD_SYNTAX, + ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX, "Name in zone doesn't exist or is not a string."); return false; } @@ -2983,21 +3068,21 @@ bool Executive::SetZones(ff_msgs::CommandStampedPtr const& cmd) { // Check to make sure safe exists for zone if (!zone_obj.isMember("safe") || !zone_obj["safe"].isBool()) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::BAD_SYNTAX, + ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX, "Safe flag in zone doesn't exist or is not a boolean"); return false; } if (zone_obj["safe"].asBool()) { - zone.type = ff_msgs::Zone::KEEPIN; + zone.type = ff_msgs::msg::Zone::KEEPIN; } else { - zone.type = ff_msgs::Zone::KEEPOUT; + zone.type = ff_msgs::msg::Zone::KEEPOUT; } // Check to make sure the sequence exists for the zone if (!zone_obj.isMember("sequence") || !zone_obj["sequence"].isArray()) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::BAD_SYNTAX, + ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX, "Sequence in zone doesn't exist or isn't an array."); return false; } @@ -3007,7 +3092,7 @@ bool Executive::SetZones(ff_msgs::CommandStampedPtr const& cmd) { zone.index = i; if (!box_array.isArray() || box_array.size() != 6) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::BAD_SYNTAX, + ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX, "Box isn't an array or doesn't have 6 points."); return false; } @@ -3016,7 +3101,7 @@ bool Executive::SetZones(ff_msgs::CommandStampedPtr const& cmd) { !box_array[2].isNumeric() || !box_array[3].isNumeric() || !box_array[4].isNumeric() || !box_array[5].isNumeric()) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::BAD_SYNTAX, + ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX, "One of the box points in zone is not numeric!"); return false; } @@ -3030,7 +3115,7 @@ bool Executive::SetZones(ff_msgs::CommandStampedPtr const& cmd) { zone.max.y = box_array[4].asFloat(); zone.max.z = box_array[5].asFloat(); - zones_srv.request.zones.push_back(zone); + zones_srv.request->zones.push_back(zone); i++; } } @@ -3038,21 +3123,21 @@ bool Executive::SetZones(ff_msgs::CommandStampedPtr const& cmd) { // Check to make sure the service is valid and running if (!zones_client_.exists()) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, "Set zones service isn't running! Choreographer may have died"); return false; } if (!zones_client_.call(zones_srv)) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, "Set zones service returned false."); return false; } - if (!zones_srv.response.success) { + if (!zones_srv.response->success) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, "Set zones was not successful."); return false; } @@ -3061,72 +3146,76 @@ bool Executive::SetZones(ff_msgs::CommandStampedPtr const& cmd) { return true; } state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, "No zones file found."); return false; } return false; } -bool Executive::SkipPlanStep(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing skip plan step command!"); +bool Executive::SkipPlanStep( + ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing skip plan step command!"); // Make sure plan execution state is paused - if (agent_state_.plan_execution_state.state != ff_msgs::ExecState::PAUSED) { + if (agent_state_.plan_execution_state.state != + ff_msgs::msg::ExecState::PAUSED) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, "Got command to skip a plan step but plan isn't paused."); return false; } - ff_msgs::AckCompletedStatus ack; - ack.status = ff_msgs::AckCompletedStatus::CANCELED; + ff_msgs::msg::AckCompletedStatus ack; + ack.status = ff_msgs::msg::AckCompletedStatus::CANCELED; // Check to see if we are skipping the last step in the plan if (sequencer_.Feedback(ack)) { - PublishPlanStatus(ff_msgs::AckStatus::QUEUED); + PublishPlanStatus(ff_msgs::msg::AckStatus::QUEUED); } else { - PublishPlanStatus(ff_msgs::AckStatus::COMPLETED); - SetPlanExecState(ff_msgs::ExecState::IDLE); + PublishPlanStatus(ff_msgs::msg::AckStatus::COMPLETED); + SetPlanExecState(ff_msgs::msg::ExecState::IDLE); } state_->AckCmd(cmd->cmd_id); return true; } -bool Executive::StartGuestScience(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing start guest science!"); +bool Executive::StartGuestScience( + ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing start guest science!"); return ProcessGuestScienceCommand(cmd); } -bool Executive::StartRecording(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing start recording command."); +bool Executive::StartRecording( + ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing start recording command."); bool successful = true; std::string err_msg; - uint8_t completed_status = ff_msgs::AckCompletedStatus::OK; + uint8_t completed_status = ff_msgs::msg::AckCompletedStatus::OK; if (cmd->args.size() != 1 || - cmd->args[0].data_type != ff_msgs::CommandArg::DATA_TYPE_STRING) { + cmd->args[0].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_STRING) { successful = false; err_msg = "Malformed arguments for start recording command."; - completed_status = ff_msgs::AckCompletedStatus::BAD_SYNTAX; + completed_status = ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX; } else { - ff_msgs::EnableRecording enable_rec_srv; - enable_rec_srv.request.enable = true; - enable_rec_srv.request.bag_description = cmd->args[0].s; + ff_util::FreeFlyerService enable_rec_srv; + enable_rec_srv.request->enable = true; + enable_rec_srv.request->bag_description = cmd->args[0].s; // Check to make sure the enable recording service exists if (!enable_recording_client_.exists()) { successful = false; err_msg = "Enable recording service not running! Node may have died!"; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; } else { // Call enable service and make sure it worked if (!enable_recording_client_.call(enable_rec_srv)) { successful = false; err_msg = "Enable recording service failed!"; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; - } else if (!enable_rec_srv.response.success) { + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; + } else if (!enable_rec_srv.response->success) { successful = false; - err_msg = enable_rec_srv.response.status; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; + err_msg = enable_rec_srv.response->status; + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; } } } @@ -3142,8 +3231,9 @@ bool Executive::StartRecording(ff_msgs::CommandStampedPtr const& cmd) { // haven't activated the pmcs yet. // This function is also as pause for a plan so if the plan flag is set, we // need to check if we are downloading data and if so, stop it. -bool Executive::StopAllMotion(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing stop all motion command!"); +bool Executive::StopAllMotion( + ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing stop all motion command!"); // We pretty much always start stop action even if stopped. See cases below // for situations we don't want to stop in bool successful = true, start_stop = true; @@ -3153,10 +3243,11 @@ bool Executive::StopAllMotion(ff_msgs::CommandStampedPtr const& cmd) { // it is, we need to check if mobility is idle. If mobility is idle, we don't // want to spin up the pmcs by excuting a stop if (cmd->cmd_src == "sys_monitor") { - if (agent_state_.mobility_state.state == ff_msgs::MobilityState::DRIFTING) { + if (agent_state_.mobility_state.state == + ff_msgs::msg::MobilityState::DRIFTING) { // Ack command as failed and don't stop state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, "PMCs are idle so don't want spin them up due to a fault"); return false; } @@ -3169,7 +3260,8 @@ bool Executive::StopAllMotion(ff_msgs::CommandStampedPtr const& cmd) { // If we are perched or docked, we will be idle and don't want to // spin up the motors to try to stop so we need to fail the stop command in // this case - if (agent_state_.mobility_state.state == ff_msgs::MobilityState::DOCKING && + if (agent_state_.mobility_state.state == + ff_msgs::msg::MobilityState::DOCKING && agent_state_.mobility_state.sub_state == 0) { err_msg = "Astrobee cannot stop while docked."; start_stop = false; @@ -3177,7 +3269,7 @@ bool Executive::StopAllMotion(ff_msgs::CommandStampedPtr const& cmd) { // Will check if we started the undock action but haven't received any // feedback in main for loop } else if (agent_state_.mobility_state.state == - ff_msgs::MobilityState::PERCHING && + ff_msgs::msg::MobilityState::PERCHING && agent_state_.mobility_state.sub_state == 0) { err_msg = "Astrobee cannot stop while perched."; start_stop = false; @@ -3203,7 +3295,7 @@ bool Executive::StopAllMotion(ff_msgs::CommandStampedPtr const& cmd) { // Don't stop if we are deactivating the PMCs or already docked but // switching localiization if (agent_state_.mobility_state.sub_state <= - ff_msgs::DockState::DOCKING_WAITING_FOR_SPIN_DOWN) { + ff_msgs::msg::DockState::DOCKING_WAITING_FOR_SPIN_DOWN) { err_msg = "Already deactivating pmcs or docked. Cannot stop."; start_stop = false; successful = false; @@ -3225,7 +3317,7 @@ bool Executive::StopAllMotion(ff_msgs::CommandStampedPtr const& cmd) { // Don't stop if we are deactivating the PMCs or already perched but // switching localization if (agent_state_.mobility_state.sub_state <= - ff_msgs::PerchState::PERCHING_WAITING_FOR_SPIN_DOWN) { + ff_msgs::msg::PerchState::PERCHING_WAITING_FOR_SPIN_DOWN) { err_msg = "Already deactivating the pmcs or perched. Cannot stop."; start_stop = false; successful = false; @@ -3243,7 +3335,7 @@ bool Executive::StopAllMotion(ff_msgs::CommandStampedPtr const& cmd) { // Figure out where we are in the undock process and only send a stop if // we are in or have completed the egressing state if (agent_state_.mobility_state.sub_state > - ff_msgs::DockState::UNDOCKING_MOVING_TO_APPROACH_POSE) { + ff_msgs::msg::DockState::UNDOCKING_MOVING_TO_APPROACH_POSE) { start_stop = false; // Set successful to true since it may have been set to false in the // if docked statement @@ -3262,7 +3354,7 @@ bool Executive::StopAllMotion(ff_msgs::CommandStampedPtr const& cmd) { // Figure out where we are in the unperching process and only send a stop // if we might not be perched any more. if (agent_state_.mobility_state.sub_state > - ff_msgs::PerchState::UNPERCHING_OPENING_GRIPPER) { + ff_msgs::msg::PerchState::UNPERCHING_OPENING_GRIPPER) { start_stop = false; // Set successful to true since it may have been set to false in the if // perched statement @@ -3288,9 +3380,9 @@ bool Executive::StopAllMotion(ff_msgs::CommandStampedPtr const& cmd) { // Ack before start action since start action will ack for us if it fails if (!successful) { - NODELET_ERROR("%s", err_msg.c_str()); + FF_ERROR("%s", err_msg.c_str()); state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, err_msg); } else if (successful && !start_stop) { // Ack successful since we cancelled an action that hadn't moved the robot @@ -3306,8 +3398,8 @@ bool Executive::StopAllMotion(ff_msgs::CommandStampedPtr const& cmd) { return successful; } -bool Executive::StopArm(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing stop arm command!"); +bool Executive::StopArm(ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing stop arm command!"); // Check for an arm action already being executed. If so, cancel it. if (IsActionRunning(ARM)) { CancelAction(ARM, "stop arm"); @@ -3324,35 +3416,37 @@ bool Executive::StopArm(ff_msgs::CommandStampedPtr const& cmd) { return true; } -bool Executive::StopGuestScience(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing stop guest science command!"); +bool Executive::StopGuestScience( + ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing stop guest science command!"); return ProcessGuestScienceCommand(cmd); } -bool Executive::StopRecording(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing stop recording command!"); +bool Executive::StopRecording( + ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing stop recording command!"); bool successful = true; std::string err_msg; - uint8_t completed_status = ff_msgs::AckCompletedStatus::OK; + uint8_t completed_status = ff_msgs::msg::AckCompletedStatus::OK; - ff_msgs::EnableRecording enable_rec_srv; - enable_rec_srv.request.enable = false; + ff_util::FreeFlyerService enable_rec_srv; + enable_rec_srv.request->enable = false; // Check to make sure the enable recording service exists if (!enable_recording_client_.exists()) { successful = false; err_msg = "Enable recording service not running! Node may have died!"; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; } else { // Call enable service and make sure it worked if (!enable_recording_client_.call(enable_rec_srv)) { successful = false; err_msg = "Enable recording service failed!"; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; - } else if (!enable_rec_srv.response.success) { + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; + } else if (!enable_rec_srv.response->success) { successful = false; - err_msg = enable_rec_srv.response.status; - completed_status = ff_msgs::AckCompletedStatus::EXEC_FAILED; + err_msg = enable_rec_srv.response->status; + completed_status = ff_msgs::msg::AckCompletedStatus::EXEC_FAILED; } } @@ -3360,12 +3454,13 @@ bool Executive::StopRecording(ff_msgs::CommandStampedPtr const& cmd) { return successful; } -bool Executive::StowArm(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing stow arm command!"); +bool Executive::StowArm(ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing stow arm command!"); // Check if Astrobee is perched. Arm control will check the rest. - if (agent_state_.mobility_state.state == ff_msgs::MobilityState::PERCHING) { + if (agent_state_.mobility_state.state == + ff_msgs::msg::MobilityState::PERCHING) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, "Can't stow arm while perched or (un)perching!"); return false; } @@ -3373,58 +3468,64 @@ bool Executive::StowArm(ff_msgs::CommandStampedPtr const& cmd) { return ArmControl(cmd); } -bool Executive::SwitchLocalization(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_DEBUG("Executive executing switch localization command!"); +bool Executive::SwitchLocalization( + ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_DEBUG("Executive executing switch localization command!"); if (FailCommandIfMoving(cmd)) { if (cmd->args.size() != 1 || - cmd->args[0].data_type != ff_msgs::CommandArg::DATA_TYPE_STRING) { + cmd->args[0].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_STRING) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::BAD_SYNTAX, + ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX, "Malformed arguments for switch localization!"); return false; } localization_goal_.command = - ff_msgs::LocalizationGoal::COMMAND_SWITCH_PIPELINE; + ff_msgs::action::Localization::Goal::COMMAND_SWITCH_PIPELINE; if (cmd->args[0].s == CommandConstants::PARAM_NAME_LOCALIZATION_MODE_NONE) { - localization_goal_.pipeline = ff_msgs::LocalizationGoal::PIPELINE_NONE; + localization_goal_.pipeline = + ff_msgs::action::Localization::Goal::PIPELINE_NONE; } else if (cmd->args[0].s == CommandConstants::PARAM_NAME_LOCALIZATION_MODE_MAPPED_LANDMARKS) { localization_goal_.pipeline = - ff_msgs::LocalizationGoal::PIPELINE_MAP_LANDMARKS; + ff_msgs::action::Localization::Goal::PIPELINE_MAP_LANDMARKS; } else if (cmd->args[0].s == CommandConstants::PARAM_NAME_LOCALIZATION_MODE_ARTAGS) { - localization_goal_.pipeline = ff_msgs::LocalizationGoal::PIPELINE_AR_TAGS; + localization_goal_.pipeline = + ff_msgs::action::Localization::Goal::PIPELINE_AR_TAGS; } else if (cmd->args[0].s == CommandConstants::PARAM_NAME_LOCALIZATION_MODE_HANDRAIL) { localization_goal_.pipeline = - ff_msgs::LocalizationGoal::PIPELINE_HANDRAIL; + ff_msgs::action::Localization::Goal::PIPELINE_HANDRAIL; } else if (cmd->args[0].s == CommandConstants::PARAM_NAME_LOCALIZATION_MODE_PERCH) { - localization_goal_.pipeline = ff_msgs::LocalizationGoal::PIPELINE_PERCH; + localization_goal_.pipeline = + ff_msgs::action::Localization::Goal::PIPELINE_PERCH; } else if (cmd->args[0].s == CommandConstants::PARAM_NAME_LOCALIZATION_MODE_TRUTH) { - localization_goal_.pipeline = ff_msgs::LocalizationGoal::PIPELINE_TRUTH; + localization_goal_.pipeline = + ff_msgs::action::Localization::Goal::PIPELINE_TRUTH; } return StartAction(LOCALIZATION, cmd->cmd_id); } return false; } -bool Executive::Undock(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing undock command!"); +bool Executive::Undock(ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing undock command!"); bool docked = false; std::string err_msg = ""; // Make sure robot is docked before attempting to undock. Only accept undock // in the ready op state so only need to check perched, stopped, or drifting - if (agent_state_.mobility_state.state == ff_msgs::MobilityState::DRIFTING) { + if (agent_state_.mobility_state.state == + ff_msgs::msg::MobilityState::DRIFTING) { err_msg = "Can't undock when not docked. Astrobee is currently drifting."; } else if (agent_state_.mobility_state.state == - ff_msgs::MobilityState::PERCHING) { + ff_msgs::msg::MobilityState::PERCHING) { err_msg = "Can't undock when not docked. Astrobee is currently perched."; } else if (agent_state_.mobility_state.state == - ff_msgs::MobilityState::STOPPING) { + ff_msgs::msg::MobilityState::STOPPING) { err_msg = "Can't undock when not docked. Astrobee is currently stopped."; } else { docked = true; @@ -3441,36 +3542,38 @@ bool Executive::Undock(ff_msgs::CommandStampedPtr const& cmd) { // need to fail an ack if we aren't docked and thus cannot undock if (!docked) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, err_msg); } return docked; } -bool Executive::UnloadNodelet(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing unload nodelet command!"); +bool Executive::UnloadNodelet( + ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing unload nodelet command!"); return LoadUnloadNodelet(cmd); } -bool Executive::Unperch(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing unperch command!"); +bool Executive::Unperch(ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing unperch command!"); bool perched = false; std::string err_msg = ""; // Make sure robot is perched before attempting to unperch. Only accept // unperch in the ready op state so only need to check docked, stopped, or // drifting - if (agent_state_.mobility_state.state == ff_msgs::MobilityState::DOCKING) { + if (agent_state_.mobility_state.state == + ff_msgs::msg::MobilityState::DOCKING) { err_msg = "Can't unperch when not perched. Astrobee is currently docked."; } else if (agent_state_.mobility_state.state == - ff_msgs::MobilityState::DRIFTING) { + ff_msgs::msg::MobilityState::DRIFTING) { err_msg = "Can't unperch when not perched. Astrobeep is currently drifting"; } else if (agent_state_.mobility_state.state == - ff_msgs::MobilityState::STOPPING) { + ff_msgs::msg::MobilityState::STOPPING) { err_msg = "Can't unperch when not perched. Astrobee is currently stopped."; } else { perched = true; - perch_goal_.command = ff_msgs::PerchGoal::UNPERCH; + perch_goal_.command = ff_msgs::action::Perch::Goal::UNPERCH; if (!StartAction(UNPERCH, cmd->cmd_id)) { return false; @@ -3481,32 +3584,32 @@ bool Executive::Unperch(ff_msgs::CommandStampedPtr const& cmd) { // if we aren't perched and thus cannot unperch if (!perched) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, err_msg); } return perched; } -bool Executive::Unterminate(ff_msgs::CommandStampedPtr const& cmd) { - ff_hw_msgs::ClearTerminate clear_srv; +bool Executive::Unterminate(ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + ff_util::FreeFlyerService clear_srv; // Clear eps terminate flag - if (!CheckServiceExists(eps_terminate_client_, + if (!CheckServiceExists(eps_terminate_client_.exists(), "EPS terminate", cmd->cmd_id)) { return false; } if (eps_terminate_client_.call(clear_srv)) { - if (!clear_srv.response.success) { + if (!clear_srv.response->success) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, - clear_srv.response.status_message); + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, + clear_srv.response->status_message); return false; } } else { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, "Eps clear terminate service returned false."); return false; } @@ -3515,12 +3618,12 @@ bool Executive::Unterminate(ff_msgs::CommandStampedPtr const& cmd) { return true; } -bool Executive::Wait(ff_msgs::CommandStampedPtr const& cmd) { - NODELET_INFO("Executive executing wait command! Duration %f", cmd->args[0].f); - if (cmd->args[0].data_type != ff_msgs::CommandArg::DATA_TYPE_FLOAT || +bool Executive::Wait(ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + FF_INFO("Executive executing wait command! Duration %f", cmd->args[0].f); + if (cmd->args[0].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_FLOAT || cmd->args[0].f < 0) { state_->AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::BAD_SYNTAX, + ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX, "Malformed arguments for wait command!"); return false; } @@ -3530,12 +3633,14 @@ bool Executive::Wait(ff_msgs::CommandStampedPtr const& cmd) { } /************************ Protected *******************************************/ -void Executive::Initialize(ros::NodeHandle *nh) { +void Executive::Initialize(NodeHandle &nh) { std::string err_msg; // Set executive in op state repo so the op_states can call this executive OpStateRepo::Instance()->SetExec(this); - nh_ = *nh; + nh_ = nh; + + sequencer_.SetNodeHandle(nh); // Read in all the action timeouts. They are in config files so that they can // be changed on the fly. @@ -3555,11 +3660,11 @@ void Executive::Initialize(ros::NodeHandle *nh) { } // Set up a timer to check and reload timeouts if they are changed. - reload_params_timer_ = nh_.createTimer(ros::Duration(1), - [this](ros::TimerEvent e) { + reload_params_timer_.createTimer(1.0, + [this]() { config_params_.CheckFilesUpdated(std::bind(&Executive::ReadParams, this));}, - false, true); + nh, false, true); // initialize actions arm_ac_.SetActiveTimeout(action_active_timeout_); @@ -3607,200 +3712,202 @@ void Executive::Initialize(ros::NodeHandle *nh) { perch_ac_.Create(nh, ACTION_BEHAVIORS_PERCH); // initialize subs - camera_state_sub_ = nh_.subscribe(TOPIC_MANAGEMENT_CAMERA_STATE, - sub_queue_size_, - &Executive::CameraStatesCallback, - this); - - cmd_sub_ = nh_.subscribe(TOPIC_COMMAND, - sub_queue_size_, - &Executive::CmdCallback, - this); - - data_sub_ = nh_.subscribe(TOPIC_COMMUNICATIONS_DDS_DATA, - sub_queue_size_, - &Executive::DataToDiskCallback, - this); - - dock_state_sub_ = nh_.subscribe(TOPIC_BEHAVIORS_DOCKING_STATE, - sub_queue_size_, - &Executive::DockStateCallback, - this); - - fault_state_sub_ = nh_.subscribe(TOPIC_MANAGEMENT_SYS_MONITOR_STATE, - sub_queue_size_, - &Executive::FaultStateCallback, - this); - - gs_ack_sub_ = nh_.subscribe(TOPIC_GUEST_SCIENCE_MANAGER_ACK, - sub_queue_size_, - &Executive::GuestScienceAckCallback, - this); - - gs_config_sub_ = nh_.subscribe(TOPIC_GUEST_SCIENCE_MANAGER_CONFIG, - sub_queue_size_, - &Executive::GuestScienceConfigCallback, - this); - - gs_state_sub_ = nh_.subscribe(TOPIC_GUEST_SCIENCE_MANAGER_STATE, - sub_queue_size_, - &Executive::GuestScienceStateCallback, - this); - - heartbeat_sub_ = nh_.subscribe(TOPIC_MANAGEMENT_SYS_MONITOR_HEARTBEAT, - sub_queue_size_, - &Executive::SysMonitorHeartbeatCallback, - this); - - - inertia_sub_ = nh_.subscribe(TOPIC_MOBILITY_INERTIA, - sub_queue_size_, - &Executive::InertiaCallback, - this); - - motion_sub_ = nh_.subscribe(TOPIC_MOBILITY_MOTION_STATE, - sub_queue_size_, - &Executive::MotionStateCallback, - this); - - perch_state_sub_ = nh_.subscribe(TOPIC_BEHAVIORS_PERCHING_STATE, - sub_queue_size_, - &Executive::PerchStateCallback, - this); - - plan_sub_ = nh_.subscribe(TOPIC_COMMUNICATIONS_DDS_PLAN, - sub_queue_size_, - &Executive::PlanCallback, - this); - - zones_sub_ = nh_.subscribe(TOPIC_COMMUNICATIONS_DDS_ZONES, - sub_queue_size_, - &Executive::ZonesCallback, - this); + camera_state_sub_ = FF_CREATE_SUBSCRIBER(nh_, + ff_msgs::msg::CameraStatesStamped, + TOPIC_MANAGEMENT_CAMERA_STATE, + sub_queue_size_, + std::bind(&Executive::CameraStatesCallback, this, std::placeholders::_1)); + + cmd_sub_ = FF_CREATE_SUBSCRIBER(nh_, + ff_msgs::msg::CommandStamped, + TOPIC_COMMAND, + sub_queue_size_, + std::bind(&Executive::CmdCallback, this, std::placeholders::_1)); + + data_sub_ = FF_CREATE_SUBSCRIBER(nh_, + ff_msgs::msg::CompressedFile, + TOPIC_COMMUNICATIONS_DDS_DATA, + sub_queue_size_, + std::bind(&Executive::DataToDiskCallback, this, std::placeholders::_1)); + + dock_state_sub_ = FF_CREATE_SUBSCRIBER(nh_, + ff_msgs::msg::DockState, + TOPIC_BEHAVIORS_DOCKING_STATE, + sub_queue_size_, + std::bind(&Executive::DockStateCallback, this, std::placeholders::_1)); + + fault_state_sub_ = FF_CREATE_SUBSCRIBER(nh_, + ff_msgs::msg::FaultState, + TOPIC_MANAGEMENT_SYS_MONITOR_STATE, + sub_queue_size_, + std::bind(&Executive::FaultStateCallback, this, std::placeholders::_1)); + + gs_ack_sub_ = FF_CREATE_SUBSCRIBER(nh_, + ff_msgs::msg::AckStamped, + TOPIC_GUEST_SCIENCE_MANAGER_ACK, + sub_queue_size_, + std::bind(&Executive::GuestScienceAckCallback, this, + std::placeholders::_1)); + + gs_config_sub_ = FF_CREATE_SUBSCRIBER(nh_, + ff_msgs::msg::GuestScienceConfig, + TOPIC_GUEST_SCIENCE_MANAGER_CONFIG, + sub_queue_size_, + std::bind(&Executive::GuestScienceConfigCallback, this, + std::placeholders::_1)); + + gs_state_sub_ = FF_CREATE_SUBSCRIBER(nh_, + ff_msgs::msg::GuestScienceState, + TOPIC_GUEST_SCIENCE_MANAGER_STATE, + sub_queue_size_, + std::bind(&Executive::GuestScienceStateCallback, this, + std::placeholders::_1)); + + heartbeat_sub_ = + FF_CREATE_SUBSCRIBER(nh_, + ff_msgs::msg::Heartbeat, + TOPIC_MANAGEMENT_SYS_MONITOR_HEARTBEAT, + sub_queue_size_, + std::bind(&Executive::SysMonitorHeartbeatCallback, this, std::placeholders::_1)); + + inertia_sub_ = FF_CREATE_SUBSCRIBER(nh_, + geometry_msgs::msg::InertiaStamped, + TOPIC_MOBILITY_INERTIA, + sub_queue_size_, + std::bind(&Executive::InertiaCallback, this, std::placeholders::_1)); + + motion_sub_ = FF_CREATE_SUBSCRIBER(nh_, + ff_msgs::msg::MotionState, + TOPIC_MOBILITY_MOTION_STATE, + sub_queue_size_, + std::bind(&Executive::MotionStateCallback, this, std::placeholders::_1)); + + perch_state_sub_ = FF_CREATE_SUBSCRIBER(nh_, + ff_msgs::msg::PerchState, + TOPIC_BEHAVIORS_PERCHING_STATE, + sub_queue_size_, + std::bind(&Executive::PerchStateCallback, this, std::placeholders::_1)); + + plan_sub_ = FF_CREATE_SUBSCRIBER(nh_, + ff_msgs::msg::CompressedFile, + TOPIC_COMMUNICATIONS_DDS_PLAN, + sub_queue_size_, + std::bind(&Executive::PlanCallback, this, std::placeholders::_1)); + + zones_sub_ = FF_CREATE_SUBSCRIBER(nh_, + ff_msgs::msg::CompressedFile, + TOPIC_COMMUNICATIONS_DDS_ZONES, + sub_queue_size_, + std::bind(&Executive::ZonesCallback, this, std::placeholders::_1)); // initialize pubs - agent_state_pub_ = nh_.advertise( - TOPIC_MANAGEMENT_EXEC_AGENT_STATE, - pub_queue_size_, - true); - - cf_ack_pub_ = nh_.advertise( - TOPIC_MANAGEMENT_EXEC_CF_ACK, - pub_queue_size_, - false); - - cmd_ack_pub_ = nh_.advertise(TOPIC_MANAGEMENT_ACK, - pub_queue_size_, - false); - - gs_cmd_pub_ = nh_.advertise( - TOPIC_MANAGEMENT_EXEC_COMMAND, - pub_queue_size_, - false); - - plan_pub_ = nh_.advertise(TOPIC_MANAGEMENT_EXEC_PLAN, - pub_queue_size_, - false); - - plan_status_pub_ = nh_.advertise( - TOPIC_MANAGEMENT_EXEC_PLAN_STATUS, - pub_queue_size_, - true); + agent_state_pub_ = FF_CREATE_PUBLISHER(nh_, + ff_msgs::msg::AgentStateStamped, + TOPIC_MANAGEMENT_EXEC_AGENT_STATE, + pub_queue_size_); + + cf_ack_pub_ = FF_CREATE_PUBLISHER(nh_, + ff_msgs::msg::CompressedFileAck, + TOPIC_MANAGEMENT_EXEC_CF_ACK, + pub_queue_size_); + + cmd_ack_pub_ = FF_CREATE_PUBLISHER(nh_, + ff_msgs::msg::AckStamped, + TOPIC_MANAGEMENT_ACK, + pub_queue_size_); + + gs_cmd_pub_ = FF_CREATE_PUBLISHER(nh_, + ff_msgs::msg::CommandStamped, + TOPIC_MANAGEMENT_EXEC_COMMAND, + pub_queue_size_); + + plan_pub_ = FF_CREATE_PUBLISHER(nh_, + ff_msgs::msg::CompressedFile, + TOPIC_MANAGEMENT_EXEC_PLAN, + pub_queue_size_); + + plan_status_pub_ = FF_CREATE_PUBLISHER(nh_, + ff_msgs::msg::PlanStatusStamped, + TOPIC_MANAGEMENT_EXEC_PLAN_STATUS, + pub_queue_size_); // initialize services - zones_client_ = nh_.serviceClient( - SERVICE_MOBILITY_SET_ZONES); + zones_client_.Create(nh_, SERVICE_MOBILITY_SET_ZONES); - laser_enable_client_ = nh_.serviceClient( - SERVICE_HARDWARE_LASER_ENABLE); + laser_enable_client_.Create(nh_, SERVICE_HARDWARE_LASER_ENABLE); - payload_power_client_ = nh_.serviceClient( - SERVICE_HARDWARE_EPS_CONF_PAYLOAD_POWER); + payload_power_client_.Create(nh_, SERVICE_HARDWARE_EPS_CONF_PAYLOAD_POWER); - pmc_enable_client_ = nh_.serviceClient( - SERVICE_HARDWARE_EPS_ENABLE_PMCS); + pmc_enable_client_.Create(nh_, SERVICE_HARDWARE_EPS_ENABLE_PMCS); - front_flashlight_client_ = nh_.serviceClient( - SERVICE_HARDWARE_LIGHT_FRONT_CONTROL); + front_flashlight_client_.Create(nh_, SERVICE_HARDWARE_LIGHT_FRONT_CONTROL); - back_flashlight_client_ = nh_.serviceClient( - SERVICE_HARDWARE_LIGHT_AFT_CONTROL); + back_flashlight_client_.Create(nh_, SERVICE_HARDWARE_LIGHT_AFT_CONTROL); - dock_cam_config_client_ = nh_.serviceClient( + dock_cam_config_client_.Create(nh_, SERVICE_MANAGEMENT_IMG_SAMPLER_CONFIG_DOCK); - dock_cam_enable_client_ = nh_.serviceClient( + dock_cam_enable_client_.Create(nh_, SERVICE_MANAGEMENT_IMG_SAMPLER_ENABLE_DOCK); - haz_cam_config_client_ = nh_.serviceClient( - SERVICE_MANAGEMENT_IMG_SAMPLER_CONFIG_HAZ); + haz_cam_config_client_.Create(nh_, SERVICE_MANAGEMENT_IMG_SAMPLER_CONFIG_HAZ); - haz_cam_enable_client_ = nh_.serviceClient( - SERVICE_MANAGEMENT_IMG_SAMPLER_ENABLE_HAZ); + haz_cam_enable_client_.Create(nh_, SERVICE_MANAGEMENT_IMG_SAMPLER_ENABLE_HAZ); - nav_cam_config_client_ = nh_.serviceClient( - SERVICE_MANAGEMENT_IMG_SAMPLER_CONFIG_NAV); + nav_cam_config_client_.Create(nh_, SERVICE_MANAGEMENT_IMG_SAMPLER_CONFIG_NAV); - nav_cam_enable_client_ = nh_.serviceClient( - SERVICE_MANAGEMENT_IMG_SAMPLER_ENABLE_NAV); + nav_cam_enable_client_.Create(nh_, SERVICE_MANAGEMENT_IMG_SAMPLER_ENABLE_NAV); - perch_cam_config_client_ = nh_.serviceClient( + perch_cam_config_client_.Create(nh_, SERVICE_MANAGEMENT_IMG_SAMPLER_CONFIG_PERCH); - perch_cam_enable_client_ = nh_.serviceClient( + perch_cam_enable_client_.Create(nh_, SERVICE_MANAGEMENT_IMG_SAMPLER_ENABLE_PERCH); - sci_cam_config_client_ = nh_.serviceClient( - SERVICE_MANAGEMENT_SCI_CAM_CONFIG); + sci_cam_config_client_.Create(nh_, SERVICE_MANAGEMENT_SCI_CAM_CONFIG); - sci_cam_enable_client_ = nh_.serviceClient( - SERVICE_MANAGEMENT_SCI_CAM_ENABLE); + sci_cam_enable_client_.Create(nh_, SERVICE_MANAGEMENT_SCI_CAM_ENABLE); - set_inertia_client_ = nh_.serviceClient( - SERVICE_MOBILITY_SET_INERTIA); + set_inertia_client_.Create(nh_, SERVICE_MOBILITY_SET_INERTIA); - set_rate_client_ = nh_.serviceClient( - SERVICE_COMMUNICATIONS_DDS_SET_TELEM_RATES); + set_rate_client_.Create(nh_, SERVICE_COMMUNICATIONS_DDS_SET_TELEM_RATES); - set_data_client_ = nh_.serviceClient( - SERVICE_MANAGEMENT_DATA_BAGGER_SET_DATA_TO_DISK); + set_data_client_.Create(nh_, SERVICE_MANAGEMENT_DATA_BAGGER_SET_DATA_TO_DISK); - enable_recording_client_ = nh_.serviceClient( + enable_recording_client_.Create(nh_, SERVICE_MANAGEMENT_DATA_BAGGER_ENABLE_RECORDING); - eps_terminate_client_ = nh_.serviceClient( - SERVICE_HARDWARE_EPS_CLEAR_TERMINATE); + eps_terminate_client_.Create(nh_, SERVICE_HARDWARE_EPS_CLEAR_TERMINATE); - enable_astrobee_intercommunication_client_ = - nh_.serviceClient( + enable_astrobee_intercommunication_client_.Create(nh_, SERVICE_COMMUNICATIONS_ENABLE_ASTROBEE_INTERCOMMS); - unload_load_nodelet_client_ = nh_.serviceClient( + unload_load_nodelet_client_.Create(nh_, SERVICE_MANAGEMENT_SYS_MONITOR_UNLOAD_LOAD_NODELET); - set_collision_distance_client_ = nh_.serviceClient( - SERVICE_MOBILITY_SET_COLLISION_DISTANCE); + set_collision_distance_client_.Create(nh_, + SERVICE_MOBILITY_SET_COLLISION_DISTANCE); // initialize configure clients later, when initialized here, the service is // invalid when we try to use it. Must have something to do with startup order // of executive, choreographer, planner, or mapper // initialize agent state - agent_state_.operating_state.state = ff_msgs::OpState::READY; - SetPlanExecState(ff_msgs::ExecState::IDLE); - agent_state_.mobility_state.state = ff_msgs::MobilityState::DRIFTING; + agent_state_.operating_state.state = ff_msgs::msg::OpState::READY; + SetPlanExecState(ff_msgs::msg::ExecState::IDLE); + agent_state_.mobility_state.state = ff_msgs::msg::MobilityState::DRIFTING; agent_state_.mobility_state.sub_state = 0; - agent_state_.guest_science_state.state = ff_msgs::ExecState::IDLE; + agent_state_.guest_science_state.state = ff_msgs::msg::ExecState::IDLE; agent_state_.proximity = 0; agent_state_.profile_name = ""; agent_state_.flight_mode = "nominal"; // Get nominal limits - ff_msgs::FlightMode flight_mode; + ff_msgs::msg::FlightMode flight_mode; if (!ff_util::FlightUtil::GetFlightMode(flight_mode, "nominal")) { err_msg = "Couldn't get flight mode nominal."; - NODELET_ERROR("%s", err_msg.c_str()); - this->AssertFault(ff_util::INITIALIZATION_FAILED, err_msg); + FF_ERROR("%s", err_msg.c_str()); + this->AssertFault(ff_util::INITIALIZATION_FAILED, err_msg, GetTimeNow()); return; } else { agent_state_.target_linear_velocity = flight_mode.hard_limit_vel; @@ -3811,77 +3918,74 @@ void Executive::Initialize(ros::NodeHandle *nh) { agent_state_.holonomic_enabled = false; agent_state_.check_obstacles = true; - agent_state_.check_zones = true; + agent_state_.check_zones = false; // true; // ANA DEBUG: JPS3D is crashing in the Validator, deactivating for now agent_state_.auto_return_enabled = true; agent_state_.immediate_enabled = true; agent_state_.replanning_enabled = false; - agent_state_.boot_time = ros::Time::now().sec; + agent_state_.boot_time = GetTimeNow().seconds(); PublishAgentState(); // Publish blank plan status so that the GDS displays the correct plan info - ff_msgs::PlanStatusStamped plan_status; - plan_status.header.stamp = ros::Time::now(); + ff_msgs::msg::PlanStatusStamped plan_status; + plan_status.header.stamp = GetTimeNow(); plan_status.name = ""; plan_status.command = -1; - plan_status_pub_.publish(plan_status); + plan_status_pub_->publish(plan_status); // Initialize camera states vector. All we care about is if we are streaming - camera_states_.states.resize(3); - camera_states_.states[0].camera_name = "nav_cam"; - camera_states_.states[0].streaming = false; - camera_states_.states[1].camera_name = "dock_cam"; - camera_states_.states[1].streaming = false; - camera_states_.states[2].camera_name = "sci_cam"; - camera_states_.states[2].streaming = false; + camera_states_->states.resize(3); + camera_states_->states[0].camera_name = "nav_cam"; + camera_states_->states[0].streaming = false; + camera_states_->states[1].camera_name = "dock_cam"; + camera_states_->states[1].streaming = false; + camera_states_->states[2].camera_name = "sci_cam"; + camera_states_->states[2].streaming = false; // Create timer for wait command with a dummy duration since it will be // changed everytime it is started. Make it one shot and don't start until // wait command received - wait_timer_ = nh_.createTimer(ros::Duration(1), - &Executive::WaitCallback, - this, - true, - false); + wait_timer_.createTimer(1.0, + std::bind(&Executive::WaitCallback, this), + nh_, + true, + false); // Create timer for monitoring the system monitor heartbeat. Don't start it // until we receive the first heartbeat from the system monitor - sys_monitor_heartbeat_timer_ = nh_.createTimer( - ros::Duration(sys_monitor_heartbeat_timeout_), - &Executive::SysMonitorTimeoutCallback, - this, - false, - false); + sys_monitor_heartbeat_timer_.createTimer(sys_monitor_heartbeat_timeout_, + std::bind(&Executive::SysMonitorTimeoutCallback, this), + nh_, + false, + false); // Create timer to make sure the system monitor was started - sys_monitor_startup_timer_ = nh_.createTimer( - ros::Duration(sys_monitor_startup_time_secs_), - &Executive::SysMonitorTimeoutCallback, - this, - true, - true); + sys_monitor_startup_timer_.createTimer(sys_monitor_startup_time_secs_, + std::bind(&Executive::SysMonitorTimeoutCallback, this), + nh_, + true, + true); // Create timer for guest science start and stop command timeout. If the guest // science manager doesn't respond to a start or stop guest science command // in the time specified, we need to ack command as failed. Make it one shot // and don't start until we send a guest science start or stop command - gs_start_stop_restart_command_timer_ = nh_.createTimer( - ros::Duration(gs_command_timeout_), - &Executive::GuestScienceStartStopRestartCmdTimeoutCallback, - this, - true, - false); + gs_start_stop_restart_command_timer_.createTimer(gs_command_timeout_, + std::bind(&Executive::GuestScienceStartStopRestartCmdTimeoutCallback, this), + nh_, + true, + false); // Create timer for guest science custom command timeout. If the guest science // manager doesn't respond to a custom guest science command in the time // specified, we need to ack command as failed. Make it one shot and don't // start until we send a guest science custom command - gs_custom_command_timer_ = nh_.createTimer(ros::Duration(gs_command_timeout_), - &Executive::GuestScienceCustomCmdTimeoutCallback, - this, - true, - false); + gs_custom_command_timer_.createTimer(gs_command_timeout_, + std::bind(&Executive::GuestScienceCustomCmdTimeoutCallback, this), + nh_, + true, + false); // Initialize the led service at the end of the initialize function as this // will turn off the booting up light and we only want to do this when the @@ -3897,94 +4001,94 @@ bool Executive::ReadParams() { // Read config files into lua if (!config_params_.ReadFiles()) { err_msg = "Error loading executive parameters. Couldn't read config files."; - NODELET_ERROR("%s", err_msg.c_str()); - this->AssertFault(ff_util::INITIALIZATION_FAILED, err_msg); + FF_ERROR("%s", err_msg.c_str()); + this->AssertFault(ff_util::INITIALIZATION_FAILED, err_msg, GetTimeNow()); return false; } // get world name if (!config_params_.GetStr("world_name", &agent_state_.world)) { - NODELET_WARN("Unable to get world name."); + FF_WARN("Unable to get world name."); agent_state_.world = "none"; } // get action active timeout if (!config_params_.GetPosReal("action_active_timeout", &action_active_timeout_)) { - NODELET_WARN("Action active timeout not specified."); + FF_WARN("Action active timeout not specified."); action_active_timeout_ = 1; } // get led service available timeout if (!config_params_.GetPosReal("led_service_available_timeout", &led_connected_timeout_)) { - NODELET_WARN("Led service available timeout not specified."); + FF_WARN("Led service available timeout not specified."); led_connected_timeout_ = 10; } // get gs manager timeout if (!config_params_.GetPosReal("gs_command_timeout", &gs_command_timeout_)) { - NODELET_WARN("Guest science command timeout not specified."); + FF_WARN("Guest science command timeout not specified."); gs_command_timeout_ = 4; } // get action feedback timeouts if (!config_params_.GetPosReal("motion_feedback_timeout", &motion_feedback_timeout_)) { - NODELET_WARN("Motion feedback timeout not specified."); + FF_WARN("Motion feedback timeout not specified."); motion_feedback_timeout_ = 1; } if (!config_params_.GetPosReal("arm_feedback_timeout", &arm_feedback_timeout_)) { - NODELET_WARN("Arm feedback timeout not specified."); + FF_WARN("Arm feedback timeout not specified."); arm_feedback_timeout_ = 4; } // get action results timeouts if (!config_params_.GetPosReal("dock_result_timeout", &dock_result_timeout_)) { - NODELET_WARN("Dock result timeout not specified."); + FF_WARN("Dock result timeout not specified."); dock_result_timeout_ = 360; } if (!config_params_.GetPosReal("perch_result_timeout", &perch_result_timeout_)) { - NODELET_WARN("Perch result timeout not specified."); + FF_WARN("Perch result timeout not specified."); perch_result_timeout_ = 360; } if (!config_params_.GetPosReal("localization_result_timeout", &localization_result_timeout_)) { - NODELET_WARN("Localization result timeout not specified."); + FF_WARN("Localization result timeout not specified."); localization_result_timeout_ = 10; } if (!config_params_.GetPosReal("sys_monitor_startup_time_secs", &sys_monitor_startup_time_secs_)) { - NODELET_WARN("System monitor startup time not specified."); + FF_WARN("System monitor startup time not specified."); sys_monitor_startup_time_secs_ = 30; } // get planner if (!config_params_.GetStr("planner", &agent_state_.planner)) { - NODELET_WARN("System monitor planner not specified."); + FF_WARN("System monitor planner not specified."); agent_state_.planner = "QuadraticProgram"; } if (!config_params_.GetPosReal("sys_monitor_heartbeat_timeout", &sys_monitor_heartbeat_timeout_)) { err_msg = "System monitor heartbeat timeout not specified."; - NODELET_ERROR("%s", err_msg.c_str()); - this->AssertFault(ff_util::INITIALIZATION_FAILED, err_msg); + FF_ERROR("%s", err_msg.c_str()); + this->AssertFault(ff_util::INITIALIZATION_FAILED, err_msg, GetTimeNow()); return false; } if (!config_params_.CheckValExists("sys_monitor_heartbeat_fault_response")) { err_msg = "Sys monitor heartbeat fault response not specified."; - NODELET_ERROR("%s", err_msg.c_str()); - this->AssertFault(ff_util::INITIALIZATION_FAILED, err_msg); + FF_ERROR("%s", err_msg.c_str()); + this->AssertFault(ff_util::INITIALIZATION_FAILED, err_msg, GetTimeNow()); return false; } @@ -3993,23 +4097,23 @@ bool Executive::ReadParams() { if (!ReadCommand(&hb_response, sys_monitor_heartbeat_fault_response_)) { err_msg = "Unable to read sys monitor heartbeat fault response."; - NODELET_ERROR("%s", err_msg.c_str()); - this->AssertFault(ff_util::INITIALIZATION_FAILED, err_msg); + FF_ERROR("%s", err_msg.c_str()); + this->AssertFault(ff_util::INITIALIZATION_FAILED, err_msg, GetTimeNow()); return false; } if (!config_params_.GetBool("sys_monitor_heartbeat_fault_blocking", &sys_monitor_heartbeat_fault_blocking_)) { err_msg == "Sys monitor heartbeat fault blocking not specified."; - NODELET_ERROR("%s", err_msg.c_str()); - this->AssertFault(ff_util::INITIALIZATION_FAILED, err_msg); + FF_ERROR("%s", err_msg.c_str()); + this->AssertFault(ff_util::INITIALIZATION_FAILED, err_msg, GetTimeNow()); return false; } if (!config_params_.CheckValExists("sys_monitor_init_fault_response")) { err_msg = "System monitor init fault response not specified."; - NODELET_ERROR("%s", err_msg.c_str()); - this->AssertFault(ff_util::INITIALIZATION_FAILED, err_msg); + FF_ERROR("%s", err_msg.c_str()); + this->AssertFault(ff_util::INITIALIZATION_FAILED, err_msg, GetTimeNow()); return false; } @@ -4018,16 +4122,16 @@ bool Executive::ReadParams() { if (!ReadCommand(&init_response, sys_monitor_init_fault_response_)) { err_msg = "Unable to read sys monitor init fault response."; - NODELET_ERROR("%s", err_msg.c_str()); - this->AssertFault(ff_util::INITIALIZATION_FAILED, err_msg); + FF_ERROR("%s", err_msg.c_str()); + this->AssertFault(ff_util::INITIALIZATION_FAILED, err_msg, GetTimeNow()); return false; } if (!config_params_.GetBool("sys_monitor_init_fault_blocking", &sys_monitor_init_fault_blocking_)) { err_msg = "Sys monitor init fault blocking not specified."; - NODELET_ERROR("%s", err_msg.c_str()); - this->AssertFault(ff_util::INITIALIZATION_FAILED, err_msg); + FF_ERROR("%s", err_msg.c_str()); + this->AssertFault(ff_util::INITIALIZATION_FAILED, err_msg, GetTimeNow()); return false; } @@ -4040,8 +4144,8 @@ bool Executive::ReadMapperParams() { if (!mapper_config_params_.ReadFiles()) { err_msg = "Error loading executive parameters."; err_msg += "Couldn't read mapper config files."; - NODELET_ERROR("%s", err_msg.c_str()); - this->AssertFault(ff_util::INITIALIZATION_FAILED, err_msg); + FF_ERROR("%s", err_msg.c_str()); + this->AssertFault(ff_util::INITIALIZATION_FAILED, err_msg, GetTimeNow()); return false; } @@ -4050,20 +4154,20 @@ bool Executive::ReadMapperParams() { double collision_distance = -1; if (!mapper_config_params_.GetTable("parameters", &mapper_params_table)) { err_msg = "Unable to read mapper parameters table."; - NODELET_ERROR("%s", err_msg.c_str()); - this->AssertFault(ff_util::INITIALIZATION_FAILED, err_msg); + FF_ERROR("%s", err_msg.c_str()); + this->AssertFault(ff_util::INITIALIZATION_FAILED, err_msg, GetTimeNow()); return false; } // Need to search for the collision distance in the mapper parameters for (int i = 1; i <= mapper_params_table.GetSize(); i++) { if (!mapper_params_table.GetTable(i, &mapper_group)) { - NODELET_ERROR("Could not read the mapper parameter table row %i", i); + FF_ERROR("Could not read the mapper parameter table row %i", i); continue; } if (!mapper_group.GetStr("id", &id)) { - NODELET_ERROR("Could not read mapper id for row %i", i); + FF_ERROR("Could not read mapper id for row %i", i); continue; } @@ -4072,8 +4176,8 @@ bool Executive::ReadMapperParams() { // Only need the default value for initialization if (!mapper_group.GetReal("default", &collision_distance)) { err_msg = "Unable to read collision distance from mapper config"; - NODELET_ERROR("%s", err_msg.c_str()); - this->AssertFault(ff_util::INITIALIZATION_FAILED, err_msg); + FF_ERROR("%s", err_msg.c_str()); + this->AssertFault(ff_util::INITIALIZATION_FAILED, err_msg, GetTimeNow()); return false; } // Stop searching for the collision distance @@ -4086,8 +4190,8 @@ bool Executive::ReadMapperParams() { agent_state_.collision_distance = collision_distance; } else { err_msg = "Unable to find the collision distance from the mapper config."; - NODELET_ERROR("%s", err_msg.c_str()); - this->AssertFault(ff_util::INITIALIZATION_FAILED, err_msg); + FF_ERROR("%s", err_msg.c_str()); + this->AssertFault(ff_util::INITIALIZATION_FAILED, err_msg, GetTimeNow()); return false; } @@ -4095,10 +4199,10 @@ bool Executive::ReadMapperParams() { } bool Executive::ReadCommand(config_reader::ConfigReader::Table *response, - ff_msgs::CommandStampedPtr cmd) { + ff_msgs::msg::CommandStamped::SharedPtr cmd) { std::string cmd_name; if (!response->GetStr("name", &cmd_name)) { - NODELET_ERROR("Fault response command name not specified."); + FF_ERROR("Fault response command name not specified."); return false; } @@ -4117,102 +4221,105 @@ bool Executive::ReadCommand(config_reader::ConfigReader::Table *response, config_reader::ConfigReader::Table arg(&args, (i + 1)); // First element in table is the type if (!arg.GetUInt(1, &type)) { - NODELET_ERROR("First command argument value is not a uint"); + FF_ERROR("First command argument value is not a uint"); return false; } // Remaining elements are the parameter values switch (type) { - case ff_msgs::CommandArg::DATA_TYPE_BOOL: + case ff_msgs::msg::CommandArg::DATA_TYPE_BOOL: { bool val; if (!arg.GetBool(2, &val)) { - NODELET_ERROR("Expected command argument to be a bool!"); + FF_ERROR("Expected command argument to be a bool!"); return false; } - cmd->args[i].data_type = ff_msgs::CommandArg::DATA_TYPE_BOOL; + cmd->args[i].data_type = ff_msgs::msg::CommandArg::DATA_TYPE_BOOL; cmd->args[i].b = val; } break; - case ff_msgs::CommandArg::DATA_TYPE_DOUBLE: + case ff_msgs::msg::CommandArg::DATA_TYPE_DOUBLE: { double val; if (!arg.GetReal(2, &val)) { - NODELET_ERROR("Expected command argument to be a double"); + FF_ERROR("Expected command argument to be a double"); return false; } - cmd->args[i].data_type = ff_msgs::CommandArg::DATA_TYPE_DOUBLE; + cmd->args[i].data_type = + ff_msgs::msg::CommandArg::DATA_TYPE_DOUBLE; cmd->args[i].d = val; } break; - case ff_msgs::CommandArg::DATA_TYPE_FLOAT: + case ff_msgs::msg::CommandArg::DATA_TYPE_FLOAT: { float val; if (!arg.GetReal(2, &val)) { - NODELET_ERROR("Expected command argument to be a float."); + FF_ERROR("Expected command argument to be a float."); return false; } - cmd->args[i].data_type = ff_msgs::CommandArg::DATA_TYPE_FLOAT; + cmd->args[i].data_type = ff_msgs::msg::CommandArg::DATA_TYPE_FLOAT; cmd->args[i].f = val; } break; - case ff_msgs::CommandArg::DATA_TYPE_INT: + case ff_msgs::msg::CommandArg::DATA_TYPE_INT: { int val; if (!arg.GetInt(2, &val)) { - NODELET_ERROR("Expected command argument to be an int."); + FF_ERROR("Expected command argument to be an int."); return false; } - cmd->args[i].data_type = ff_msgs::CommandArg::DATA_TYPE_INT; + cmd->args[i].data_type = ff_msgs::msg::CommandArg::DATA_TYPE_INT; cmd->args[i].i = val; } break; - case ff_msgs::CommandArg::DATA_TYPE_LONGLONG: + case ff_msgs::msg::CommandArg::DATA_TYPE_LONGLONG: { int64_t val; if (!arg.GetLongLong(2, &val)) { - NODELET_ERROR("Expected command argument to be an int."); + FF_ERROR("Expected command argument to be an int."); return false; } - cmd->args[i].data_type = ff_msgs::CommandArg::DATA_TYPE_LONGLONG; + cmd->args[i].data_type = + ff_msgs::msg::CommandArg::DATA_TYPE_LONGLONG; cmd->args[i].ll = val; } break; - case ff_msgs::CommandArg::DATA_TYPE_STRING: + case ff_msgs::msg::CommandArg::DATA_TYPE_STRING: { std::string val; if (!arg.GetStr(2, &val)) { - NODELET_ERROR("Expected command argument to be a string"); + FF_ERROR("Expected command argument to be a string"); return false; } - cmd->args[i].data_type = ff_msgs::CommandArg::DATA_TYPE_STRING; + cmd->args[i].data_type = + ff_msgs::msg::CommandArg::DATA_TYPE_STRING; cmd->args[i].s = val; } break; - case ff_msgs::CommandArg::DATA_TYPE_VEC3d: + case ff_msgs::msg::CommandArg::DATA_TYPE_VEC3D: { int j; double val; - cmd->args[i].data_type = ff_msgs::CommandArg::DATA_TYPE_VEC3d; + cmd->args[i].data_type = ff_msgs::msg::CommandArg::DATA_TYPE_VEC3D; for (j = 0; j < 3; ++j) { // Index to get vector values in table starts at 2 if (!arg.GetReal((j + 2), &val)) { - NODELET_ERROR("Expected command argument to be double."); + FF_ERROR("Expected command argument to be double."); return false; } cmd->args[i].vec3d[j] = val; } } break; - case ff_msgs::CommandArg::DATA_TYPE_MAT33f: + case ff_msgs::msg::CommandArg::DATA_TYPE_MAT33F: { int j; float val; - cmd->args[i].data_type = ff_msgs::CommandArg::DATA_TYPE_MAT33f; + cmd->args[i].data_type = ff_msgs::msg::CommandArg::DATA_TYPE_MAT33F; for (j = 0; j < 9; ++j) { // Index in get matrix values in table starts at 2 if (!arg.GetReal((j + 2), &val)) { - NODELET_ERROR("Expected command argument to be a float."); + FF_ERROR("Expected command argument to be a float."); return false; } cmd->args[i].mat33f[j] = val; @@ -4220,7 +4327,7 @@ bool Executive::ReadCommand(config_reader::ConfigReader::Table *response, } break; default: - NODELET_ERROR("Type for command argument unrecognized!"); + FF_ERROR("Type for command argument unrecognized!"); return false; } } @@ -4230,10 +4337,12 @@ bool Executive::ReadCommand(config_reader::ConfigReader::Table *response, } void Executive::PublishAgentState() { - agent_state_.header.stamp = ros::Time::now(); - agent_state_pub_.publish(agent_state_); + agent_state_.header.stamp = GetTimeNow(); + agent_state_pub_->publish(agent_state_); } } // namespace executive -PLUGINLIB_EXPORT_CLASS(executive::Executive, nodelet::Nodelet) +#include "rclcpp_components/register_node_macro.hpp" + +RCLCPP_COMPONENTS_REGISTER_NODE(executive::Executive) diff --git a/management/executive/src/op_state.cc b/management/executive/src/op_state.cc index d74e5277f6..ebf5c74297 100644 --- a/management/executive/src/op_state.cc +++ b/management/executive/src/op_state.cc @@ -40,13 +40,13 @@ void OpState::SetExec(Executive *const exec) { } } -OpState* OpState::HandleCmd(ff_msgs::CommandStampedPtr const& cmd) { - ROS_ERROR("Executive: Handle command not implemented yet!"); +OpState* OpState::HandleCmd(ff_msgs::msg::CommandStamped::SharedPtr const cmd) { + exec_->Error("Executive: Handle command not implemented yet!"); return this; } // Handles commands that are excepted in every op state -OpState* OpState::HandleCmd(ff_msgs::CommandStampedPtr const& cmd, +OpState* OpState::HandleCmd(ff_msgs::msg::CommandStamped::SharedPtr const cmd, bool& completed, bool& successful) { completed = true; @@ -86,18 +86,18 @@ OpState* OpState::HandleResult(ff_util::FreeFlyerActionState::Enum const& state, std::string const& result_response, std::string const& cmd_id, Action const& action) { - ROS_ERROR("Executive: Handle action result not implemented yet!"); + exec_->Error("Executive: Handle action result not implemented yet!"); return this; } OpState* OpState::HandleWaitCallback() { - ROS_ERROR("Executive: Handle wait callback not implemented yet!"); + exec_->Error("Executive: Handle wait callback not implemented yet!"); return this; } -OpState* OpState::HandleGuestScienceAck(ff_msgs::AckStampedConstPtr const& - ack) { - ROS_ERROR("Executive: Handle guest science ack not implemented yet!"); +OpState* OpState::HandleGuestScienceAck( + ff_msgs::msg::AckStamped::SharedPtr const ack) { + exec_->Error("Executive: Handle guest science ack not implemented yet!"); return this; } @@ -184,52 +184,52 @@ std::string OpState::GetActionString(Action const& action) { action_str = "Unperch"; break; default: - ROS_ERROR("Executive: Action unknown or wrong in action result."); + exec_->Error("Executive: Action unknown or wrong in action result."); } return action_str; } -bool OpState::PausePlan(ff_msgs::CommandStampedPtr const& cmd) { +bool OpState::PausePlan(ff_msgs::msg::CommandStamped::SharedPtr const cmd) { AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, ("Pause plan not accepted in opstate " + name() + "!")); return false; } OpState* OpState::TransitionToState(unsigned char id) { - if (id == ff_msgs::OpState::READY) { + if (id == ff_msgs::msg::OpState::READY) { return OpStateRepo::Instance()->ready()->StartupState(); - } else if (id == ff_msgs::OpState::PLAN_EXECUTION) { + } else if (id == ff_msgs::msg::OpState::PLAN_EXECUTION) { return OpStateRepo::Instance()->plan_exec()->StartupState(); - } else if (id == ff_msgs::OpState::TELEOPERATION) { + } else if (id == ff_msgs::msg::OpState::TELEOPERATION) { return OpStateRepo::Instance()->teleop()->StartupState(); - } else if (id == ff_msgs::OpState::AUTO_RETURN) { + } else if (id == ff_msgs::msg::OpState::AUTO_RETURN) { return OpStateRepo::Instance()->auto_return()->StartupState(); - } else if (id == ff_msgs::OpState::FAULT) { + } else if (id == ff_msgs::msg::OpState::FAULT) { return OpStateRepo::Instance()->fault()->StartupState(); } - ROS_WARN("Executive: unknown state id in transition to state."); + exec_->Warn("Executive: unknown state id in transition to state."); return this; } void OpState::SetPlanStatus(bool successful, std::string err_msg) { - exec_->SetPlanExecState(ff_msgs::ExecState::PAUSED); + exec_->SetPlanExecState(ff_msgs::msg::ExecState::PAUSED); if (successful) { // Ack run plan command as cancelled since we are pausing the plan until the // fault is cleared AckCmd(exec_->GetRunPlanCmdId(), - ff_msgs::AckCompletedStatus::CANCELED, + ff_msgs::msg::AckCompletedStatus::CANCELED, "Executive had to execute the fault command."); exec_->AckCurrentPlanItem(); - exec_->PublishPlanStatus(ff_msgs::AckStatus::QUEUED); + exec_->PublishPlanStatus(ff_msgs::msg::AckStatus::QUEUED); } else { err_msg.append(" Executive also received a fault!"); AckCmd(exec_->GetRunPlanCmdId(), - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, err_msg); - exec_->PublishPlanStatus(ff_msgs::AckStatus::REQUEUED); + exec_->PublishPlanStatus(ff_msgs::msg::AckStatus::REQUEUED); } } diff --git a/management/executive/src/op_state_auto_return.cc b/management/executive/src/op_state_auto_return.cc index 470cf4192e..3224c171e9 100644 --- a/management/executive/src/op_state_auto_return.cc +++ b/management/executive/src/op_state_auto_return.cc @@ -19,7 +19,8 @@ #include "executive/op_state_auto_return.h" namespace executive { -OpState* OpStateAutoReturn::HandleCmd(ff_msgs::CommandStampedPtr const& cmd) { +OpState* OpStateAutoReturn::HandleCmd( + ff_msgs::msg::CommandStamped::SharedPtr const cmd) { bool completed = false, successful = false; // Check if command is accepted in every op state and if so, execute it OpState::HandleCmd(cmd, completed, successful); @@ -62,7 +63,9 @@ OpState* OpStateAutoReturn::HandleCmd(ff_msgs::CommandStampedPtr const& cmd) { } else { std::string err_msg = "Command " + cmd->cmd_name + " not accepted in op state auto return."; - AckCmd(cmd->cmd_id, ff_msgs::AckCompletedStatus::EXEC_FAILED, err_msg); + AckCmd(cmd->cmd_id, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, + err_msg); } return this; } diff --git a/management/executive/src/op_state_fault.cc b/management/executive/src/op_state_fault.cc index 7548cc39d3..9706067184 100644 --- a/management/executive/src/op_state_fault.cc +++ b/management/executive/src/op_state_fault.cc @@ -20,7 +20,8 @@ namespace executive { -OpState* OpStateFault::HandleCmd(ff_msgs::CommandStampedPtr const& cmd) { +OpState* OpStateFault::HandleCmd( + ff_msgs::msg::CommandStamped::SharedPtr const cmd) { bool completed = false, successful = false; std::string err_msg; // Check if command is accepted in every op state and execute it if it is @@ -91,19 +92,22 @@ OpState* OpStateFault::HandleCmd(ff_msgs::CommandStampedPtr const& cmd) { } else { err_msg = "Command " + cmd->cmd_name + " not accepted in op state" + " fault."; - AckCmd(cmd->cmd_id, ff_msgs::AckCompletedStatus::EXEC_FAILED, err_msg); - ROS_WARN("Executive: %s", err_msg.c_str()); + AckCmd(cmd->cmd_id, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, + err_msg); + exec_->Warn(err_msg); } return this; } OpState* OpStateFault::HandleGuestScienceAck( - ff_msgs::AckStampedConstPtr const& ack) { + ff_msgs::msg::AckStamped::SharedPtr const ack) { // If the command is not part of a plan, it gets acked in the executive // If the command isn't done, don't do anything. - if (ack->completed_status.status == ff_msgs::AckCompletedStatus::NOT) { + if (ack->completed_status.status == ff_msgs::msg::AckCompletedStatus::NOT) { return this; - } else if (ack->completed_status.status != ff_msgs::AckCompletedStatus::OK) { + } else if (ack->completed_status.status != + ff_msgs::msg::AckCompletedStatus::OK) { SetPlanStatus(false, ack->message); } else { SetPlanStatus(true); @@ -138,11 +142,10 @@ OpState* OpStateFault::HandleResult( if (cmd_id == "plan") { SetPlanStatus(false, err_msg); } else { - AckCmd(cmd_id, ff_msgs::AckCompletedStatus::EXEC_FAILED, err_msg); + AckCmd(cmd_id, ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, err_msg); } } return this; } - } // namespace executive diff --git a/management/executive/src/op_state_plan_exec.cc b/management/executive/src/op_state_plan_exec.cc index 2f3e34685f..ed7dd3402a 100644 --- a/management/executive/src/op_state_plan_exec.cc +++ b/management/executive/src/op_state_plan_exec.cc @@ -37,8 +37,8 @@ OpState* OpStatePlanExec::StartupState(std::string const& cmd_id) { // plan may not be an action meaning the op state would be ready while // executing a plan which is no good exec_->SetOpState(this); - exec_->SetPlanExecState(ff_msgs::ExecState::EXECUTING); - exec_->PublishPlanStatus(ff_msgs::AckStatus::EXECUTING); + exec_->SetPlanExecState(ff_msgs::msg::ExecState::EXECUTING); + exec_->PublishPlanStatus(ff_msgs::msg::AckStatus::EXECUTING); // Don't need to check for empty plan since this was checked before the // transition to plan execution state but do need to check for invalid @@ -51,15 +51,16 @@ OpState* OpStatePlanExec::StartupState(std::string const& cmd_id) { // contained instantaneous commands only and we don't do anything. If the // plan execution state is executing, we know the first item wasn't // successful, so we need to pause the plan. - if (exec_->GetPlanExecState() == ff_msgs::AckStatus::EXECUTING) { - exec_->SetPlanExecState(ff_msgs::ExecState::PAUSED); - exec_->PublishPlanStatus(ff_msgs::AckStatus::QUEUED); + if (exec_->GetPlanExecState() == ff_msgs::msg::AckStatus::EXECUTING) { + exec_->SetPlanExecState(ff_msgs::msg::ExecState::PAUSED); + exec_->PublishPlanStatus(ff_msgs::msg::AckStatus::QUEUED); } } return temp_op_state; } -OpState* OpStatePlanExec::HandleCmd(ff_msgs::CommandStampedPtr const& cmd) { +OpState* OpStatePlanExec::HandleCmd( + ff_msgs::msg::CommandStamped::SharedPtr const cmd) { std::string err_msg; bool completed = false, successful = false; @@ -176,8 +177,8 @@ OpState* OpStatePlanExec::HandleCmd(ff_msgs::CommandStampedPtr const& cmd) { waiting_ = true; } else { err_msg = "Plan contains unknown command: " + cmd->cmd_name; - ROS_ERROR("%s", err_msg.c_str()); - AckPlanCmdFailed(ff_msgs::AckCompletedStatus::BAD_SYNTAX, err_msg); + exec_->Error(err_msg); + AckPlanCmdFailed(ff_msgs::msg::AckCompletedStatus::BAD_SYNTAX, err_msg); return OpStateRepo::Instance()->ready()->StartupState(); } } else { @@ -201,10 +202,10 @@ OpState* OpStatePlanExec::HandleCmd(ff_msgs::CommandStampedPtr const& cmd) { exec_->StopWaitTimer(); waiting_ = false; exec_->AckCurrentPlanItem(); - exec_->PublishPlanStatus(ff_msgs::AckStatus::QUEUED); + exec_->PublishPlanStatus(ff_msgs::msg::AckStatus::QUEUED); // Ack run plan command here since the current step has completed AckCmd(exec_->GetRunPlanCmdId(), - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, "Executive had to execute the fault command."); } @@ -212,18 +213,18 @@ OpState* OpStatePlanExec::HandleCmd(ff_msgs::CommandStampedPtr const& cmd) { } } else if (cmd->cmd_name == CommandConstants::CMD_NAME_IDLE_PROPULSION) { AckCmd(exec_->GetRunPlanCmdId(), - ff_msgs::AckCompletedStatus::CANCELED, + ff_msgs::msg::AckCompletedStatus::CANCELED, "Run plan command failed due to an idle propulsion command."); - exec_->SetPlanExecState(ff_msgs::ExecState::PAUSED); + exec_->SetPlanExecState(ff_msgs::msg::ExecState::PAUSED); if (waiting_) { exec_->StopWaitTimer(); waiting_ = false; exec_->AckCurrentPlanItem(); - exec_->PublishPlanStatus(ff_msgs::AckStatus::QUEUED); + exec_->PublishPlanStatus(ff_msgs::msg::AckStatus::QUEUED); } else { - exec_->PublishPlanStatus(ff_msgs::AckStatus::REQUEUED); + exec_->PublishPlanStatus(ff_msgs::msg::AckStatus::REQUEUED); } if (exec_->IdlePropulsion(cmd)) { @@ -251,9 +252,9 @@ OpState* OpStatePlanExec::HandleCmd(ff_msgs::CommandStampedPtr const& cmd) { if (exec_->IsActionRunning(ARM)) { // Stop Arm will cancel the arm action so just need to pause plan exec_->PublishCmdAck(exec_->GetRunPlanCmdId(), - ff_msgs::AckCompletedStatus::CANCELED); - exec_->SetPlanExecState(ff_msgs::ExecState::PAUSED); - exec_->PublishPlanStatus(ff_msgs::AckStatus::REQUEUED); + ff_msgs::msg::AckCompletedStatus::CANCELED); + exec_->SetPlanExecState(ff_msgs::msg::ExecState::PAUSED); + exec_->PublishPlanStatus(ff_msgs::msg::AckStatus::REQUEUED); if (exec_->StopArm(cmd)) { return OpStateRepo::Instance()->teleop()->StartupState(); } else { @@ -266,8 +267,10 @@ OpState* OpStatePlanExec::HandleCmd(ff_msgs::CommandStampedPtr const& cmd) { err_msg = "Command " + cmd->cmd_name + "not accepted in op state" + " plan execution."; // Don't stop plan, just send a failed ack - AckCmd(cmd->cmd_id, ff_msgs::AckCompletedStatus::EXEC_FAILED, err_msg); - ROS_WARN("Executive: %s", err_msg.c_str()); + AckCmd(cmd->cmd_id, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, + err_msg); + exec_->Warn(err_msg); } } return this; @@ -288,19 +291,20 @@ OpState* OpStatePlanExec::HandleWaitCallback() { } OpState* OpStatePlanExec::HandleGuestScienceAck( - ff_msgs::AckStampedConstPtr const& ack) { + ff_msgs::msg::AckStamped::SharedPtr const ack) { // Only need to handle guest science acks that are from plan commands. The // executive will handle the rest. // Check if command is still executing the command and return if so - if (ack->completed_status.status == ff_msgs::AckCompletedStatus::NOT) { + if (ack->completed_status.status == ff_msgs::msg::AckCompletedStatus::NOT) { return this; - } else if (ack->completed_status.status != ff_msgs::AckCompletedStatus::OK) { + } else if (ack->completed_status.status != + ff_msgs::msg::AckCompletedStatus::OK) { AckCmd(exec_->GetRunPlanCmdId(), ack->completed_status.status, ack->message, ack->status.status); - exec_->SetPlanExecState(ff_msgs::ExecState::PAUSED); - exec_->PublishPlanStatus(ff_msgs::AckStatus::REQUEUED); + exec_->SetPlanExecState(ff_msgs::msg::ExecState::PAUSED); + exec_->PublishPlanStatus(ff_msgs::msg::AckStatus::REQUEUED); return OpStateRepo::Instance()->ready()->StartupState(); } else { return AckStartPlanItem(); @@ -315,9 +319,9 @@ void OpStatePlanExec::AckCmd(std::string const& cmd_id, // Check if command is a plan command if (cmd_id == "plan") { // Only need to check for commands are completed and that fail - if (completed_status != ff_msgs::AckCompletedStatus::OK && - completed_status != ff_msgs::AckCompletedStatus::NOT && - completed_status != ff_msgs::AckCompletedStatus::CANCELED) { + if (completed_status != ff_msgs::msg::AckCompletedStatus::OK && + completed_status != ff_msgs::msg::AckCompletedStatus::NOT && + completed_status != ff_msgs::msg::AckCompletedStatus::CANCELED) { AckPlanCmdFailed(completed_status, message); } } else { @@ -334,11 +338,12 @@ void OpStatePlanExec::AckPlanCmdFailed(uint8_t completed_status, // Call ack command with the run plan command id so the run plan command id // gets acked AckCmd(exec_->GetRunPlanCmdId(), completed_status, message); - exec_->SetPlanExecState(ff_msgs::ExecState::PAUSED); - exec_->PublishPlanStatus(ff_msgs::AckStatus::REQUEUED); + exec_->SetPlanExecState(ff_msgs::msg::ExecState::PAUSED); + exec_->PublishPlanStatus(ff_msgs::msg::AckStatus::REQUEUED); } -bool OpStatePlanExec::PausePlan(ff_msgs::CommandStampedPtr const& cmd) { +bool OpStatePlanExec::PausePlan( + ff_msgs::msg::CommandStamped::SharedPtr const cmd) { // Check to see if the command came from the plan. If it did, we don't need // to cancel actions since plans are sequential if (cmd->cmd_id == "plan") { @@ -347,21 +352,21 @@ bool OpStatePlanExec::PausePlan(ff_msgs::CommandStampedPtr const& cmd) { // Ack the pause as completed in the plan exec_->AckCurrentPlanItem(); // Publish plan status with the next item queued - exec_->PublishPlanStatus(ff_msgs::AckStatus::QUEUED); + exec_->PublishPlanStatus(ff_msgs::msg::AckStatus::QUEUED); - exec_->SetPlanExecState(ff_msgs::ExecState::PAUSED); + exec_->SetPlanExecState(ff_msgs::msg::ExecState::PAUSED); } else { exec_->PublishCmdAck(exec_->GetRunPlanCmdId(), - ff_msgs::AckCompletedStatus::CANCELED); + ff_msgs::msg::AckCompletedStatus::CANCELED); - exec_->SetPlanExecState(ff_msgs::ExecState::PAUSED); + exec_->SetPlanExecState(ff_msgs::msg::ExecState::PAUSED); if (exec_->AreActionsRunning()) { // TODO(Katie) Cancel instead of requeueing if the pause came in while // executing a segment if (exec_->IsActionRunning(EXECUTE)) { - exec_->PublishPlanStatus(ff_msgs::AckStatus::REQUEUED); + exec_->PublishPlanStatus(ff_msgs::msg::AckStatus::REQUEUED); } else { - exec_->PublishPlanStatus(ff_msgs::AckStatus::REQUEUED); + exec_->PublishPlanStatus(ff_msgs::msg::AckStatus::REQUEUED); } exec_->StopAllMotion(cmd); @@ -372,11 +377,11 @@ bool OpStatePlanExec::PausePlan(ff_msgs::CommandStampedPtr const& cmd) { exec_->StopWaitTimer(); waiting_ = false; exec_->AckCurrentPlanItem(); - exec_->PublishPlanStatus(ff_msgs::AckStatus::QUEUED); + exec_->PublishPlanStatus(ff_msgs::msg::AckStatus::QUEUED); exec_->PublishCmdAck(cmd->cmd_id); } else { AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, "Executive: Don't know how to pause command being executed!"); return false; } @@ -396,7 +401,7 @@ OpState* OpStatePlanExec::HandleActionComplete( std::string err_msg = GenerateActionFailedMsg(state, action, result); AckCmd(exec_->GetRunPlanCmdId(), - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, err_msg); // Start a stop action since we don't know what the mobility state @@ -404,22 +409,22 @@ OpState* OpStatePlanExec::HandleActionComplete( exec_->FillMotionGoal(STOP); exec_->StartAction(STOP, "internal"); - exec_->SetPlanExecState(ff_msgs::ExecState::PAUSED); - exec_->PublishPlanStatus(ff_msgs::AckStatus::REQUEUED); + exec_->SetPlanExecState(ff_msgs::msg::ExecState::PAUSED); + exec_->PublishPlanStatus(ff_msgs::msg::AckStatus::REQUEUED); return OpStateRepo::Instance()->teleop()->StartupState(); } OpState* OpStatePlanExec::AckStartPlanItem() { // Returns true if there are more commands/segments in the plan if (exec_->AckCurrentPlanItem()) { - ROS_DEBUG("Starting next item in plan!"); - exec_->PublishPlanStatus(ff_msgs::AckStatus::EXECUTING); + exec_->Debug("Starting next item in plan!"); + exec_->PublishPlanStatus(ff_msgs::msg::AckStatus::EXECUTING); return StartNextPlanItem(); } else { - ROS_DEBUG("Plan complete!"); + exec_->Debug("Plan complete!"); AckCmd(exec_->GetRunPlanCmdId()); - exec_->PublishPlanStatus(ff_msgs::AckStatus::COMPLETED); - exec_->SetPlanExecState(ff_msgs::ExecState::IDLE); + exec_->PublishPlanStatus(ff_msgs::msg::AckStatus::COMPLETED); + exec_->SetPlanExecState(ff_msgs::msg::ExecState::IDLE); return OpStateRepo::Instance()->ready()->StartupState(); } return this; @@ -430,16 +435,16 @@ OpState* OpStatePlanExec::StartNextPlanItem() { std::string err_msg; if (it == sequencer::ItemType::SEGMENT) { - ROS_DEBUG("Got and sending segment."); + exec_->Debug("Got and sending segment."); exec_->FillMotionGoal(EXECUTE); if (!exec_->ConfigureMobility(first_segment_, err_msg)) { - AckPlanCmdFailed(ff_msgs::AckCompletedStatus::EXEC_FAILED, err_msg); + AckPlanCmdFailed(ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, err_msg); return OpStateRepo::Instance()->ready()->StartupState(); } if (!exec_->StartAction(EXECUTE, "plan")) { - AckPlanCmdFailed(ff_msgs::AckCompletedStatus::EXEC_FAILED, err_msg); + AckPlanCmdFailed(ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, err_msg); return OpStateRepo::Instance()->ready()->StartupState(); } @@ -447,16 +452,16 @@ OpState* OpStatePlanExec::StartNextPlanItem() { first_segment_ = false; } } else if (it == sequencer::ItemType::COMMAND) { - ROS_DEBUG("Executing next command."); + exec_->Debug("Executing next command."); return HandleCmd(exec_->GetPlanCommand()); } else { // Plan is empty so it must have completed successfully // This covers the crazy case of a paused plan where the wait command was // the part of the plan that got paused and it was the last item in the // plan. - ROS_INFO("Plan complete!!!"); + exec_->Info("Plan complete!!!"); AckCmd(exec_->GetRunPlanCmdId()); - exec_->SetPlanExecState(ff_msgs::ExecState::IDLE); + exec_->SetPlanExecState(ff_msgs::msg::ExecState::IDLE); return OpStateRepo::Instance()->ready()->StartupState(); } return this; diff --git a/management/executive/src/op_state_ready.cc b/management/executive/src/op_state_ready.cc index fbf7524580..a1854d9787 100644 --- a/management/executive/src/op_state_ready.cc +++ b/management/executive/src/op_state_ready.cc @@ -21,7 +21,8 @@ #include namespace executive { -OpState* OpStateReady::HandleCmd(ff_msgs::CommandStampedPtr const& cmd) { +OpState* OpStateReady::HandleCmd( + ff_msgs::msg::CommandStamped::SharedPtr const cmd) { std::string err_msg; bool completed = false, successful = false; @@ -122,16 +123,18 @@ OpState* OpStateReady::HandleCmd(ff_msgs::CommandStampedPtr const& cmd) { exec_->SetZones(cmd); } else if (cmd->cmd_name == CommandConstants::CMD_NAME_SIMPLE_MOVE6DOF) { // Make sure we are stopped, not docked or perched, before moving - if ((exec_->GetMobilityState().state == ff_msgs::MobilityState::STOPPING && + if ((exec_->GetMobilityState().state == + ff_msgs::msg::MobilityState::STOPPING && exec_->GetMobilityState().sub_state == 0) || - exec_->GetMobilityState().state == ff_msgs::MobilityState::DRIFTING) { + exec_->GetMobilityState().state == + ff_msgs::msg::MobilityState::DRIFTING) { if (!exec_->FillMotionGoal(MOVE, cmd)) { return this; } if (!exec_->ConfigureMobility(false, err_msg)) { AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, err_msg); return this; } @@ -141,7 +144,7 @@ OpState* OpStateReady::HandleCmd(ff_msgs::CommandStampedPtr const& cmd) { } } else { AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, "Cannot move when in ready and not stopped."); return this; } @@ -184,14 +187,16 @@ OpState* OpStateReady::HandleCmd(ff_msgs::CommandStampedPtr const& cmd) { } else { err_msg = "Command " + cmd->cmd_name + " not accepted in operating state " + "ready."; - AckCmd(cmd->cmd_id, ff_msgs::AckCompletedStatus::EXEC_FAILED, err_msg); - ROS_WARN("Executive: %s", err_msg.c_str()); + AckCmd(cmd->cmd_id, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, + err_msg); + exec_->Warn(err_msg); } return this; } OpState* OpStateReady::HandleGuestScienceAck( - ff_msgs::AckStampedConstPtr const& ack) { + ff_msgs::msg::AckStamped::SharedPtr const ack) { // There is a small possibility that the ready state will have to handle a // guest science ack for a plan. This is possible if the plan state starts a // guest science command, a fault occurs so we transition to the fault state, @@ -199,9 +204,10 @@ OpState* OpStateReady::HandleGuestScienceAck( // science command completes. // If the command is not part of a plan, it gets acked in the executive // If the command is not done, don't do anything. - if (ack->completed_status.status == ff_msgs::AckCompletedStatus::NOT) { + if (ack->completed_status.status == ff_msgs::msg::AckCompletedStatus::NOT) { return this; - } else if (ack->completed_status.status != ff_msgs::AckCompletedStatus::OK) { + } else if (ack->completed_status.status != + ff_msgs::msg::AckCompletedStatus::OK) { SetPlanStatus(false, ack->message); } else { SetPlanStatus(true); diff --git a/management/executive/src/op_state_repo.cc b/management/executive/src/op_state_repo.cc index b02e529b18..921d9cb426 100644 --- a/management/executive/src/op_state_repo.cc +++ b/management/executive/src/op_state_repo.cc @@ -31,13 +31,13 @@ void OpStateRepo::SetExec(Executive *const exec) { } OpStateRepo::OpStateRepo() { - ready_.reset(new OpStateReady("ready", ff_msgs::OpState::READY)); + ready_.reset(new OpStateReady("ready", ff_msgs::msg::OpState::READY)); plan_exec_.reset(new OpStatePlanExec("plan execution", - ff_msgs::OpState::PLAN_EXECUTION)); + ff_msgs::msg::OpState::PLAN_EXECUTION)); teleop_.reset(new OpStateTeleop("teleoperation", - ff_msgs::OpState::TELEOPERATION)); + ff_msgs::msg::OpState::TELEOPERATION)); auto_return_.reset(new OpStateAutoReturn("auto return", - ff_msgs::OpState::AUTO_RETURN)); - fault_.reset(new OpStateFault("fault", ff_msgs::OpState::FAULT)); + ff_msgs::msg::OpState::AUTO_RETURN)); + fault_.reset(new OpStateFault("fault", ff_msgs::msg::OpState::FAULT)); } } // namespace executive diff --git a/management/executive/src/op_state_teleop.cc b/management/executive/src/op_state_teleop.cc index c0ff1cd2e2..19fffd3195 100644 --- a/management/executive/src/op_state_teleop.cc +++ b/management/executive/src/op_state_teleop.cc @@ -19,7 +19,9 @@ #include "executive/op_state_teleop.h" namespace executive { -OpState* OpStateTeleop::HandleCmd(ff_msgs::CommandStampedPtr const& cmd) { + +OpState* OpStateTeleop::HandleCmd( + ff_msgs::msg::CommandStamped::SharedPtr const cmd) { bool completed = false, successful = false; std::string err_msg; @@ -81,10 +83,13 @@ OpState* OpStateTeleop::HandleCmd(ff_msgs::CommandStampedPtr const& cmd) { exec_->SetZones(cmd); } else if (cmd->cmd_name == CommandConstants::CMD_NAME_SIMPLE_MOVE6DOF) { // Make sure we aren't docked, docking, perched, or perching - if (exec_->GetMobilityState().state != ff_msgs::MobilityState::DOCKING && - exec_->GetMobilityState().state != ff_msgs::MobilityState::PERCHING) { + if (exec_->GetMobilityState().state != + ff_msgs::msg::MobilityState::DOCKING && + exec_->GetMobilityState().state != + ff_msgs::msg::MobilityState::PERCHING) { // If flying, need to stop - if (exec_->GetMobilityState().state == ff_msgs::MobilityState::FLYING) { + if (exec_->GetMobilityState().state == + ff_msgs::msg::MobilityState::FLYING) { exec_->CancelAction(MOVE, "move"); // Need to stop before we can issue the next move exec_->FillMotionGoal(STOP); @@ -100,7 +105,7 @@ OpState* OpStateTeleop::HandleCmd(ff_msgs::CommandStampedPtr const& cmd) { if (!exec_->ConfigureMobility(false, err_msg)) { AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, err_msg); return this; } @@ -109,7 +114,7 @@ OpState* OpStateTeleop::HandleCmd(ff_msgs::CommandStampedPtr const& cmd) { } } else { AckCmd(cmd->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, "Cannot execute a move when docked, docking, perched or perching"); } } else if (cmd->cmd_name == CommandConstants::CMD_NAME_SKIP_PLAN_STEP) { @@ -127,7 +132,9 @@ OpState* OpStateTeleop::HandleCmd(ff_msgs::CommandStampedPtr const& cmd) { } else { err_msg = "Command " + cmd->cmd_name + " not accepted in op state" + " teleop."; - AckCmd(cmd->cmd_id, ff_msgs::AckCompletedStatus::EXEC_FAILED, err_msg); + AckCmd(cmd->cmd_id, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, + err_msg); } // Check if actions are running. If they aren't, go back to ready mode @@ -165,7 +172,7 @@ OpState* OpStateTeleop::HandleResult( if (!exec_->ConfigureMobility(false, err_msg)) { AckCmd(move_cmd_->cmd_id, - ff_msgs::AckCompletedStatus::EXEC_FAILED, + ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, err_msg); move_cmd_ = NULL; return OpStateRepo::Instance()->ready()->StartupState(); @@ -197,7 +204,7 @@ OpState* OpStateTeleop::HandleResult( if (cmd_id == "plan") { SetPlanStatus(false, err_msg); } else { - AckCmd(cmd_id, ff_msgs::AckCompletedStatus::EXEC_FAILED, err_msg); + AckCmd(cmd_id, ff_msgs::msg::AckCompletedStatus::EXEC_FAILED, err_msg); } } diff --git a/management/executive/src/utils/sequencer/command_conversion.cc b/management/executive/src/utils/sequencer/command_conversion.cc index 4fdcd5cfe0..1518bee146 100644 --- a/management/executive/src/utils/sequencer/command_conversion.cc +++ b/management/executive/src/utils/sequencer/command_conversion.cc @@ -20,8 +20,8 @@ #include #include -#include -#include +#include +#include #include @@ -32,54 +32,54 @@ namespace { // generic "do nothing" function used for commands that have no arguments bool GenNoop(const jsonloader::Command *plan_cmd, - ff_msgs::CommandStamped *dds_cmd) { + ff_msgs::msg::CommandStamped *dds_cmd) { return true; } bool GenDock(const jsonloader::Command *plan_cmd, - ff_msgs::CommandStamped *dds_cmd) { + ff_msgs::msg::CommandStamped *dds_cmd) { const jsonloader::DockCommand *d_cmd = dynamic_cast(plan_cmd); - ff_msgs::CommandArg arg; - arg.data_type = ff_msgs::CommandArg::DATA_TYPE_INT; + ff_msgs::msg::CommandArg arg; + arg.data_type = ff_msgs::msg::CommandArg::DATA_TYPE_INT; arg.i = d_cmd->berth_number(); dds_cmd->args.push_back(arg); return true; } bool GenWait(const jsonloader::Command *plan_cmd, - ff_msgs::CommandStamped *dds_cmd) { + ff_msgs::msg::CommandStamped *dds_cmd) { const jsonloader::StationKeepCommand *sk_cmd = dynamic_cast(plan_cmd); - ff_msgs::CommandArg arg; - arg.data_type = ff_msgs::CommandArg::DATA_TYPE_FLOAT; + ff_msgs::msg::CommandArg arg; + arg.data_type = ff_msgs::msg::CommandArg::DATA_TYPE_FLOAT; arg.f = sk_cmd->duration(); dds_cmd->args.push_back(arg); return true; } bool GenPower(const jsonloader::Command *plan_cmd, - ff_msgs::CommandStamped *dds_cmd) { + ff_msgs::msg::CommandStamped *dds_cmd) { const jsonloader::PowerItemCommand *p_cmd = dynamic_cast(plan_cmd); - ff_msgs::CommandArg arg; - arg.data_type = ff_msgs::CommandArg::DATA_TYPE_STRING; + ff_msgs::msg::CommandArg arg; + arg.data_type = ff_msgs::msg::CommandArg::DATA_TYPE_STRING; arg.s = p_cmd->which(); dds_cmd->args.push_back(arg); return true; } bool GenFlashlight(const jsonloader::Command *flash_cmd, - ff_msgs::CommandStamped *dds_cmd) { + ff_msgs::msg::CommandStamped *dds_cmd) { const jsonloader::FlashlightCommand *f_cmd = dynamic_cast(flash_cmd); - ff_msgs::CommandArg arg; - arg.data_type = ff_msgs::CommandArg::DATA_TYPE_STRING; + ff_msgs::msg::CommandArg arg; + arg.data_type = ff_msgs::msg::CommandArg::DATA_TYPE_STRING; arg.s = f_cmd->which(); dds_cmd->args.push_back(arg); - ff_msgs::CommandArg bright_arg; - bright_arg.data_type = ff_msgs::CommandArg::DATA_TYPE_FLOAT; + ff_msgs::msg::CommandArg bright_arg; + bright_arg.data_type = ff_msgs::msg::CommandArg::DATA_TYPE_FLOAT; bright_arg.f = f_cmd->brightness(); dds_cmd->args.push_back(bright_arg); @@ -87,90 +87,90 @@ bool GenFlashlight(const jsonloader::Command *flash_cmd, } bool GenArm(const jsonloader::Command *plan_cmd, - ff_msgs::CommandStamped *dds_cmd) { + ff_msgs::msg::CommandStamped *dds_cmd) { const jsonloader::ArmPanAndTiltCommand *arm_cmd = dynamic_cast(plan_cmd); - ff_msgs::CommandArg pan_arg; - pan_arg.data_type = ff_msgs::CommandArg::DATA_TYPE_FLOAT; + ff_msgs::msg::CommandArg pan_arg; + pan_arg.data_type = ff_msgs::msg::CommandArg::DATA_TYPE_FLOAT; pan_arg.f = arm_cmd->pan(); dds_cmd->args.push_back(pan_arg); - ff_msgs::CommandArg tilt_arg; - tilt_arg.data_type = ff_msgs::CommandArg::DATA_TYPE_FLOAT; + ff_msgs::msg::CommandArg tilt_arg; + tilt_arg.data_type = ff_msgs::msg::CommandArg::DATA_TYPE_FLOAT; tilt_arg.f = arm_cmd->tilt(); dds_cmd->args.push_back(tilt_arg); - ff_msgs::CommandArg which_arg; - which_arg.data_type = ff_msgs::CommandArg::DATA_TYPE_STRING; + ff_msgs::msg::CommandArg which_arg; + which_arg.data_type = ff_msgs::msg::CommandArg::DATA_TYPE_STRING; which_arg.s = arm_cmd->which(); dds_cmd->args.push_back(which_arg); return true; } bool GenGripper(const jsonloader::Command *plan_cmd, - ff_msgs::CommandStamped *dds_cmd) { + ff_msgs::msg::CommandStamped *dds_cmd) { const jsonloader::GripperCommand *g_cmd = dynamic_cast(plan_cmd); - ff_msgs::CommandArg arg; - arg.data_type = ff_msgs::CommandArg::DATA_TYPE_BOOL; + ff_msgs::msg::CommandArg arg; + arg.data_type = ff_msgs::msg::CommandArg::DATA_TYPE_BOOL; arg.b = g_cmd->open(); dds_cmd->args.push_back(arg); return true; } bool GenGuestScience(const jsonloader::Command *plan_cmd, - ff_msgs::CommandStamped *dds_cmd) { + ff_msgs::msg::CommandStamped *dds_cmd) { const jsonloader::GuestScienceCommand *g_cmd = dynamic_cast(plan_cmd); - ff_msgs::CommandArg arg; - arg.data_type = ff_msgs::CommandArg::DATA_TYPE_STRING; + ff_msgs::msg::CommandArg arg; + arg.data_type = ff_msgs::msg::CommandArg::DATA_TYPE_STRING; arg.s = g_cmd->apk(); dds_cmd->args.push_back(arg); return true; } bool GenCustomScience(const jsonloader::Command *plan_cmd, - ff_msgs::CommandStamped *dds_cmd) { + ff_msgs::msg::CommandStamped *dds_cmd) { const jsonloader::CustomGuestScienceCommand *g_cmd = dynamic_cast(plan_cmd); - ff_msgs::CommandArg arg; - arg.data_type = ff_msgs::CommandArg::DATA_TYPE_STRING; + ff_msgs::msg::CommandArg arg; + arg.data_type = ff_msgs::msg::CommandArg::DATA_TYPE_STRING; arg.s = g_cmd->apk(); dds_cmd->args.push_back(arg); - ff_msgs::CommandArg arg2; - arg2.data_type = ff_msgs::CommandArg::DATA_TYPE_STRING; + ff_msgs::msg::CommandArg arg2; + arg2.data_type = ff_msgs::msg::CommandArg::DATA_TYPE_STRING; arg2.s = g_cmd->command(); dds_cmd->args.push_back(arg2); return true; } bool GenSetCamera(const jsonloader::Command *plan_cmd, - ff_msgs::CommandStamped *dds_cmd) { + ff_msgs::msg::CommandStamped *dds_cmd) { const jsonloader::SetCameraCommand *s_cmd = dynamic_cast(plan_cmd); - ff_msgs::CommandArg arg; - arg.data_type = ff_msgs::CommandArg::DATA_TYPE_STRING; + ff_msgs::msg::CommandArg arg; + arg.data_type = ff_msgs::msg::CommandArg::DATA_TYPE_STRING; arg.s = s_cmd->camera(); dds_cmd->args.push_back(arg); - ff_msgs::CommandArg arg5; - arg5.data_type = ff_msgs::CommandArg::DATA_TYPE_STRING; + ff_msgs::msg::CommandArg arg5; + arg5.data_type = ff_msgs::msg::CommandArg::DATA_TYPE_STRING; arg5.s = s_cmd->mode(); dds_cmd->args.push_back(arg5); - ff_msgs::CommandArg arg2; - arg2.data_type = ff_msgs::CommandArg::DATA_TYPE_STRING; + ff_msgs::msg::CommandArg arg2; + arg2.data_type = ff_msgs::msg::CommandArg::DATA_TYPE_STRING; arg2.s = s_cmd->resolution(); dds_cmd->args.push_back(arg2); - ff_msgs::CommandArg arg3; - arg3.data_type = ff_msgs::CommandArg::DATA_TYPE_FLOAT; + ff_msgs::msg::CommandArg arg3; + arg3.data_type = ff_msgs::msg::CommandArg::DATA_TYPE_FLOAT; arg3.f = s_cmd->frame_rate(); dds_cmd->args.push_back(arg3); - ff_msgs::CommandArg arg4; - arg4.data_type = ff_msgs::CommandArg::DATA_TYPE_FLOAT; + ff_msgs::msg::CommandArg arg4; + arg4.data_type = ff_msgs::msg::CommandArg::DATA_TYPE_FLOAT; arg4.f = s_cmd->bandwidth(); dds_cmd->args.push_back(arg4); @@ -178,103 +178,103 @@ bool GenSetCamera(const jsonloader::Command *plan_cmd, } bool GenRecordCamera(const jsonloader::Command *plan_cmd, - ff_msgs::CommandStamped *dds_cmd) { + ff_msgs::msg::CommandStamped *dds_cmd) { const jsonloader::RecordCameraCommand *g_cmd = dynamic_cast(plan_cmd); - ff_msgs::CommandArg arg; - arg.data_type = ff_msgs::CommandArg::DATA_TYPE_STRING; + ff_msgs::msg::CommandArg arg; + arg.data_type = ff_msgs::msg::CommandArg::DATA_TYPE_STRING; arg.s = g_cmd->camera(); dds_cmd->args.push_back(arg); - ff_msgs::CommandArg arg2; - arg2.data_type = ff_msgs::CommandArg::DATA_TYPE_BOOL; + ff_msgs::msg::CommandArg arg2; + arg2.data_type = ff_msgs::msg::CommandArg::DATA_TYPE_BOOL; arg2.b = g_cmd->record(); dds_cmd->args.push_back(arg2); return true; } bool GenStreamCamera(const jsonloader::Command *plan_cmd, - ff_msgs::CommandStamped *dds_cmd) { + ff_msgs::msg::CommandStamped *dds_cmd) { const jsonloader::StreamCameraCommand *g_cmd = dynamic_cast(plan_cmd); - ff_msgs::CommandArg arg; - arg.data_type = ff_msgs::CommandArg::DATA_TYPE_STRING; + ff_msgs::msg::CommandArg arg; + arg.data_type = ff_msgs::msg::CommandArg::DATA_TYPE_STRING; arg.s = g_cmd->camera(); dds_cmd->args.push_back(arg); - ff_msgs::CommandArg arg2; - arg2.data_type = ff_msgs::CommandArg::DATA_TYPE_BOOL; + ff_msgs::msg::CommandArg arg2; + arg2.data_type = ff_msgs::msg::CommandArg::DATA_TYPE_BOOL; arg2.b = g_cmd->stream(); dds_cmd->args.push_back(arg2); return true; } bool GenSetPlanner(const jsonloader::Command *plan_cmd, - ff_msgs::CommandStamped *dds_cmd) { + ff_msgs::msg::CommandStamped *dds_cmd) { const jsonloader::SetPlannerCommand *d_cmd = dynamic_cast(plan_cmd); - ff_msgs::CommandArg arg; - arg.data_type = ff_msgs::CommandArg::DATA_TYPE_STRING; + ff_msgs::msg::CommandArg arg; + arg.data_type = ff_msgs::msg::CommandArg::DATA_TYPE_STRING; arg.s = d_cmd->planner(); dds_cmd->args.push_back(arg); return true; } bool GenChkObstacles(const jsonloader::Command *plan_cmd, - ff_msgs::CommandStamped *dds_cmd) { + ff_msgs::msg::CommandStamped *dds_cmd) { const jsonloader::SetCheckObstaclesCommand *d_cmd = dynamic_cast(plan_cmd); - ff_msgs::CommandArg arg; - arg.data_type = ff_msgs::CommandArg::DATA_TYPE_BOOL; + ff_msgs::msg::CommandArg arg; + arg.data_type = ff_msgs::msg::CommandArg::DATA_TYPE_BOOL; arg.b = d_cmd->checkObstacles(); dds_cmd->args.push_back(arg); return true; } bool GenChkZones(const jsonloader::Command *plan_cmd, - ff_msgs::CommandStamped *dds_cmd) { + ff_msgs::msg::CommandStamped *dds_cmd) { const jsonloader::SetCheckZonesCommand *d_cmd = dynamic_cast(plan_cmd); - ff_msgs::CommandArg arg; - arg.data_type = ff_msgs::CommandArg::DATA_TYPE_BOOL; + ff_msgs::msg::CommandArg arg; + arg.data_type = ff_msgs::msg::CommandArg::DATA_TYPE_BOOL; arg.b = d_cmd->checkZones(); dds_cmd->args.push_back(arg); return true; } bool GenHolonomic(const jsonloader::Command *plan_cmd, - ff_msgs::CommandStamped *dds_cmd) { + ff_msgs::msg::CommandStamped *dds_cmd) { const jsonloader::SetHolonomicModeCommand *d_cmd = dynamic_cast(plan_cmd); - ff_msgs::CommandArg arg; - arg.data_type = ff_msgs::CommandArg::DATA_TYPE_BOOL; + ff_msgs::msg::CommandArg arg; + arg.data_type = ff_msgs::msg::CommandArg::DATA_TYPE_BOOL; arg.b = d_cmd->enableHolonomic(); dds_cmd->args.push_back(arg); return true; } bool GenSwitchLocalization(const jsonloader::Command *plan_cmd, - ff_msgs::CommandStamped *dds_cmd) { + ff_msgs::msg::CommandStamped *dds_cmd) { const jsonloader::SwitchLocalizationCommand *d_cmd = dynamic_cast(plan_cmd); - ff_msgs::CommandArg arg; - arg.data_type = ff_msgs::CommandArg::DATA_TYPE_STRING; + ff_msgs::msg::CommandArg arg; + arg.data_type = ff_msgs::msg::CommandArg::DATA_TYPE_STRING; arg.s = d_cmd->mode(); dds_cmd->args.push_back(arg); return true; } bool GenTelemRate(const jsonloader::Command *plan_cmd, - ff_msgs::CommandStamped *dds_cmd) { + ff_msgs::msg::CommandStamped *dds_cmd) { const jsonloader::SetTelemetryRateCommand *d_cmd = dynamic_cast(plan_cmd); - ff_msgs::CommandArg arg; - arg.data_type = ff_msgs::CommandArg::DATA_TYPE_STRING; + ff_msgs::msg::CommandArg arg; + arg.data_type = ff_msgs::msg::CommandArg::DATA_TYPE_STRING; arg.s = d_cmd->telemetryName(); dds_cmd->args.push_back(arg); - ff_msgs::CommandArg arg2; - arg2.data_type = ff_msgs::CommandArg::DATA_TYPE_FLOAT; + ff_msgs::msg::CommandArg arg2; + arg2.data_type = ff_msgs::msg::CommandArg::DATA_TYPE_FLOAT; arg2.f = d_cmd->rate(); dds_cmd->args.push_back(arg2); @@ -282,11 +282,11 @@ bool GenTelemRate(const jsonloader::Command *plan_cmd, } bool GenStartRecording(const jsonloader::Command *plan_cmd, - ff_msgs::CommandStamped *dds_cmd) { + ff_msgs::msg::CommandStamped *dds_cmd) { const jsonloader::StartRecordingCommand *d_cmd = dynamic_cast(plan_cmd); - ff_msgs::CommandArg arg; - arg.data_type = ff_msgs::CommandArg::DATA_TYPE_STRING; + ff_msgs::msg::CommandArg arg; + arg.data_type = ff_msgs::msg::CommandArg::DATA_TYPE_STRING; arg.s = d_cmd->description(); dds_cmd->args.push_back(arg); return true; @@ -298,7 +298,7 @@ namespace sequencer { namespace internal { namespace jl = jsonloader; -using cc = ff_msgs::CommandConstants; +using cc = ff_msgs::msg::CommandConstants; extern const std::unordered_map kCmdGenMap = { { jl::kCmdDock, { cc::CMD_NAME_DOCK, cc::CMD_SUBSYS_MOBILITY, GenDock } }, diff --git a/management/executive/src/utils/sequencer/plan_io.cc b/management/executive/src/utils/sequencer/plan_io.cc index e2bb5613d5..866fb824c7 100644 --- a/management/executive/src/utils/sequencer/plan_io.cc +++ b/management/executive/src/utils/sequencer/plan_io.cc @@ -34,15 +34,15 @@ bool sequencer::DecompressData(const char* data, const std::size_t length, io::filtering_ostream ostream; switch (type) { - case ff_msgs::CompressedFile::TYPE_NONE: + case ff_msgs::msg::CompressedFile::TYPE_NONE: break; - case ff_msgs::CompressedFile::TYPE_DEFLATE: + case ff_msgs::msg::CompressedFile::TYPE_DEFLATE: ostream.push(io::zlib_decompressor()); break; - case ff_msgs::CompressedFile::TYPE_GZ: + case ff_msgs::msg::CompressedFile::TYPE_GZ: ostream.push(io::gzip_decompressor()); break; - case ff_msgs::CompressedFile::TYPE_BZ2: + case ff_msgs::msg::CompressedFile::TYPE_BZ2: ostream.push(io::bzip2_decompressor()); break; default: diff --git a/management/executive/src/utils/sequencer/sequencer.cc b/management/executive/src/utils/sequencer/sequencer.cc index 89b8f6bd7b..2496493d70 100644 --- a/management/executive/src/utils/sequencer/sequencer.cc +++ b/management/executive/src/utils/sequencer/sequencer.cc @@ -26,11 +26,13 @@ #include -#include +#include #include #include +FF_DEFINE_LOGGER("sequencer"); + namespace sequencer { namespace { @@ -45,9 +47,9 @@ const char * kWaypointTypes[] = { }; */ -ff_msgs::ControlState +ff_msgs::msg::ControlState Waypoint2TrajectoryPoint(jsonloader::Waypoint const& w) { - ff_msgs::ControlState p; + ff_msgs::msg::ControlState p; sv::pose_vel_accel::StateVector sv; sv.vec = w.cwaypoint(); @@ -66,13 +68,16 @@ Waypoint2TrajectoryPoint(jsonloader::Waypoint const& w) { p.accel.angular = msg_conversions::eigen_to_ros_vector( sv::pose_vel_accel::GetAngularAcceleration(sv.vec).cast()); - p.when.nsec = w.ctime().nsec(); + p.when.nanosec = w.ctime().nsec(); p.when.sec = w.ctime().sec(); return p; } -inline ff_msgs::Status MakeStatus(std::uint8_t status, int point, int command, int duration) { - ff_msgs::Status s; +inline ff_msgs::msg::Status MakeStatus(std::uint8_t status, + int point, + int command, + int duration) { + ff_msgs::msg::Status s; s.status.status = status; s.point = point; s.command = command; @@ -82,7 +87,8 @@ inline ff_msgs::Status MakeStatus(std::uint8_t status, int point, int command, i } // namespace -bool LoadPlan(ff_msgs::CompressedFile::ConstPtr const& cf, Sequencer *seq) { +bool LoadPlan(ff_msgs::msg::CompressedFile::SharedPtr const& cf, + Sequencer *seq) { std::string out; // sanity check @@ -111,9 +117,9 @@ bool LoadPlan(ff_msgs::CompressedFile::ConstPtr const& cf, Sequencer *seq) { return true; } -std::vector +std::vector Segment2Trajectory(jsonloader::Segment const& seg) { - std::vector traj; + std::vector traj; traj.reserve(seg.waypoints().size()); for (jsonloader::Waypoint const& wp : seg.waypoints()) { traj.push_back(Waypoint2TrajectoryPoint(wp)); @@ -144,12 +150,12 @@ bool Sequencer::HaveInertia() const noexcept { return plan_.inertia_configuration().valid(); } -geometry_msgs::InertiaStamped Sequencer::GetInertia() const noexcept { - geometry_msgs::InertiaStamped i; +geometry_msgs::msg::InertiaStamped Sequencer::GetInertia() const noexcept { + geometry_msgs::msg::InertiaStamped i; jsonloader::InertiaConfiguration const& ic = plan_.inertia_configuration(); if (!ic.valid()) { - ROS_WARN("requesting inertia, but no inertia was provided"); + FF_WARN("requesting inertia, but no inertia was provided"); } i.header.frame_id = ic.name(); @@ -173,10 +179,11 @@ bool Sequencer::HaveOperatingLimits() const noexcept { return plan_.operating_limits().valid(); } -bool Sequencer::GetOperatingLimits(ff_msgs::AgentStateStamped &state) const noexcept { +bool Sequencer::GetOperatingLimits( + ff_msgs::msg::AgentStateStamped &state) const noexcept { jsonloader::OperatingLimits const & ol = plan_.operating_limits(); if (!ol.valid()) { - ROS_WARN("requesting operating limits, but none provided."); + FF_WARN("requesting operating limits, but none provided."); return false; } @@ -193,6 +200,10 @@ bool Sequencer::GetOperatingLimits(ff_msgs::AgentStateStamped &state) const noex return true; } +void Sequencer::SetNodeHandle(NodeHandle nh) { + nh_ = nh; +} + ItemType Sequencer::CurrentType(bool reset_time) noexcept { if (!valid_) return ItemType::NONE; @@ -210,11 +221,12 @@ ItemType Sequencer::CurrentType(bool reset_time) noexcept { jsonloader::Milestone const& m = plan_.GetMilestone(current_milestone_); jsonloader::Station const& s = dynamic_cast(m); if (s.commands().size() == 0) { - AppendStatus(MakeStatus(ff_msgs::AckCompletedStatus::OK, 0, -1, 0)); + AppendStatus(MakeStatus(ff_msgs::msg::AckCompletedStatus::OK, 0, -1, 0)); current_milestone_++; } else { station_idx_ = - AppendStatus(MakeStatus(ff_msgs::AckCompletedStatus::NOT, 0, -1, 0)); + AppendStatus(MakeStatus(ff_msgs::msg::AckCompletedStatus::NOT, + 0, -1, 0)); if (current_command_ <= 0) current_command_ = 0; } @@ -226,7 +238,7 @@ ItemType Sequencer::CurrentType(bool reset_time) noexcept { // http://imgur.com/vGyMAnB if (reset_time) - start_ = ros::Time::now(); + start_ = nh_->get_clock()->now(); if (plan_.GetMilestone(current_milestone_).IsSegment()) return ItemType::SEGMENT; @@ -238,7 +250,7 @@ ItemType Sequencer::CurrentType(bool reset_time) noexcept { jsonloader::Segment Sequencer::CurrentSegment() noexcept { jsonloader::Milestone const& m = plan_.GetMilestone(current_milestone_); if (!m.IsSegment()) { - ROS_WARN("requesting a segment, but current milestone is not a segment."); + FF_WARN("requesting a segment, but current milestone is not a segment"); return jsonloader::Segment(); } @@ -247,8 +259,9 @@ jsonloader::Segment Sequencer::CurrentSegment() noexcept { return dynamic_cast(m); } -ff_msgs::CommandStamped::Ptr Sequencer::CurrentCommand() noexcept { - ff_msgs::CommandStamped::Ptr cmd(new ff_msgs::CommandStamped()); +ff_msgs::msg::CommandStamped::SharedPtr Sequencer::CurrentCommand() noexcept { + ff_msgs::msg::CommandStamped::SharedPtr cmd = + std::make_shared(); cmd->subsys_name = "INVALID"; cmd->cmd_name = "INVALID"; cmd->cmd_id = "plan"; @@ -257,7 +270,7 @@ ff_msgs::CommandStamped::Ptr Sequencer::CurrentCommand() noexcept { jsonloader::Milestone const& m = plan_.GetMilestone(current_milestone_); if (!m.IsStation()) { - ROS_WARN("requesting a command, but current milestone is not a station."); + FF_WARN("requesting a command, but current milestone is not a station"); cmd.reset(); return cmd; } @@ -267,15 +280,15 @@ ff_msgs::CommandStamped::Ptr Sequencer::CurrentCommand() noexcept { auto it = internal::kCmdGenMap.find(plan_cmd->type()); if (it == internal::kCmdGenMap.end()) { - ROS_ERROR("do not know how to generate command %s", - plan_cmd->type().data()); + FF_ERROR("do not know how to generate command %s", + plan_cmd->type().data()); cmd.reset(); return cmd; } if (!it->second.fn(plan_cmd, cmd.get())) { - ROS_ERROR("error converting command %s", - plan_cmd->type().data()); + FF_ERROR("error converting command %s", + plan_cmd->type().data()); cmd.reset(); return cmd; } @@ -286,13 +299,13 @@ ff_msgs::CommandStamped::Ptr Sequencer::CurrentCommand() noexcept { return cmd; } -bool Sequencer::Feedback(ff_msgs::AckCompletedStatus const& ack) noexcept { +bool Sequencer::Feedback(ff_msgs::msg::AckCompletedStatus const& ack) noexcept { // WE DUN! - ros::Time end = ros::Time::now(); - ros::Duration d = end - start_; + rclcpp::Time end = nh_->get_clock()->now(); + rclcpp::Duration d = end - start_; // update the current thing's status - AppendStatus(MakeStatus(ack.status, current_milestone_, current_command_, d.sec)); + AppendStatus(MakeStatus(ack.status, current_milestone_, current_command_, d.seconds())); // skip ahead jsonloader::Milestone const& m = plan_.GetMilestone(current_milestone_); @@ -303,7 +316,7 @@ bool Sequencer::Feedback(ff_msgs::AckCompletedStatus const& ack) noexcept { } else { jsonloader::Station const& s = dynamic_cast(m); current_command_++; - station_duration_ += d.sec; + station_duration_ += d.seconds(); if (static_cast(current_command_) < s.commands().size()) return true; @@ -333,25 +346,26 @@ bool Sequencer::Feedback(ff_msgs::AckCompletedStatus const& ack) noexcept { // log a temporary entry for the current station, keep track of the index station_idx_ = AppendStatus( // feel like lisp yet? - MakeStatus(ff_msgs::AckCompletedStatus::NOT, + MakeStatus(ff_msgs::msg::AckCompletedStatus::NOT, current_milestone_, -1, 0)); station_duration_ = 0; return true; } // update plan status for the skipped station, CUZ WE ON FIAH - AppendStatus(MakeStatus(ff_msgs::AckCompletedStatus::OK, + AppendStatus(MakeStatus(ff_msgs::msg::AckCompletedStatus::OK, current_milestone_, -1, 0)); current_milestone_++; return static_cast(current_milestone_) < plan_.NumMilestones(); } -void Sequencer::Feedback(ff_msgs::ControlFeedback const& progress) noexcept { +void Sequencer::Feedback( + ff_msgs::msg::ControlFeedback const& progress) noexcept { current_index_ = progress.index; // ain't no thang } -int Sequencer::AppendStatus(ff_msgs::Status const& msg) noexcept { +int Sequencer::AppendStatus(ff_msgs::msg::Status const& msg) noexcept { status_.history.push_back(msg); if (status_.history.size() > kMaxHistory) status_.history.erase(status_.history.begin()); @@ -363,13 +377,13 @@ bool Sequencer::valid() const noexcept { return valid_; } -ff_msgs::PlanStatusStamped const& Sequencer::plan_status() noexcept { +ff_msgs::msg::PlanStatusStamped const& Sequencer::plan_status() noexcept { status_.point = current_milestone_; status_.command = current_command_; if (static_cast(current_milestone_) >= plan_.NumMilestones()) { - status_.status.status = ff_msgs::AckStatus::COMPLETED; + status_.status.status = ff_msgs::msg::AckStatus::COMPLETED; } else { - status_.status.status = ff_msgs::AckStatus::EXECUTING; + status_.status.status = ff_msgs::msg::AckStatus::EXECUTING; } return status_; } diff --git a/management/executive/tools/data_to_disk_pub.cc b/management/executive/tools/data_to_disk_pub.cc index f90c780cbb..377bb9b7b4 100644 --- a/management/executive/tools/data_to_disk_pub.cc +++ b/management/executive/tools/data_to_disk_pub.cc @@ -16,14 +16,16 @@ * under the License. */ +#include +#include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include + +#include #include @@ -44,12 +46,20 @@ namespace fs = boost::filesystem; namespace io = boost::iostreams; +FF_DEFINE_LOGGER("data_to_disk_pub") + DEFINE_string(compression, "none", "Type of compression [none, deflate, gzip]"); constexpr uintmax_t kMaxSize = 128 * 1024; -ros::Time data_pub_time; -ros::Publisher command_pub; + +rclcpp::Time data_pub_time; + +Publisher command_pub; +Publisher data_to_disk_pub; + +ff_msgs::msg::CompressedFile cf; +ff_util::FreeFlyerTimer data_sub_connected_timer; bool ValidateCompression(const char* name, std::string const &value) { if (value == "none" || value == "gzip" || value == "deflate") @@ -60,29 +70,35 @@ bool ValidateCompression(const char* name, std::string const &value) { return false; } -void on_connect(ros::SingleSubscriberPublisher const& sub, - ff_msgs::CompressedFile &cf) { // NOLINT - ROS_INFO("subscriber present: sending data to disk"); - cf.header.stamp = ros::Time::now(); - sub.publish(cf); +void on_connect() { + FF_INFO("subscriber present: sending data to disk"); + data_to_disk_pub->publish(cf); } -void on_cf_ack(ff_msgs::CompressedFileAckConstPtr const& cf_ack) { - ROS_INFO("ack received: sending set data to disk command"); +void on_cf_ack(ff_msgs::msg::CompressedFileAck::SharedPtr const cf_ack) { + FF_INFO("ack received: sending set data to disk command"); // compressed file ack is latched so we need to check the timestamp to make // sure this plan is being acked if (data_pub_time < cf_ack->header.stamp) { - ff_msgs::CommandStamped cmd; - cmd.cmd_name = ff_msgs::CommandConstants::CMD_NAME_SET_DATA_TO_DISK; + ff_msgs::msg::CommandStamped cmd; + cmd.cmd_name = ff_msgs::msg::CommandConstants::CMD_NAME_SET_DATA_TO_DISK; cmd.subsys_name = "Astrobee"; - command_pub.publish(cmd); - ros::shutdown(); + command_pub->publish(cmd); + rclcpp::shutdown(); + } +} + +void TimerCallback() { + if (data_to_disk_pub->get_subscription_count() > 0) { + on_connect(); + data_sub_connected_timer.stop(); } } int main(int argc, char** argv) { ff_common::InitFreeFlyerApplication(&argc, &argv); - ros::init(argc, argv, "data_to_disk_publisher"); + rclcpp::init(argc, argv); + NodeHandle nh; if (!google::RegisterFlagValidator(&FLAGS_compression, &ValidateCompression)) { std::cerr << "Failed to register compression flag validator." << std::endl; @@ -96,8 +112,6 @@ int main(int argc, char** argv) { return -1; } - ff_msgs::CompressedFile cf; - cf.header.seq = 1; cf.header.frame_id = "world"; // Load the plan @@ -107,13 +121,13 @@ int main(int argc, char** argv) { io::filtering_ostream out; if (FLAGS_compression == "deflate") { - cf.type = ff_msgs::CompressedFile::TYPE_DEFLATE; + cf.type = ff_msgs::msg::CompressedFile::TYPE_DEFLATE; out.push(io::zlib_compressor()); } else if (FLAGS_compression == "gzip") { - cf.type = ff_msgs::CompressedFile::TYPE_GZ; + cf.type = ff_msgs::msg::CompressedFile::TYPE_GZ; out.push(io::gzip_compressor()); } else { - cf.type = ff_msgs::CompressedFile::TYPE_NONE; + cf.type = ff_msgs::msg::CompressedFile::TYPE_NONE; } out.push(io::back_inserter(data)); @@ -135,23 +149,33 @@ int main(int argc, char** argv) { cf.file.push_back(static_cast(c)); } - ros::NodeHandle n; - data_pub_time = ros::Time::now(); + data_pub_time = nh->get_clock()->now(); + + data_sub_connected_timer.createTimer(1.0, &TimerCallback, nh); // Publishes the data to disk to the executive using subscriber status // callbacks - ros::Publisher data_to_disk_pub = n.advertise( - TOPIC_COMMUNICATIONS_DDS_DATA, 10, - std::bind(&on_connect, std::placeholders::_1, cf)); + data_to_disk_pub = FF_CREATE_PUBLISHER(nh, + ff_msgs::msg::CompressedFile, + TOPIC_COMMUNICATIONS_DDS_DATA, + 10); - // After the zones are received, commands a set zones to the executive - command_pub = n.advertise(TOPIC_COMMAND, 5, false); + // After the zones are received, send command to set zones to the executive + command_pub = FF_CREATE_PUBLISHER(nh, + ff_msgs::msg::CommandStamped, + TOPIC_COMMAND, + 5); // Subscriber that receives confirmation that the zones were received - ros::Subscriber cf_acK_pub = n.subscribe(TOPIC_MANAGEMENT_EXEC_CF_ACK, 10, &on_cf_ack); - - ROS_INFO("waiting for a subscriber..."); - ros::spin(); + Subscriber cf_acK_sub = + FF_CREATE_SUBSCRIBER(nh, + ff_msgs::msg::CompressedFileAck, + TOPIC_MANAGEMENT_EXEC_CF_ACK, + 10, + std::bind(&on_cf_ack, std::placeholders::_1)); + + FF_INFO("waiting for a subscriber..."); + rclcpp::spin(nh); return 0; } diff --git a/management/executive/tools/ekf_switch_mux.cc b/management/executive/tools/ekf_switch_mux.cc deleted file mode 100644 index 7c6ceaeec2..0000000000 --- a/management/executive/tools/ekf_switch_mux.cc +++ /dev/null @@ -1,76 +0,0 @@ -/* Copyright (c) 2017, United States Government, as represented by the - * Administrator of the National Aeronautics and Space Administration. - * - * All rights reserved. - * - * The Astrobee platform is licensed under the Apache License, Version 2.0 - * (the "License"); you may not use this file except in compliance with the - * License. You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT - * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the - * License for the specific language governing permissions and limitations - * under the License. - */ - -// A node to switch the EKF input from landmarks to sparse mapping, -// or something else. This is needed because both landmarks and registration -// need to be switched at the same time. - -#include -#include - -#include - -#include - -#include -#include - -const std::string kRegistrationNode = "mux_registration"; -const std::string kLandmarksNode = "mux_landmarks"; - -int main(int argc, char **argv) { - ff_common::InitFreeFlyerApplication(&argc, &argv); - ros::init(argc, argv, "ekf_switch_mux"); - - if (argc < 2) { - std::cerr << "Usage: " << argv[0] << " [sparse_mapping|marker_tracking]" - << std::endl; - return 1; - } - - ros::NodeHandle n("~"); - - - if (!ros::service::waitForService("mux_registration/select", 10000)) { - std::cerr << "Error: no mux services? is mux_registration running?" - << std::endl; - return 1; - } - - topic_tools::MuxSelect reg_sel; - reg_sel.request.topic = std::string(argv[1]) + "/registration"; - - if (!ros::service::call("/mux_registration/select", reg_sel)) { - std::cerr << "Error: error calling mux_registration/select" - << std::endl; - return 1; - } - - topic_tools::MuxSelect land_sel; - land_sel.request.topic = std::string(argv[1]) + "/landmarks"; - - if (!ros::service::call("/mux_landmarks/select", land_sel)) { - std::cerr << "Error: error calling mux_landmarks/select" - << std::endl; - std::cerr << "NOTE: mux_registration and mux_landmarks are inconsistent!" - << std::endl; - return 1; - } - - return 0; -} diff --git a/management/executive/tools/plan_pub.cc b/management/executive/tools/plan_pub.cc index a263e73f7d..23fbffeadb 100644 --- a/management/executive/tools/plan_pub.cc +++ b/management/executive/tools/plan_pub.cc @@ -16,15 +16,17 @@ * under the License. */ +#include +#include #include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include + +#include #include @@ -45,6 +47,20 @@ namespace fs = boost::filesystem; namespace io = boost::iostreams; +FF_DEFINE_LOGGER("plan_pub") + +DEFINE_string(compression, "none", + "Type of compression [none, deflate, gzip]"); + +constexpr uintmax_t kMaxSize = 128 * 1024; + +rclcpp::Time plan_pub_time; + +Publisher command_pub; +Publisher plan_pub; + +ff_msgs::msg::CompressedFile cf; +ff_util::FreeFlyerTimer data_sub_connected_timer; bool ValidateCompression(const char* name, std::string const &value) { if (value == "none" || value == "gzip" || value == "deflate") @@ -55,58 +71,53 @@ bool ValidateCompression(const char* name, std::string const &value) { return false; } -DEFINE_string(compression, "none", - "Type of compression [none, deflate, gzip]"); - -constexpr uintmax_t kMaxSize = 128 * 1024; - -ros::Publisher command_pub; -ros::Time plan_pub_time; - -void on_connect(ros::SingleSubscriberPublisher const& sub, - ff_msgs::CompressedFile &cf) { - ROS_INFO("subscriber present: sending plan"); - cf.header.stamp = ros::Time::now(); - sub.publish(cf); +void on_connect() { + FF_INFO("subscriber present: sending plan"); + plan_pub->publish(cf); } -void on_cf_ack(ff_msgs::CompressedFileAckConstPtr const& cf_ack) { - ROS_INFO("Got compressed file ack!"); +void on_cf_ack(ff_msgs::msg::CompressedFileAck::SharedPtr const cf_ack) { + FF_INFO("Got compressed file ack!"); // compressed file ack is latched so we need to check the timestamp to make // sure this plan is being acked // ROS_WARN_STREAM(plan_pub_time << " : " << cf_ack->header.stamp); if (plan_pub_time <= cf_ack->header.stamp) { - ROS_INFO("Compressed file ack is valid! Sending set plan!"); - ff_msgs::CommandStamped cmd; - cmd.cmd_name = ff_msgs::CommandConstants::CMD_NAME_SET_PLAN; + FF_INFO("Compressed file ack is valid! Sending set plan!"); + ff_msgs::msg::CommandStamped cmd; + cmd.cmd_name = ff_msgs::msg::CommandConstants::CMD_NAME_SET_PLAN; cmd.subsys_name = "Astrobee"; - command_pub.publish(cmd); + command_pub->publish(cmd); } } // Plan status is published after the plan is set so send run plan to start plan -void on_plan_status(ff_msgs::PlanStatusStamped::ConstPtr const& ps) { - ROS_INFO("Got plan status!"); +void on_plan_status(ff_msgs::msg::PlanStatusStamped::SharedPtr const ps) { + FF_INFO("Got plan status!"); // plan status is latched so we need to check the timestamp to make sure this // plan is loaded // ROS_WARN_STREAM(plan_pub_time << " : " << ps->header.stamp); if (plan_pub_time <= ps->header.stamp) { - ff_msgs::CommandStamped cmd; - cmd.cmd_name = ff_msgs::CommandConstants::CMD_NAME_RUN_PLAN; + ff_msgs::msg::CommandStamped cmd; + cmd.cmd_name = ff_msgs::msg::CommandConstants::CMD_NAME_RUN_PLAN; cmd.subsys_name = "Astrobee"; - command_pub.publish(cmd); + command_pub->publish(cmd); + + FF_INFO_STREAM("received plan status: " << ps->name); + rclcpp::shutdown(); + } +} - ROS_INFO_STREAM("received plan status: " << ps->name); - ros::shutdown(); +void TimerCallback() { + if (plan_pub->get_subscription_count() > 0) { + on_connect(); + data_sub_connected_timer.stop(); } } int main(int argc, char** argv) { ff_common::InitFreeFlyerApplication(&argc, &argv); - ros::init(argc, argv, "plan_pub"); - ros::NodeHandle n; - - ros::Time::waitForValid(); + rclcpp::init(argc, argv); + NodeHandle nh; if (!google::RegisterFlagValidator(&FLAGS_compression, &ValidateCompression)) { std::cerr << "Failed to register compression flag validator." << std::endl; @@ -120,8 +131,6 @@ int main(int argc, char** argv) { return -1; } - ff_msgs::CompressedFile cf; - cf.header.seq = 1; cf.header.frame_id = "world"; // Load the plan @@ -131,13 +140,13 @@ int main(int argc, char** argv) { io::filtering_ostream out; if (FLAGS_compression == "deflate") { - cf.type = ff_msgs::CompressedFile::TYPE_DEFLATE; + cf.type = ff_msgs::msg::CompressedFile::TYPE_DEFLATE; out.push(io::zlib_compressor()); } else if (FLAGS_compression == "gzip") { - cf.type = ff_msgs::CompressedFile::TYPE_GZ; + cf.type = ff_msgs::msg::CompressedFile::TYPE_GZ; out.push(io::gzip_compressor()); } else { - cf.type = ff_msgs::CompressedFile::TYPE_NONE; + cf.type = ff_msgs::msg::CompressedFile::TYPE_NONE; } out.push(io::back_inserter(data)); @@ -159,30 +168,44 @@ int main(int argc, char** argv) { cf.file.push_back(static_cast(c)); } + plan_pub_time = nh->get_clock()->now(); + + command_pub = FF_CREATE_PUBLISHER(nh, + ff_msgs::msg::CommandStamped, + TOPIC_COMMAND, + 5); + + Subscriber cf_ack_sub = + FF_CREATE_SUBSCRIBER(nh, + ff_msgs::msg::CompressedFileAck, + TOPIC_MANAGEMENT_EXEC_CF_ACK, + 10, + std::bind(&on_cf_ack, std::placeholders::_1)); + + + Subscriber plan_status_sub = + FF_CREATE_SUBSCRIBER(nh, + ff_msgs::msg::PlanStatusStamped, + TOPIC_MANAGEMENT_EXEC_PLAN_STATUS, + 10, + std::bind(&on_plan_status, std::placeholders::_1)); + + std::chrono::nanoseconds ns(1000000000); + while (nh->count_publishers(TOPIC_MANAGEMENT_EXEC_CF_ACK) == 0 || + nh->count_publishers(TOPIC_MANAGEMENT_EXEC_PLAN_STATUS) == 0 || + command_pub->get_subscription_count() == 0) { + rclcpp::sleep_for(ns); + } - plan_pub_time = ros::Time::now(); - std::string sub_topic_plan = TOPIC_COMMUNICATIONS_DDS_PLAN; - - std::string sub_topic_command = TOPIC_COMMAND; - command_pub = n.advertise(sub_topic_command, 5); - - ros::Subscriber cf_ack_sub = n.subscribe(TOPIC_MANAGEMENT_EXEC_CF_ACK, 10, - &on_cf_ack); - - ros::Subscriber plan_status_sub = - n.subscribe(TOPIC_MANAGEMENT_EXEC_PLAN_STATUS, 10, &on_plan_status); + plan_pub = FF_CREATE_PUBLISHER(nh, + ff_msgs::msg::CompressedFile, + TOPIC_COMMUNICATIONS_DDS_PLAN, + 10); - while (cf_ack_sub.getNumPublishers() == 0 || - plan_status_sub.getNumPublishers() == 0 || - command_pub.getNumSubscribers() == 0) { - ros::Duration(0.5).sleep(); + if (plan_pub->get_subscription_count() == 0) { + data_sub_connected_timer.createTimer(1.0, &TimerCallback, nh); } - ros::Publisher plan_pub = - n.advertise(sub_topic_plan, 10, - std::bind(&on_connect, std::placeholders::_1, cf)); - - ROS_INFO("waiting for a subscriber..."); - ros::spin(); + rclcpp::spin(nh); return 0; } diff --git a/management/executive/tools/simple_move.cc b/management/executive/tools/simple_move.cc index 41b0bc3f20..fe13d43412 100644 --- a/management/executive/tools/simple_move.cc +++ b/management/executive/tools/simple_move.cc @@ -16,68 +16,74 @@ * under the License. */ -#include - -#include -#include -#include -#include -#include -#include #include +#include + +#include +#include +#include +#include +#include +#include #include +FF_DEFINE_LOGGER("simple_move") + std::string unique_cmd_id; -void AckCallback(ff_msgs::AckStampedConstPtr const& Ack) { +void AckCallback(ff_msgs::msg::AckStamped::SharedPtr const Ack) { // Check if the ack corresponds to the command we sent if (Ack->cmd_id == unique_cmd_id) { // Check if command hasn't completed - if (Ack->completed_status.status == ff_msgs::AckCompletedStatus::NOT) { - ROS_INFO("Move command is being executed and has not completed."); + if (Ack->completed_status.status == ff_msgs::msg::AckCompletedStatus::NOT) { + FF_INFO("Move command is being executed and has not completed."); return; // Return so we don't shut down prematurely - } else if (Ack->completed_status.status == ff_msgs::AckCompletedStatus::OK) { + } else if (Ack->completed_status.status == + ff_msgs::msg::AckCompletedStatus::OK) { // Command completed successfully - ROS_INFO("Move command completed successfully!"); + FF_INFO("Move command completed successfully!"); } else { // Command failed - ROS_INFO("Move command failed! Message: %s", Ack->message.c_str()); + FF_INFO("Move command failed! Message: %s", Ack->message.c_str()); } } // Command finshed so exit - ros::shutdown(); + rclcpp::shutdown(); } int main(int argc, char** argv) { - ros::init(argc, argv, "teleop_pub"); - ros::NodeHandle nh; + rclcpp::init(argc, argv); + NodeHandle nh; if (argc <= 1) { - ROS_ERROR("Error! Must provide x, y, and z as arguments!"); + FF_ERROR("Error! Must provide x, y, and z as arguments!"); return -1; } // Make publisher to publish the command - ros::Publisher cmd_publisher; - cmd_publisher = nh.advertise(TOPIC_COMMAND, 10); + Publisher cmd_publisher = + FF_CREATE_PUBLISHER(nh, ff_msgs::msg::CommandStamped, TOPIC_COMMAND, 10); // Make subscriber to receive command acks to see if the command completed // successfully - ros::Subscriber ack_subscriber; - ack_subscriber = nh.subscribe(TOPIC_MANAGEMENT_ACK, 10, &AckCallback); + Subscriber ack_subscriber = + FF_CREATE_SUBSCRIBER(nh, + ff_msgs::msg::AckStamped, + TOPIC_MANAGEMENT_ACK, + 10, + std::bind(&AckCallback, std::placeholders::_1)); // Make ros command message to send to the executive - ff_msgs::CommandStamped move_cmd; - - move_cmd.header.stamp = ros::Time::now(); + ff_msgs::msg::CommandStamped move_cmd; // Command names listed in CommandConstants.h - move_cmd.cmd_name = ff_msgs::CommandConstants::CMD_NAME_SIMPLE_MOVE6DOF; + move_cmd.cmd_name = ff_msgs::msg::CommandConstants::CMD_NAME_SIMPLE_MOVE6DOF; // Command id needs to be a unique id that you will use make sure the command // was executed, usually a combination of username and timestamp - unique_cmd_id = "guest_science" + std::to_string(move_cmd.header.stamp.sec); + unique_cmd_id = "guest_science" + + std::to_string(nh->get_clock()->now().seconds()); move_cmd.cmd_id = unique_cmd_id; // Source of the command, set to guest_science so that the system knows that @@ -92,24 +98,24 @@ int main(int argc, char** argv) { // Set frame to be world, although I don't believe frame is being used so you // can really set it to anything - move_cmd.args[0].data_type = ff_msgs::CommandArg::DATA_TYPE_STRING; + move_cmd.args[0].data_type = ff_msgs::msg::CommandArg::DATA_TYPE_STRING; move_cmd.args[0].s = "world"; // Set location where you want Astrobee to go to - move_cmd.args[1].data_type = ff_msgs::CommandArg::DATA_TYPE_VEC3d; + move_cmd.args[1].data_type = ff_msgs::msg::CommandArg::DATA_TYPE_VEC3D; move_cmd.args[1].vec3d[0] = atof(argv[1]); // x move_cmd.args[1].vec3d[1] = atof(argv[2]); // y move_cmd.args[1].vec3d[2] = atof(argv[3]); // z (This axis may not currently work // Tolerance not used! If you want to set the tolerance, you need to use the // set operational limits command. I set tolerance to 0 - move_cmd.args[2].data_type = ff_msgs::CommandArg::DATA_TYPE_VEC3d; + move_cmd.args[2].data_type = ff_msgs::msg::CommandArg::DATA_TYPE_VEC3D; move_cmd.args[2].vec3d[0] = 0; move_cmd.args[2].vec3d[1] = 0; move_cmd.args[2].vec3d[2] = 0; // Target attitude, quaternion, only the first 4 values are used - move_cmd.args[3].data_type = ff_msgs::CommandArg::DATA_TYPE_MAT33f; + move_cmd.args[3].data_type = ff_msgs::msg::CommandArg::DATA_TYPE_MAT33F; move_cmd.args[3].mat33f[0] = 0; move_cmd.args[3].mat33f[1] = 0; move_cmd.args[3].mat33f[2] = 0; @@ -121,9 +127,9 @@ int main(int argc, char** argv) { move_cmd.args[3].mat33f[8] = 0; // Send command - cmd_publisher.publish(move_cmd); + cmd_publisher->publish(move_cmd); - ROS_INFO("waiting for executive to execute the command..."); - ros::spin(); + FF_INFO("waiting for executive to execute the command..."); + rclcpp::spin(nh); return 0; } diff --git a/management/executive/tools/teleop_tool.cc b/management/executive/tools/teleop_tool.cc index f38bffe202..a25719e843 100644 --- a/management/executive/tools/teleop_tool.cc +++ b/management/executive/tools/teleop_tool.cc @@ -21,24 +21,28 @@ #include // Include ROS -#include +#include +#include +#include +#include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + #include -#include // Gflags DEFINE_bool(dock, false, "Send dock command"); @@ -76,9 +80,9 @@ bool get_face_forward, get_op_limits, get_planner, get_state, get_faults; bool reset_bias, reset_ekf, set_check_zones, set_face_forward, set_planner; bool set_op_limits, send_mob_command, mob_command_finished; -geometry_msgs::TransformStamped tfs; +geometry_msgs::msg::TransformStamped tfs; -ros::Publisher cmd_pub; +Publisher cmd_pub; uint8_t modeMove = 0, modeGetInfo = 0; @@ -93,7 +97,8 @@ bool Finished() { return false; } -void AgentStateCallback(ff_msgs::AgentStateStampedConstPtr const& state) { +void AgentStateCallback( + ff_msgs::msg::AgentStateStamped::SharedPtr const state) { if (get_face_forward) { get_face_forward = false; if (state->holonomic_enabled) { @@ -126,19 +131,19 @@ void AgentStateCallback(ff_msgs::AgentStateStampedConstPtr const& state) { get_state = false; std::string msg = "\nOperating State: "; switch (state->operating_state.state) { - case ff_msgs::OpState::READY: + case ff_msgs::msg::OpState::READY: msg += "Ready\n"; break; - case ff_msgs::OpState::PLAN_EXECUTION: + case ff_msgs::msg::OpState::PLAN_EXECUTION: msg += "Plan Execution\n"; break; - case ff_msgs::OpState::TELEOPERATION: + case ff_msgs::msg::OpState::TELEOPERATION: msg += "Teleoperation\n"; break; - case ff_msgs::OpState::AUTO_RETURN: + case ff_msgs::msg::OpState::AUTO_RETURN: msg += "Auto Return\n"; break; - case ff_msgs::OpState::FAULT: + case ff_msgs::msg::OpState::FAULT: msg += "Fault\n"; break; default: @@ -147,49 +152,54 @@ void AgentStateCallback(ff_msgs::AgentStateStampedConstPtr const& state) { msg += "Mobility State: "; switch (state->mobility_state.state) { - case ff_msgs::MobilityState::DRIFTING: + case ff_msgs::msg::MobilityState::DRIFTING: msg += "Drifting"; break; - case ff_msgs::MobilityState::STOPPING: + case ff_msgs::msg::MobilityState::STOPPING: if (state->mobility_state.sub_state == 0) { msg += "Stopped"; } else { msg += "Stopping"; } break; - case ff_msgs::MobilityState::FLYING: + case ff_msgs::msg::MobilityState::FLYING: msg += "Flying"; break; - case ff_msgs::MobilityState::DOCKING: - if (state->mobility_state.sub_state == ff_msgs::DockState::DOCKED) { + case ff_msgs::msg::MobilityState::DOCKING: + if (state->mobility_state.sub_state == + ff_msgs::msg::DockState::DOCKED) { msg += "Docked"; } else if (state->mobility_state.sub_state < - ff_msgs::DockState::DOCKED) { + ff_msgs::msg::DockState::DOCKED) { msg += "Undocking (on step "; msg += std::to_string(state->mobility_state.sub_state * -1); msg += " of "; - msg += std::to_string(((ff_msgs::DockState::UNDOCKING_MAX_STATE * -1) + msg += + std::to_string(((ff_msgs::msg::DockState::UNDOCKING_MAX_STATE * -1) - 1)); msg += ")"; } else { msg += "Docking (on step "; - msg += std::to_string((ff_msgs::DockState::DOCKING_MAX_STATE - + msg += std::to_string((ff_msgs::msg::DockState::DOCKING_MAX_STATE - state->mobility_state.sub_state)); - msg += " of " + std::to_string(ff_msgs::DockState::DOCKING_MAX_STATE); + msg += " of "; + msg += std::to_string(ff_msgs::msg::DockState::DOCKING_MAX_STATE); msg += ")"; } break; - case ff_msgs::MobilityState::PERCHING: + case ff_msgs::msg::MobilityState::PERCHING: if (state->mobility_state.sub_state == 0) { msg += "Perched"; } else if (state->mobility_state.sub_state < 0) { msg += "Unperching (on step " + (state->mobility_state.sub_state * -1); - msg += " of " + (ff_msgs::PerchState::UNPERCHING_MAX_STATE * -1); + msg += " of "; + msg += (ff_msgs::msg::PerchState::UNPERCHING_MAX_STATE * -1); msg += ")"; } else { msg += "Perching (on step " + state->mobility_state.sub_state; - msg += " of " + ff_msgs::PerchState::PERCHING_MAX_STATE; + msg += " of "; + msg += ff_msgs::msg::PerchState::PERCHING_MAX_STATE; msg += ")"; } break; @@ -200,11 +210,11 @@ void AgentStateCallback(ff_msgs::AgentStateStampedConstPtr const& state) { } if (Finished()) { - ros::shutdown(); + rclcpp::shutdown(); } } -void FaultStateCallback(ff_msgs::FaultStateConstPtr const& state) { +void FaultStateCallback(ff_msgs::msg::FaultState::SharedPtr const state) { get_faults = false; // Output faults @@ -223,43 +233,43 @@ void FaultStateCallback(ff_msgs::FaultStateConstPtr const& state) { } bool SendMobilityCommand() { - ff_msgs::CommandStamped cmd; - cmd.header.stamp = ros::Time::now(); + ff_msgs::msg::CommandStamped cmd; cmd.subsys_name = "Astrobee"; if (FLAGS_dock) { - cmd.cmd_name = ff_msgs::CommandConstants::CMD_NAME_DOCK; - cmd.cmd_id = ff_msgs::CommandConstants::CMD_NAME_DOCK; + cmd.cmd_name = ff_msgs::msg::CommandConstants::CMD_NAME_DOCK; + cmd.cmd_id = ff_msgs::msg::CommandConstants::CMD_NAME_DOCK; // Dock has one argument cmd.args.resize(1); - cmd.args[0].data_type = ff_msgs::CommandArg::DATA_TYPE_INT; + cmd.args[0].data_type = ff_msgs::msg::CommandArg::DATA_TYPE_INT; cmd.args[0].i = FLAGS_berth; } if (FLAGS_move) { - cmd.cmd_name = ff_msgs::CommandConstants::CMD_NAME_SIMPLE_MOVE6DOF; - cmd.cmd_id = ff_msgs::CommandConstants::CMD_NAME_SIMPLE_MOVE6DOF; + cmd.cmd_name = ff_msgs::msg::CommandConstants::CMD_NAME_SIMPLE_MOVE6DOF; + cmd.cmd_id = ff_msgs::msg::CommandConstants::CMD_NAME_SIMPLE_MOVE6DOF; // Move command has 4 arguements cmd.args.resize(4); // Set frame - cmd.args[0].data_type = ff_msgs::CommandArg::DATA_TYPE_STRING; + cmd.args[0].data_type = ff_msgs::msg::CommandArg::DATA_TYPE_STRING; if (FLAGS_relative) { - cmd.args[0].s = (FLAGS_ns.empty() ? "body" : FLAGS_ns + "/" + std::string(FRAME_NAME_BODY)); + cmd.args[0].s = (FLAGS_ns.empty() ? "body" : FLAGS_ns + "/" + + std::string(FRAME_NAME_BODY)); } else { cmd.args[0].s = "world"; } // Set tolerance. Currently not used but needs to be in the command - cmd.args[2].data_type = ff_msgs::CommandArg::DATA_TYPE_VEC3d; + cmd.args[2].data_type = ff_msgs::msg::CommandArg::DATA_TYPE_VEC3D; cmd.args[2].vec3d[0] = 0; cmd.args[2].vec3d[1] = 0; cmd.args[2].vec3d[2] = 0; // Initialize position to be the current position if not a relative move - cmd.args[1].data_type = ff_msgs::CommandArg::DATA_TYPE_VEC3d; + cmd.args[1].data_type = ff_msgs::msg::CommandArg::DATA_TYPE_VEC3D; if (FLAGS_relative) { cmd.args[1].vec3d[0] = 0; cmd.args[1].vec3d[1] = 0; @@ -293,7 +303,7 @@ bool SendMobilityCommand() { } // Parse and set the attitude - roll, pitch then yaw - cmd.args[3].data_type = ff_msgs::CommandArg::DATA_TYPE_MAT33f; + cmd.args[3].data_type = ff_msgs::msg::CommandArg::DATA_TYPE_MAT33F; if (FLAGS_att.empty()) { if (FLAGS_relative) { cmd.args[3].mat33f[0] = 0; @@ -351,16 +361,16 @@ bool SendMobilityCommand() { } if (FLAGS_stop) { - cmd.cmd_name = ff_msgs::CommandConstants::CMD_NAME_STOP_ALL_MOTION; - cmd.cmd_id = ff_msgs::CommandConstants::CMD_NAME_STOP_ALL_MOTION; + cmd.cmd_name = ff_msgs::msg::CommandConstants::CMD_NAME_STOP_ALL_MOTION; + cmd.cmd_id = ff_msgs::msg::CommandConstants::CMD_NAME_STOP_ALL_MOTION; } if (FLAGS_undock) { - cmd.cmd_name = ff_msgs::CommandConstants::CMD_NAME_UNDOCK; - cmd.cmd_id = ff_msgs::CommandConstants::CMD_NAME_UNDOCK; + cmd.cmd_name = ff_msgs::msg::CommandConstants::CMD_NAME_UNDOCK; + cmd.cmd_id = ff_msgs::msg::CommandConstants::CMD_NAME_UNDOCK; } - cmd_pub.publish(cmd); + cmd_pub->publish(cmd); // Signify that the main mobility command has been sent send_mob_command = false; std::cout << "\nStarted " << cmd.cmd_id << " command. It may take some time "; @@ -369,14 +379,13 @@ bool SendMobilityCommand() { } bool SendResetBias() { - ff_msgs::CommandStamped cmd; - cmd.header.stamp = ros::Time::now(); + ff_msgs::msg::CommandStamped cmd; cmd.subsys_name = "Astrobee"; - cmd.cmd_name = ff_msgs::CommandConstants::CMD_NAME_INITIALIZE_BIAS; - cmd.cmd_id = ff_msgs::CommandConstants::CMD_NAME_INITIALIZE_BIAS; + cmd.cmd_name = ff_msgs::msg::CommandConstants::CMD_NAME_INITIALIZE_BIAS; + cmd.cmd_id = ff_msgs::msg::CommandConstants::CMD_NAME_INITIALIZE_BIAS; - cmd_pub.publish(cmd); + cmd_pub->publish(cmd); // Change to false so we don't send the command again reset_bias = false; std::cout << "\nResetting the bias.\n"; @@ -384,14 +393,13 @@ bool SendResetBias() { } bool SendResetEkf() { - ff_msgs::CommandStamped cmd; - cmd.header.stamp = ros::Time::now(); + ff_msgs::msg::CommandStamped cmd; cmd.subsys_name = "Astrobee"; - cmd.cmd_name = ff_msgs::CommandConstants::CMD_NAME_RESET_EKF; - cmd.cmd_id = ff_msgs::CommandConstants::CMD_NAME_RESET_EKF; + cmd.cmd_name = ff_msgs::msg::CommandConstants::CMD_NAME_RESET_EKF; + cmd.cmd_id = ff_msgs::msg::CommandConstants::CMD_NAME_RESET_EKF; - cmd_pub.publish(cmd); + cmd_pub->publish(cmd); // Change to false so we don't send the command again reset_ekf = false; std::cout << "\nResetting the ekf.\n"; @@ -399,23 +407,22 @@ bool SendResetEkf() { } bool SendSetCheckZones() { - ff_msgs::CommandStamped cmd; - cmd.header.stamp = ros::Time::now(); + ff_msgs::msg::CommandStamped cmd; cmd.subsys_name = "Astrobee"; - cmd.cmd_name = ff_msgs::CommandConstants::CMD_NAME_SET_CHECK_ZONES; - cmd.cmd_id = ff_msgs::CommandConstants::CMD_NAME_SET_CHECK_ZONES; + cmd.cmd_name = ff_msgs::msg::CommandConstants::CMD_NAME_SET_CHECK_ZONES; + cmd.cmd_id = ff_msgs::msg::CommandConstants::CMD_NAME_SET_CHECK_ZONES; // Set check zones has one argument cmd.args.resize(1); - cmd.args[0].data_type = ff_msgs::CommandArg::DATA_TYPE_BOOL; + cmd.args[0].data_type = ff_msgs::msg::CommandArg::DATA_TYPE_BOOL; if (FLAGS_set_check_zones == "on") { cmd.args[0].b = true; } else { cmd.args[0].b = false; } - cmd_pub.publish(cmd); + cmd_pub->publish(cmd); // Change to false so we don't send the command again set_check_zones = false; std::cout << "\nSetting check zones.\n"; @@ -423,19 +430,18 @@ bool SendSetCheckZones() { } bool SendSetFaceForward() { - ff_msgs::CommandStamped cmd; - cmd.header.stamp = ros::Time::now(); + ff_msgs::msg::CommandStamped cmd; cmd.subsys_name = "Astrobee"; - cmd.cmd_name = ff_msgs::CommandConstants::CMD_NAME_SET_HOLONOMIC_MODE; + cmd.cmd_name = ff_msgs::msg::CommandConstants::CMD_NAME_SET_HOLONOMIC_MODE; // For this example only, we will set the command id to the name command // This not standard so please don't do it. This should be a unique id, // usually a combination of username and timestamp - cmd.cmd_id = ff_msgs::CommandConstants::CMD_NAME_SET_HOLONOMIC_MODE; + cmd.cmd_id = ff_msgs::msg::CommandConstants::CMD_NAME_SET_HOLONOMIC_MODE; // Set holonomic has one argument cmd.args.resize(1); - cmd.args[0].data_type = ff_msgs::CommandArg::DATA_TYPE_BOOL; + cmd.args[0].data_type = ff_msgs::msg::CommandArg::DATA_TYPE_BOOL; if (FLAGS_set_face_forward == "on") { // Holonomic means don't fly face forward so we need to set holonomic to // false when we want to face forward @@ -444,7 +450,7 @@ bool SendSetFaceForward() { cmd.args[0].b = true; } - cmd_pub.publish(cmd); + cmd_pub->publish(cmd); // Change to false so we don't send command again set_face_forward = false; std::cout << "\nSetting holonomic (face forward) mode.\n"; @@ -452,49 +458,48 @@ bool SendSetFaceForward() { } bool SendSetOpLimits() { - ff_msgs::CommandStamped cmd; - cmd.header.stamp = ros::Time::now(); + ff_msgs::msg::CommandStamped cmd; cmd.subsys_name = "Astrobee"; - cmd.cmd_name = ff_msgs::CommandConstants::CMD_NAME_SET_OPERATING_LIMITS; - cmd.cmd_id = ff_msgs::CommandConstants::CMD_NAME_SET_OPERATING_LIMITS; + cmd.cmd_name = ff_msgs::msg::CommandConstants::CMD_NAME_SET_OPERATING_LIMITS; + cmd.cmd_id = ff_msgs::msg::CommandConstants::CMD_NAME_SET_OPERATING_LIMITS; // Set op limits has seven arguments cmd.args.resize(7); // Set profile name - cmd.args[0].data_type = ff_msgs::CommandArg::DATA_TYPE_STRING; + cmd.args[0].data_type = ff_msgs::msg::CommandArg::DATA_TYPE_STRING; cmd.args[0].s = "user_profile"; // Set flight mode - cmd.args[1].data_type = ff_msgs::CommandArg::DATA_TYPE_STRING; + cmd.args[1].data_type = ff_msgs::msg::CommandArg::DATA_TYPE_STRING; cmd.args[1].s = FLAGS_mode; // Set desired limits // Target Linear Velocity - cmd.args[2].data_type = ff_msgs::CommandArg::DATA_TYPE_FLOAT; + cmd.args[2].data_type = ff_msgs::msg::CommandArg::DATA_TYPE_FLOAT; cmd.args[2].f = FLAGS_vel; // Target Linear Acceleration - cmd.args[3].data_type = ff_msgs::CommandArg::DATA_TYPE_FLOAT; + cmd.args[3].data_type = ff_msgs::msg::CommandArg::DATA_TYPE_FLOAT; cmd.args[3].f = FLAGS_accel; // Target Angular Velocity - cmd.args[4].data_type = ff_msgs::CommandArg::DATA_TYPE_FLOAT; + cmd.args[4].data_type = ff_msgs::msg::CommandArg::DATA_TYPE_FLOAT; cmd.args[4].f = FLAGS_omega; // Target Angular Acceleration - cmd.args[5].data_type = ff_msgs::CommandArg::DATA_TYPE_FLOAT; + cmd.args[5].data_type = ff_msgs::msg::CommandArg::DATA_TYPE_FLOAT; cmd.args[5].f = FLAGS_alpha; // Set collision distance - cmd.args[6].data_type = ff_msgs::CommandArg::DATA_TYPE_FLOAT; + cmd.args[6].data_type = ff_msgs::msg::CommandArg::DATA_TYPE_FLOAT; cmd.args[6].f = FLAGS_collision_distance; if (cmd.args[6].f == -1) { cmd.args[6].f = 0.25; } - cmd_pub.publish(cmd); + cmd_pub->publish(cmd); // Change set op limits to false so we don't send the command again set_op_limits = false; std::cout << "\nSetting operating limits.\n"; @@ -502,19 +507,18 @@ bool SendSetOpLimits() { } bool SendSetPlanner() { - ff_msgs::CommandStamped cmd; - cmd.header.stamp = ros::Time::now(); + ff_msgs::msg::CommandStamped cmd; cmd.subsys_name = "Astrobee"; - cmd.cmd_name = ff_msgs::CommandConstants::CMD_NAME_SET_PLANNER; - cmd.cmd_id = ff_msgs::CommandConstants::CMD_NAME_SET_PLANNER; + cmd.cmd_name = ff_msgs::msg::CommandConstants::CMD_NAME_SET_PLANNER; + cmd.cmd_id = ff_msgs::msg::CommandConstants::CMD_NAME_SET_PLANNER; // Set planner has one argument cmd.args.resize(1); - cmd.args[0].data_type = ff_msgs::CommandArg::DATA_TYPE_STRING; + cmd.args[0].data_type = ff_msgs::msg::CommandArg::DATA_TYPE_STRING; cmd.args[0].s = FLAGS_set_planner; - cmd_pub.publish(cmd); + cmd_pub->publish(cmd); // Change to false so we don't send command again set_planner = false; std::cout << "\nSetting planner.\n"; @@ -558,10 +562,11 @@ bool SendNextCommand() { return true; } -void AckCallback(ff_msgs::AckStampedConstPtr const& ack) { - if (ack->completed_status.status == ff_msgs::AckCompletedStatus::NOT) { +void AckCallback(ff_msgs::msg::AckStamped::SharedPtr const ack) { + if (ack->completed_status.status == ff_msgs::msg::AckCompletedStatus::NOT) { return; - } else if (ack->completed_status.status == ff_msgs::AckCompletedStatus::OK) { + } else if (ack->completed_status.status == + ff_msgs::msg::AckCompletedStatus::OK) { std::cout << "\n" << ack->cmd_id << " command completed successfully!\n"; // Check if the mobility command was sent and if it was, we can say the // mobility command was finished since we only send one command at a time @@ -571,60 +576,59 @@ void AckCallback(ff_msgs::AckStampedConstPtr const& ack) { } if (!SendNextCommand()) { - ros::shutdown(); + rclcpp::shutdown(); return; } } else if (ack->completed_status.status == - ff_msgs::AckCompletedStatus::CANCELED) { + ff_msgs::msg::AckCompletedStatus::CANCELED) { std::cout << "\n" << ack->cmd_id << " command was cancelled. This may have"; std::cout << " been due to a fault in the system or if someone had issued "; std::cout << "another command. Aborting!\n"; - ros::shutdown(); + rclcpp::shutdown(); return; } else { // Command failed due to bad syntax or an actual failure std::cout << "\n" << ack->cmd_id << " command failed! " << ack->message; std::cout << "\n"; - ros::shutdown(); + rclcpp::shutdown(); return; } if (Finished()) { - ros::shutdown(); + rclcpp::shutdown(); } } -void MoveFeedbackCallback(ff_msgs::MotionActionFeedbackConstPtr const& fb) { +void EkfStateCallback(ff_msgs::msg::EkfState::SharedPtr const state) { std::cout << '\r' << std::flush; std::cout << std::fixed << std::setprecision(2) - << "pos: x: " << fb->feedback.progress.setpoint.pose.position.x - << " y: " << fb->feedback.progress.setpoint.pose.position.y - << " z: " << fb->feedback.progress.setpoint.pose.position.z - << " att: x: " << fb->feedback.progress.setpoint.pose.orientation.x - << " y: " << fb->feedback.progress.setpoint.pose.orientation.y - << " z: " << fb->feedback.progress.setpoint.pose.orientation.z - << " w: " << fb->feedback.progress.setpoint.pose.orientation.w; + << "pos: x: " << state->pose.position.x + << " y: " << state->pose.position.y + << " z: " << state->pose.position.z + << " att: x: " << state->pose.orientation.x + << " y: " << state->pose.orientation.y + << " z: " << state->pose.orientation.z + << " w: " << state->pose.orientation.w; } -// TODO(Katie) Finish me -void DockFeedbackCallback(ff_msgs::DockActionFeedbackConstPtr const& fb) { - if (fb->feedback.state.state > ff_msgs::DockState::INITIALIZING) { +void DockStateCallback(ff_msgs::msg::DockState::SharedPtr const state) { + if (state->state > ff_msgs::msg::DockState::INITIALIZING) { std::cout << "Astrobee failed to un/dock and is trying to recover.\n\n"; - } else if (fb->feedback.state.state < ff_msgs::DockState::DOCKED) { - std::cout << "Undocking " << (fb->feedback.state.state * -1) << " (of " << - ((ff_msgs::DockState::UNDOCKING_MAX_STATE * -1) - 1) << ")\n"; - } else if (fb->feedback.state.state < ff_msgs::DockState::UNKNOWN && - fb->feedback.state.state > ff_msgs::DockState::DOCKED) { + } else if (state->state < ff_msgs::msg::DockState::DOCKED) { + std::cout << "Undocking " << (state->state * -1) << " (of " << + ((ff_msgs::msg::DockState::UNDOCKING_MAX_STATE * -1) - 1) << ")\n"; + } else if (state->state < ff_msgs::msg::DockState::UNKNOWN && + state->state > ff_msgs::msg::DockState::DOCKED) { std::cout << "Docking " << - std::to_string((ff_msgs::DockState::DOCKING_MAX_STATE - - fb->feedback.state.state + 1)) << " (of " << - ff_msgs::DockState::DOCKING_MAX_STATE << ")\n"; + std::to_string((ff_msgs::msg::DockState::DOCKING_MAX_STATE - + state->state + 1)) << " (of " << + ff_msgs::msg::DockState::DOCKING_MAX_STATE << ")\n"; } } // Main entry point for application int main(int argc, char** argv) { // Initialize a ros node - ros::init(argc, argv, "simple_move", ros::init_options::AnonymousName); + rclcpp::init(argc, argv); // Gather some data from the command google::SetUsageMessage("Usage: rosrun executive simple_move "); @@ -721,23 +725,40 @@ int main(int argc, char** argv) { } // Create a node handle - ros::NodeHandle nh(std::string("/") + FLAGS_ns); + // TODO(Katie or Marina) It seems like the node name passed to the Node + // object doesn't actually do anything. Does the namespace? + NodeHandle nh = std::make_shared("teleop_tool", + ("/" + FLAGS_ns)); // TF2 Subscriber - tf2_ros::Buffer tf_buffer; - tf2_ros::TransformListener tf_listener(tf_buffer); + std::shared_ptr tf_buffer = + std::shared_ptr(new tf2_ros::Buffer(nh->get_clock())); + std::shared_ptr tf_listener = + std::shared_ptr(new + tf2_ros::TransformListener(*tf_buffer)); // Initialize publishers - cmd_pub = nh.advertise(TOPIC_COMMAND, 10); + cmd_pub = FF_CREATE_PUBLISHER(nh, + ff_msgs::msg::CommandStamped, + TOPIC_COMMAND, + 10); // Initialize subscribers - ros::Subscriber ack_sub, agent_state_sub, fault_state_sub, dock_sub, move_sub; - ack_sub = nh.subscribe(TOPIC_MANAGEMENT_ACK, 10, &AckCallback); + Subscriber ack_sub = FF_CREATE_SUBSCRIBER(nh, + ff_msgs::msg::AckStamped, + TOPIC_MANAGEMENT_ACK, + 10, + std::bind(&AckCallback, std::placeholders::_1)); + Subscriber agent_state_sub; + Subscriber fault_state_sub; + Subscriber dock_sub; + Subscriber ekf_sub; // Hacky time out int count = 0; - while (ack_sub.getNumPublishers() == 0) { - ros::Duration(0.2).sleep(); + std::chrono::nanoseconds nanoseconds(200000000); + while (nh->count_publishers(TOPIC_MANAGEMENT_ACK) == 0) { + rclcpp::sleep_for(nanoseconds); // Only wait 2 seconds if (count == 9) { std::cout << "No publisher for acks topics. This tool will not work "; @@ -751,11 +772,11 @@ int main(int argc, char** argv) { if (FLAGS_get_pose || FLAGS_move) { std::string ns = FLAGS_ns; // Wait for transform listener to start up - ros::Duration(1.0).sleep(); + rclcpp::sleep_for(nanoseconds); try { - tfs = tf_buffer.lookupTransform(std::string(FRAME_NAME_WORLD), + tfs = tf_buffer->lookupTransform(std::string(FRAME_NAME_WORLD), (ns.empty() ? "body" : ns + "/" + std::string(FRAME_NAME_BODY)), - ros::Time(0)); + rclcpp::Time(0)); } catch (tf2::TransformException &ex) { std::cout << "Could not query the pose of robot: " << ex.what() << "\n\n"; return 1; @@ -779,47 +800,55 @@ int main(int argc, char** argv) { if (FLAGS_get_state || FLAGS_get_face_forward || FLAGS_get_op_limits || FLAGS_get_planner) { - agent_state_sub = nh.subscribe(TOPIC_MANAGEMENT_EXEC_AGENT_STATE, - 10, - &AgentStateCallback); + agent_state_sub = FF_CREATE_SUBSCRIBER(nh, + ff_msgs::msg::AgentStateStamped, + TOPIC_MANAGEMENT_EXEC_AGENT_STATE, + 10, + std::bind(&AgentStateCallback, std::placeholders::_1)); } if (FLAGS_get_faults) { - fault_state_sub = nh.subscribe(TOPIC_MANAGEMENT_SYS_MONITOR_STATE, - 10, - &FaultStateCallback); + fault_state_sub = FF_CREATE_SUBSCRIBER(nh, + ff_msgs::msg::FaultState, + TOPIC_MANAGEMENT_SYS_MONITOR_STATE, + 10, + std::bind(&FaultStateCallback, std::placeholders::_1)); } if (FLAGS_move) { - std::string topic_name = ACTION_MOBILITY_MOTION; - topic_name += "/feedback"; - move_sub = nh.subscribe(topic_name, 10, &MoveFeedbackCallback); + ekf_sub = FF_CREATE_SUBSCRIBER(nh, + ff_msgs::msg::EkfState, + TOPIC_GNC_EKF, + 10, + std::bind(&EkfStateCallback, std::placeholders::_1)); } if (FLAGS_dock || FLAGS_undock) { - std::string topic_name = ACTION_BEHAVIORS_DOCK; - topic_name += "/feedback"; - dock_sub = nh.subscribe(topic_name, 10, &DockFeedbackCallback); + dock_sub = FF_CREATE_SUBSCRIBER(nh, + ff_msgs::msg::DockState, + TOPIC_BEHAVIORS_DOCKING_STATE, + 10, + std::bind(&DockStateCallback, std::placeholders::_1)); // Hacky time out int dock_count = 0; - while (dock_sub.getNumPublishers() == 0) { - ros::Duration(0.2).sleep(); + while (nh->count_publishers(TOPIC_BEHAVIORS_DOCKING_STATE) == 0) { + rclcpp::sleep_for(nanoseconds); // Only wait 2 seconds if (dock_count == 9) { - std::cout << "No publisher for dock feedback. This tool will not work "; + std::cout << "No publisher for dock state. This tool will not work "; std::cout << "without this.\n\n"; return 1; } dock_count++; } } - + if (!SendNextCommand()) { return 1; } - + // Synchronous mode - ros::spin(); + rclcpp::spin(nh); // Finish commandline flags google::ShutDownCommandLineFlags(); diff --git a/management/executive/tools/zones_pub.cc b/management/executive/tools/zones_pub.cc index 4c33572c99..a03e858361 100644 --- a/management/executive/tools/zones_pub.cc +++ b/management/executive/tools/zones_pub.cc @@ -16,14 +16,16 @@ * under the License. */ +#include +#include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include + +#include #include @@ -44,12 +46,20 @@ namespace fs = boost::filesystem; namespace io = boost::iostreams; +FF_DEFINE_LOGGER("zones_pub") + DEFINE_string(compression, "none", "Type of compression [none, deflate, gzip]"); constexpr uintmax_t kMaxSize = 128 * 1024; -ros::Time zones_pub_time; -ros::Publisher command_pub; + +rclcpp::Time zones_pub_time; + +Publisher command_pub; +Publisher zones_pub; + +ff_msgs::msg::CompressedFile cf; +ff_util::FreeFlyerTimer data_sub_connected_timer; bool ValidateCompression(const char* name, std::string const &value) { if (value == "none" || value == "gzip" || value == "deflate") @@ -60,29 +70,35 @@ bool ValidateCompression(const char* name, std::string const &value) { return false; } -void on_connect(ros::SingleSubscriberPublisher const& sub, - ff_msgs::CompressedFile &cf) { // NOLINT - ROS_INFO("subscriber present: sending zones"); - cf.header.stamp = ros::Time::now(); - sub.publish(cf); +void on_connect() { + FF_INFO("subscriber present: sending zones"); + zones_pub->publish(cf); } -void on_cf_ack(ff_msgs::CompressedFileAckConstPtr const& cf_ack) { - ROS_INFO("ack received: sending zone update command"); +void on_cf_ack(ff_msgs::msg::CompressedFileAck::SharedPtr const cf_ack) { + FF_INFO("ack received: sending zone update command"); // compressed file ack is latched so we need to check the timestamp to make // sure this plan is being acked if (zones_pub_time < cf_ack->header.stamp) { - ff_msgs::CommandStamped cmd; - cmd.cmd_name = ff_msgs::CommandConstants::CMD_NAME_SET_ZONES; + ff_msgs::msg::CommandStamped cmd; + cmd.cmd_name = ff_msgs::msg::CommandConstants::CMD_NAME_SET_ZONES; cmd.subsys_name = "Astrobee"; - command_pub.publish(cmd); - ros::shutdown(); + command_pub->publish(cmd); + rclcpp::shutdown(); + } +} + +void TimerCallback() { + if (zones_pub->get_subscription_count() > 0) { + on_connect(); + data_sub_connected_timer.stop(); } } int main(int argc, char** argv) { ff_common::InitFreeFlyerApplication(&argc, &argv); - ros::init(argc, argv, "zone_publisher"); + rclcpp::init(argc, argv); + NodeHandle nh; if (!google::RegisterFlagValidator(&FLAGS_compression, &ValidateCompression)) { std::cerr << "Failed to register compression flag validator." << std::endl; @@ -96,8 +112,6 @@ int main(int argc, char** argv) { return -1; } - ff_msgs::CompressedFile cf; - cf.header.seq = 1; cf.header.frame_id = "world"; // Load the plan @@ -107,13 +121,13 @@ int main(int argc, char** argv) { io::filtering_ostream out; if (FLAGS_compression == "deflate") { - cf.type = ff_msgs::CompressedFile::TYPE_DEFLATE; + cf.type = ff_msgs::msg::CompressedFile::TYPE_DEFLATE; out.push(io::zlib_compressor()); } else if (FLAGS_compression == "gzip") { - cf.type = ff_msgs::CompressedFile::TYPE_GZ; + cf.type = ff_msgs::msg::CompressedFile::TYPE_GZ; out.push(io::gzip_compressor()); } else { - cf.type = ff_msgs::CompressedFile::TYPE_NONE; + cf.type = ff_msgs::msg::CompressedFile::TYPE_NONE; } out.push(io::back_inserter(data)); @@ -135,22 +149,32 @@ int main(int argc, char** argv) { cf.file.push_back(static_cast(c)); } - ros::NodeHandle n; - zones_pub_time = ros::Time::now(); + zones_pub_time = nh->get_clock()->now(); + + data_sub_connected_timer.createTimer(1.0, &TimerCallback, nh); // Publishes the zones when to the executive using subscriber status callbacks - ros::Publisher zone_pub = n.advertise( - TOPIC_COMMUNICATIONS_DDS_ZONES, 10, - std::bind(&on_connect, std::placeholders::_1, cf)); + zones_pub = FF_CREATE_PUBLISHER(nh, + ff_msgs::msg::CompressedFile, + TOPIC_COMMUNICATIONS_DDS_ZONES, + 10); // After the zones are received, commands a set zones to the executive - command_pub = n.advertise(TOPIC_COMMAND, 5); + command_pub = FF_CREATE_PUBLISHER(nh, + ff_msgs::msg::CommandStamped, + TOPIC_COMMAND, + 5); // Subscriber that receives confirmation that the zones were received - ros::Subscriber cf_acK_pub = n.subscribe(TOPIC_MANAGEMENT_EXEC_CF_ACK, 10, &on_cf_ack); - - ROS_INFO("waiting for a subscriber..."); - ros::spin(); + Subscriber cf_acK_sub = + FF_CREATE_SUBSCRIBER(nh, + ff_msgs::msg::CompressedFileAck, + TOPIC_MANAGEMENT_EXEC_CF_ACK, + 10, + std::bind(&on_cf_ack, std::placeholders::_1)); + + FF_INFO("waiting for a subscriber..."); + rclcpp::spin(nh); return 0; } diff --git a/management/sys_monitor/CMakeLists.txt b/management/sys_monitor/CMakeLists.txt index 7161918f8e..dbd770da24 100644 --- a/management/sys_monitor/CMakeLists.txt +++ b/management/sys_monitor/CMakeLists.txt @@ -15,26 +15,24 @@ # 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(sys_monitor) ## Compile as C++14, supported in ROS Kinetic and newer +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() add_compile_options(-std=c++14) -add_definitions(-DROS1) - -## Find catkin macros and libraries -find_package(catkin2 REQUIRED COMPONENTS - roscpp - ff_util - ff_msgs - nodelet - config_reader -) +set( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Werror -O3 -fPIC" ) + +## Find ament and libraries +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(ff_msgs REQUIRED) +find_package(config_reader REQUIRED) +find_package(ff_util REQUIRED) -catkin_package( - LIBRARIES sys_monitor - DEPENDS roscpp ff_util ff_msgs nodelet config_reader -) ########### ## Build ## @@ -43,70 +41,78 @@ catkin_package( # Specify additional locations of header files include_directories( include - ${catkin_INCLUDE_DIRS} ) # Declare C++ libraries -add_library(sys_monitor +add_library(sys_monitor SHARED src/sys_monitor.cc ) -add_dependencies(sys_monitor ${catkin_EXPORTED_TARGETS}) -target_link_libraries(sys_monitor ${catkin_LIBRARIES}) +target_compile_definitions(sys_monitor PRIVATE "COMPOSITION_BUILDING_DLL") +ament_target_dependencies(sys_monitor rclcpp rclcpp_components ff_msgs config_reader ff_util) +rclcpp_components_register_nodes(sys_monitor "sys_monitor::SysMonitor") ## Declare a C++ executable: fault_tester -add_executable(fault_tester tools/fault_tester.cc) -add_dependencies(fault_tester ${catkin_EXPORTED_TARGETS}) -target_link_libraries(fault_tester - sys_monitor gflags glog ${catkin_LIBRARIES}) - -if(CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) - # System monitor initialization fault tester - add_rostest_gtest(test_init_sys_monitor - test/test_init_sys_monitor.test - test/test_init_sys_monitor.cc - ) - - target_link_libraries(test_init_sys_monitor - ${catkin_LIBRARIES} glog - ) - # System monitor tester - add_rostest_gtest(test_sys_monitor - test/test_sys_monitor.test - test/test_sys_monitor.cc - ) - - target_link_libraries(test_sys_monitor - ${catkin_LIBRARIES} glog - ) -endif() +#add_executable(fault_tester tools/fault_tester.cc) +#ament_target_dependencies(fault_tester rclcpp rclcpp_components ff_msgs config_reader ff_util) +#target_link_libraries(fault_tester sys_monitor gflags glog) + + +#if(CATKIN_ENABLE_TESTING) +# find_package(rostest REQUIRED) +# # System monitor initialization fault tester +# add_rostest_gtest(test_init_sys_monitor +# test/test_init_sys_monitor.test +# test/test_init_sys_monitor.cc +# ) + +# target_link_libraries(test_init_sys_monitor +# ${catkin_LIBRARIES} glog +# ) +# # System monitor tester +# add_rostest_gtest(test_sys_monitor +# test/test_sys_monitor.test +# test/test_sys_monitor.cc +# ) + +# target_link_libraries(test_sys_monitor +# ${catkin_LIBRARIES} glog +# ) +#endif() ############# ## Install ## ############# +ament_export_include_directories(include) + # 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} + EXPORT sys_monitor + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include ) -# Mark nodelet_plugin for installation -install(FILES nodelet_plugins.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) # Install C++ executables -install(TARGETS fault_tester DESTINATION bin) -install(CODE "execute_process( - COMMAND ln -s ../../bin/fault_tester share/${PROJECT_NAME} - WORKING_DIRECTORY ${CMAKE_INSTALL_PREFIX} - OUTPUT_QUIET - ERROR_QUIET - )") +#install(TARGETS fault_tester RUNTIME DESTINATION lib/${PROJECT_NAME}) +#install(CODE "execute_process( +# COMMAND ln -s ../../bin/fault_tester share/${PROJECT_NAME} +# WORKING_DIRECTORY ${CMAKE_INSTALL_PREFIX} +# OUTPUT_QUIET +# ERROR_QUIET +# )") + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE +) # 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() diff --git a/management/sys_monitor/COLCON_IGNORE b/management/sys_monitor/COLCON_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/management/sys_monitor/include/sys_monitor/sys_monitor.h b/management/sys_monitor/include/sys_monitor/sys_monitor.h index b507655759..f03966d917 100644 --- a/management/sys_monitor/include/sys_monitor/sys_monitor.h +++ b/management/sys_monitor/include/sys_monitor/sys_monitor.h @@ -19,37 +19,35 @@ #ifndef SYS_MONITOR_SYS_MONITOR_H_ #define SYS_MONITOR_SYS_MONITOR_H_ -#include -#include -#include -#include -#include - #include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + #include #include -#include +#include +#include +#include +#include #include #include #include #include -using ff_msgs::CommandArg; +using ff_msgs::msg::CommandArg; namespace sys_monitor { -class SysMonitor : public ff_util::FreeFlyerNodelet { +class SysMonitor : public ff_util::FreeFlyerComponent { public: - SysMonitor(); + explicit SysMonitor(const rclcpp::NodeOptions& options); ~SysMonitor(); /** @@ -59,9 +57,9 @@ class SysMonitor : public ff_util::FreeFlyerNodelet { * @param time_occurred time fault occurred */ void AddFault(unsigned int fault_id, std::string const& fault_msg = "", - ros::Time time_occurred = ros::Time::now()); + rclcpp::Time time_occurred = rclcpp::Time(0)); - void AddFault(ff_msgs::Fault const& fault, bool check_added = false); + void AddFault(ff_msgs::msg::Fault const& fault, bool check_added = false); void ChangeFaultErrMsg(unsigned int fault_id, std::string err_msg); @@ -72,12 +70,12 @@ class SysMonitor : public ff_util::FreeFlyerNodelet { public: Watchdog(SysMonitor *const sys_monitor, std::string const& node_name, - ros::Duration const& timeout, + double const& timeout, uint const allowed_misses, uint const fault_id); uint fault_id(); uint misses_allowed(); - ff_msgs::HeartbeatConstPtr previous_hb(); + ff_msgs::msg::Heartbeat::SharedPtr previous_hb(); bool hb_fault_occurring(); bool heartbeat_started(); bool unloaded(); @@ -91,11 +89,11 @@ class SysMonitor : public ff_util::FreeFlyerNodelet { void unloaded(bool is_unloaded); void ResetTimer(); void StopTimer(); - void previous_hb(ff_msgs::HeartbeatConstPtr hb); - void TimerCallBack(ros::TimerEvent const& te); + void previous_hb(ff_msgs::msg::Heartbeat::SharedPtr hb); + void TimerCallBack(); private: - ros::Timer timer_; + ff_util::FreeFlyerTimer timer_; SysMonitor *const monitor_; uint missed_count_; uint const misses_allowed_; @@ -106,7 +104,7 @@ class SysMonitor : public ff_util::FreeFlyerNodelet { std::string nodelet_manager_; std::string nodelet_name_; std::string nodelet_type_; - ff_msgs::HeartbeatConstPtr previous_hb_; + ff_msgs::msg::Heartbeat::SharedPtr previous_hb_; }; typedef std::shared_ptr WatchdogPtr; @@ -114,11 +112,11 @@ class SysMonitor : public ff_util::FreeFlyerNodelet { Fault(std::string const& node_name_in, bool const blocking_in, bool const warning_in, - ff_msgs::CommandStampedPtr response_in); + std::shared_ptr response_in); std::string const node_name; bool const blocking; bool const warning; - ff_msgs::CommandStampedPtr response; + std::shared_ptr response; }; /** @@ -128,7 +126,7 @@ class SysMonitor : public ff_util::FreeFlyerNodelet { * @param allowed_misses allowable missed watchdog timeouts * @param fault_id unique fault id */ - void AddWatchDog(ros::Duration const& timeout, std::string const& node_name, + void AddWatchDog(double const& timeout, std::string const& node_name, uint const allowed_misses, uint const fault_id); void SetFaultState(unsigned int fault_id, bool adding_fault); @@ -137,19 +135,19 @@ class SysMonitor : public ff_util::FreeFlyerNodelet { * Heartbeat callback will reset watchdog for the sender's node * @param heartbeat received message */ - void HeartbeatCallback(ff_msgs::HeartbeatConstPtr const& heartbeat); + void HeartbeatCallback(const ff_msgs::msg::Heartbeat::SharedPtr heartbeat); void AssertFault(ff_util::FaultKeys enum_key, std::string const& message, - ros::Time time_fault_occurred = ros::Time::now()); + rclcpp::Time time_fault_occurred = rclcpp::Time(0)); void ClearFault(ff_util::FaultKeys enum_key); - virtual void Initialize(ros::NodeHandle *nh); + virtual void Initialize(NodeHandle &nh); void OutputFaultTables(); - void PublishCmd(ff_msgs::CommandStampedPtr cmd); + void PublishCmd(std::shared_ptr cmd); void PublishFaultConfig(); @@ -157,15 +155,15 @@ class SysMonitor : public ff_util::FreeFlyerNodelet { void PublishFaultResponse(unsigned int fault_id); - void PublishHeartbeatCallback(ros::TimerEvent const& te); + void PublishHeartbeatCallback(); void PublishHeartbeat(bool initialization_fault = false); void PublishTimeDiff(float time_diff_sec); - void StartupTimerCallback(ros::TimerEvent const& te); + void StartupTimerCallback(); - void ReloadNodeletTimerCallback(ros::TimerEvent const& te); + void ReloadNodeletTimerCallback(); /** * Read params will read in all the parameters from the lua config files. * When reloading parameters with this function, the watch dog will be cleared @@ -178,28 +176,30 @@ class SysMonitor : public ff_util::FreeFlyerNodelet { * If no command is specified, false is returned. */ bool ReadCommand(config_reader::ConfigReader::Table *entry, - ff_msgs::CommandStampedPtr cmd); + std::shared_ptr cmd); - bool NodeletService(ff_msgs::UnloadLoadNodelet::Request &req, - ff_msgs::UnloadLoadNodelet::Response &res); - int LoadNodelet(ff_msgs::UnloadLoadNodelet::Request &req); + bool NodeletService(const std::shared_ptr req, + std::shared_ptr res); + int LoadNodelet(std::shared_ptr req); int UnloadNodelet(std::string const& nodelet, std::string const& manager); - ff_msgs::FaultState fault_state_; - ff_msgs::FaultConfig fault_config_; - ff_msgs::Heartbeat heartbeat_; + ff_msgs::msg::FaultState fault_state_; + ff_msgs::msg::FaultConfig fault_config_; + ff_msgs::msg::Heartbeat heartbeat_; - nodelet::NodeletLoad load_service_; - nodelet::NodeletUnload unload_service_; + ff_util::FreeFlyerService load_service_; + ff_util::FreeFlyerService unload_service_; - ros::NodeHandle nh_; - ros::Publisher pub_cmd_, pub_heartbeat_; - ros::Publisher pub_fault_config_, pub_fault_state_; - ros::Publisher pub_time_sync_; - ros::Timer reload_params_timer_, startup_timer_, reload_nodelet_timer_; - ros::Timer heartbeat_timer_; - ros::ServiceServer unload_load_nodelet_service_; - ros::Subscriber sub_hb_; + NodeHandle nh_; + Publisher pub_cmd_; + Publisher pub_heartbeat_; + Publisher pub_fault_config_; + Publisher pub_fault_state_; + Publisher pub_time_sync_; + ff_util::FreeFlyerTimer reload_params_timer_, startup_timer_, reload_nodelet_timer_; + ff_util::FreeFlyerTimer heartbeat_timer_; + rclcpp::Service::SharedPtr unload_load_nodelet_service_; + Subscriber sub_hb_; std::map> all_faults_; std::map watch_dogs_; @@ -219,6 +219,7 @@ class SysMonitor : public ff_util::FreeFlyerNodelet { unsigned int startup_time_, reload_nodelet_timeout_, heartbeat_pub_rate_; float time_drift_thres_sec_; }; +typedef std::unique_ptr SysMonitorPtr; } // namespace sys_monitor #endif // SYS_MONITOR_SYS_MONITOR_H_ diff --git a/management/sys_monitor/nodelet_plugins.xml b/management/sys_monitor/nodelet_plugins.xml deleted file mode 100644 index 5fa83d8507..0000000000 --- a/management/sys_monitor/nodelet_plugins.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - Nodelet to monitor node heartbeats and faults. - - - diff --git a/management/sys_monitor/package.xml b/management/sys_monitor/package.xml index 0a98ba1a9c..ee2e155d72 100644 --- a/management/sys_monitor/package.xml +++ b/management/sys_monitor/package.xml @@ -1,5 +1,5 @@ - + sys_monitor 0.0.0 @@ -14,18 +14,21 @@ Astrobee Flight Software - catkin + + ament_cmake ff_util ff_msgs - roscpp - nodelet + rclcpp + rclcpp_component config_reader - ff_util - ff_msgs - roscpp - nodelet - config_reader + + ff_util + ff_msgs + rclcpp + rclcpp_component + config_reader + - + ament_cmake diff --git a/management/sys_monitor/src/sys_monitor.cc b/management/sys_monitor/src/sys_monitor.cc index f529ce099d..06fa052d86 100644 --- a/management/sys_monitor/src/sys_monitor.cc +++ b/management/sys_monitor/src/sys_monitor.cc @@ -18,9 +18,12 @@ #include "sys_monitor/sys_monitor.h" +FF_DEFINE_LOGGER("sys_monitor"); + namespace sys_monitor { -SysMonitor::SysMonitor() : - ff_util::FreeFlyerNodelet(NODE_SYS_MONITOR, false), + +SysMonitor::SysMonitor(const rclcpp::NodeOptions& options) : + ff_util::FreeFlyerComponent(options, NODE_SYS_MONITOR, false), time_diff_node_("imu_aug"), time_diff_fault_triggered_(false), log_time_llp_(true), @@ -37,7 +40,7 @@ SysMonitor::~SysMonitor() { void SysMonitor::AddFault(unsigned int fault_id, std::string const& fault_msg, - ros::Time time_occurred) { + rclcpp::Time time_occurred) { bool found = false; // Check to make sure fault is not already in the fault state for (uint i = 0; i < fault_state_.faults.size(); i++) { @@ -49,9 +52,9 @@ void SysMonitor::AddFault(unsigned int fault_id, // If fault isn't in fault state, add it and publish fault state if (!found) { - ROS_ERROR("Fault with id %i occurred. Fault error message: %s", fault_id, + FF_ERROR("Fault with id %i occurred. Fault error message: %s", fault_id, fault_msg.c_str()); - ff_msgs::Fault fault; + ff_msgs::msg::Fault fault; fault.id = fault_id; fault.time_of_fault = time_occurred; fault.msg.assign(fault_msg, 0, 128); @@ -63,7 +66,7 @@ void SysMonitor::AddFault(unsigned int fault_id, } } -void SysMonitor::AddFault(ff_msgs::Fault const& fault, bool check_added) { +void SysMonitor::AddFault(ff_msgs::msg::Fault const& fault, bool check_added) { unsigned int fault_id = fault.id; bool found = false; if (check_added) { @@ -77,7 +80,7 @@ void SysMonitor::AddFault(ff_msgs::Fault const& fault, bool check_added) { } if (!found) { - ROS_ERROR("Fault with id %i occurred. Fault error message: %s", fault_id, + FF_ERROR("Fault with id %i occurred. Fault error message: %s", fault_id, fault.msg.c_str()); fault_state_.faults.push_back(fault); SetFaultState(fault_id, true); @@ -108,7 +111,7 @@ void SysMonitor::RemoveFault(unsigned int fault_id) { PublishFaultState(); } -void SysMonitor::AddWatchDog(ros::Duration const& timeout, +void SysMonitor::AddWatchDog(double const& timeout, std::string const& node_name, uint const allowed_misses, uint const fault_id) { @@ -120,7 +123,7 @@ void SysMonitor::AddWatchDog(ros::Duration const& timeout, fault_id)); watch_dogs_.emplace(node_name, watchDog); } else { - NODELET_INFO("AddWatchDog() already exists for %s", + FF_INFO("AddWatchDog() already exists for %s", node_name.c_str()); } } @@ -136,40 +139,40 @@ void SysMonitor::SetFaultState(unsigned int fault_id, bool adding_fault) { } } } else { - NODELET_ERROR("Fault %i wasn't found when setting fault state.", fault_id); + FF_ERROR("Fault %i wasn't found when setting fault state.", fault_id); } // Set the state appropriately if (fault_state_.faults.size() > 0) { if (num_current_blocking_fault_ > 0) { - fault_state_.state = ff_msgs::FaultState::BLOCKED; + fault_state_.state = ff_msgs::msg::FaultState::BLOCKED; fault_state_.hr_state = "There are "; fault_state_.hr_state += std::to_string(num_current_blocking_fault_); fault_state_.hr_state += " blocking faults currently occurring."; } else if (num_current_blocking_fault_ < 0) { // This should never happen but add output just in case - NODELET_ERROR("Number of blocking faults is negative!!"); + FF_ERROR("Number of blocking faults is negative!!"); } else { - fault_state_.state = ff_msgs::FaultState::FAULT; + fault_state_.state = ff_msgs::msg::FaultState::FAULT; fault_state_.hr_state = "There are "; fault_state_.hr_state += std::to_string(fault_state_.faults.size()); fault_state_.hr_state += " faults currently occurring."; } } else { // No faults - fault_state_.state = ff_msgs::FaultState::FUNCTIONAL; + fault_state_.state = ff_msgs::msg::FaultState::FUNCTIONAL; fault_state_.hr_state = "Functional"; } } -void SysMonitor::HeartbeatCallback(ff_msgs::HeartbeatConstPtr const& hb) { +void SysMonitor::HeartbeatCallback(const ff_msgs::msg::Heartbeat::SharedPtr hb) { uint i = 0, j = 0, tmp_id; bool fault_found = true; // Report time if the heartbeat came from the imu aug or guest science manager // node. if (hb->node == time_diff_node_ || hb->node == "guest_science_manager") { - ff_msgs::TimeSyncStamped time_sync_msg; - ros::Time time_now = ros::Time::now(); + ff_msgs::msg::TimeSyncStamped time_sync_msg; + rclcpp::Time time_now = GetTimeNow(); // Check time drift from llp, use time in imu_aug heartbeat if (hb->node == time_diff_node_) { @@ -177,8 +180,8 @@ void SysMonitor::HeartbeatCallback(ff_msgs::HeartbeatConstPtr const& hb) { // Output time difference to the ros log for the first imu aug heartbeat if (log_time_llp_) { - NODELET_INFO_STREAM("LLP time is " << hb->header.stamp << - " and the MLP time is " << time_now << "."); + FF_INFO_STREAM("LLP time is " << (rclcpp::Time(hb->header.stamp)).seconds() << + " and the MLP time is " << time_now.seconds() << "."); log_time_llp_ = false; } @@ -186,7 +189,7 @@ void SysMonitor::HeartbeatCallback(ff_msgs::HeartbeatConstPtr const& hb) { // trigger fault // This fault only applies to clock skew between the MLP and the LLP since // this skew causes navigation failures. - float time_diff_sec = (time_now - hb->header.stamp).toSec(); + float time_diff_sec = (time_now - rclcpp::Time(hb->header.stamp)).seconds(); if (abs(time_diff_sec) > time_drift_thres_sec_) { if (!time_diff_fault_triggered_) { std::string key = ff_util::fault_keys[ff_util::TIME_DIFF_TOO_HIGH]; @@ -208,16 +211,16 @@ void SysMonitor::HeartbeatCallback(ff_msgs::HeartbeatConstPtr const& hb) { // Output time difference to the ros log for the first guest science // manager heartbeat if (log_time_hlp_) { - NODELET_INFO_STREAM("HLP time is " << hb->header.stamp << - " and the MLP time is "<< time_now << "."); + FF_INFO_STREAM("HLP time is " << rclcpp::Time(hb->header.stamp).seconds() << + " and the MLP time is "<< time_now.seconds() << "."); log_time_hlp_ = false; } } - time_sync_msg.header.stamp = ros::Time::now(); + time_sync_msg.header.stamp = GetTimeNow(); time_sync_msg.mlp_time = time_now; time_sync_msg.remote_time = hb->header.stamp; - pub_time_sync_.publish(time_sync_msg); + pub_time_sync_->publish(time_sync_msg); } // Check to see if node heartbeat is set up in watchdogs @@ -238,7 +241,7 @@ void SysMonitor::HeartbeatCallback(ff_msgs::HeartbeatConstPtr const& hb) { } // Get last heartbeat for fault comparison - ff_msgs::HeartbeatConstPtr previous_hb = wd->previous_hb(); + ff_msgs::msg::Heartbeat::SharedPtr previous_hb = wd->previous_hb(); // Check if nodelet was unloaded and has restarted. Need to remove faults // from previous heartbeat @@ -322,26 +325,26 @@ void SysMonitor::HeartbeatCallback(ff_msgs::HeartbeatConstPtr const& hb) { } if (!found) { - NODELET_WARN("Heartbeat unrecognized from %s!", hb->node.c_str()); + FF_WARN("Heartbeat unrecognized from %s!", hb->node.c_str()); unwatched_heartbeats_.push_back(hb->node); } } } -void SysMonitor::Initialize(ros::NodeHandle *nh) { - nh_ = *nh; +void SysMonitor::Initialize(NodeHandle &nh) { + nh_ = nh; // Set node name in heartbeat message heartbeat_.node = NODE_SYS_MONITOR; // Set nodelet manager name - heartbeat_.nodelet_manager = ros::this_node::getName(); + heartbeat_.nodelet_manager = this->GetName(); // Heartbeat must be latching - pub_heartbeat_ = nh_.advertise( - TOPIC_MANAGEMENT_SYS_MONITOR_HEARTBEAT, - pub_queue_size_, - true); + pub_heartbeat_ = FF_CREATE_PUBLISHER(nh_, + ff_msgs::msg::Heartbeat, + TOPIC_MANAGEMENT_SYS_MONITOR_HEARTBEAT, + pub_queue_size_); // Add config files to config reader config_params_.AddFile("management/sys_monitor.config"); @@ -353,70 +356,63 @@ void SysMonitor::Initialize(ros::NodeHandle *nh) { // Create a callback timer which checks to see if the config files have been // changed. - reload_params_timer_ = nh_.createTimer(ros::Duration(1), - [this](ros::TimerEvent e) { + reload_params_timer_.createTimer(1.0, + [this]() { config_params_.CheckFilesUpdated(std::bind(&SysMonitor::ReadParams, - this)); }, false, true); + this)); }, nh_, false, true); // Create a startup timer. Timer will be used to check if all the heartbeats // had started in the specified amount of time. - startup_timer_ = nh_.createTimer(ros::Duration(startup_time_), - &SysMonitor::StartupTimerCallback, - this, - true, - true); + startup_timer_.createTimer(startup_time_, + std::bind(&SysMonitor::StartupTimerCallback, this), + nh_, true, true); // Create a reload nodelet timer. Timer will be used to check if the nodelets // that died on startup got restarted successfully. - reload_nodelet_timer_ = nh_.createTimer( - ros::Duration(reload_nodelet_timeout_), - &SysMonitor::ReloadNodeletTimerCallback, - this, - true, - false); + reload_nodelet_timer_.createTimer(reload_nodelet_timeout_, + std::bind(&SysMonitor::ReloadNodeletTimerCallback, this), + nh_, true, false); // Create a timer to publish the heartbeat for the system monitor. - heartbeat_timer_ = nh_.createTimer(ros::Duration(heartbeat_pub_rate_), - &SysMonitor::PublishHeartbeatCallback, - this, - false, - true); + heartbeat_timer_.createTimer(heartbeat_pub_rate_, + std::bind(&SysMonitor::PublishHeartbeatCallback, this), + nh_, false, true); - sub_hb_ = nh_.subscribe(TOPIC_HEARTBEAT, + sub_hb_ = FF_CREATE_SUBSCRIBER(nh_, + ff_msgs::msg::Heartbeat, + TOPIC_HEARTBEAT, sub_queue_size_, - &SysMonitor::HeartbeatCallback, - this, - ros::TransportHints().tcpNoDelay()); + std::bind(&SysMonitor::HeartbeatCallback, this, std::placeholders::_1)); - pub_cmd_ = nh_.advertise(TOPIC_COMMAND, - pub_queue_size_, - false); + pub_cmd_ = FF_CREATE_PUBLISHER(nh_, + ff_msgs::msg::CommandStamped, + TOPIC_COMMAND, + pub_queue_size_); // All configs should be latching - pub_fault_config_ = nh_.advertise( - TOPIC_MANAGEMENT_SYS_MONITOR_CONFIG, - pub_queue_size_, - true); + pub_fault_config_ = FF_CREATE_PUBLISHER(nh_, + ff_msgs::msg::FaultConfig, + TOPIC_MANAGEMENT_SYS_MONITOR_CONFIG, + pub_queue_size_); // All states should be latching - pub_fault_state_ = nh_.advertise( - TOPIC_MANAGEMENT_SYS_MONITOR_STATE, - pub_queue_size_, - true); + pub_fault_state_ = FF_CREATE_PUBLISHER(nh_, + ff_msgs::msg::FaultState, + TOPIC_MANAGEMENT_SYS_MONITOR_STATE, + pub_queue_size_); - pub_time_sync_ = nh_.advertise( - TOPIC_MANAGEMENT_SYS_MONITOR_TIME_SYNC, - pub_queue_size_, - false); + pub_time_sync_ = FF_CREATE_PUBLISHER(nh_, + ff_msgs::msg::TimeSyncStamped, + TOPIC_MANAGEMENT_SYS_MONITOR_TIME_SYNC, + pub_queue_size_); // Set up service - unload_load_nodelet_service_ = nh_.advertiseService( + unload_load_nodelet_service_ = nh_->create_service( SERVICE_MANAGEMENT_SYS_MONITOR_UNLOAD_LOAD_NODELET, - &SysMonitor::NodeletService, - this); + std::bind(&SysMonitor::NodeletService, this, std::placeholders::_1, std::placeholders::_2)); // Initialize and publish fault state when first starting up - fault_state_.state = ff_msgs::FaultState::STARTING_UP; + fault_state_.state = ff_msgs::msg::FaultState::STARTING_UP; fault_state_.hr_state = "System starting up."; PublishFaultState(); @@ -439,11 +435,11 @@ void SysMonitor::Initialize(ros::NodeHandle *nh) { // Function used for debugging purposes only void SysMonitor::OutputFaultTables() { std::string subsys, node, warning, blocking_str; - ff_msgs::CommandStampedConstPtr tmp_cmd; + std::shared_ptr tmp_cmd; bool blocking; // Output all faults first - NODELET_DEBUG("All faults: "); + FF_DEBUG("All faults: "); for (unsigned int i = 0; i < fault_config_.faults.size(); i++) { subsys = fault_config_.subsystems[fault_config_.faults[i].subsystem]; node = fault_config_.nodes[fault_config_.faults[i].node]; @@ -465,21 +461,21 @@ void SysMonitor::OutputFaultTables() { blocking_str = "false"; } - NODELET_DEBUG_STREAM("fault: " << + FF_DEBUG_STREAM("fault: " << "\nid: " << fault_config_.faults[i].id << "\nsubsys: " << subsys.c_str() << "\nnode: " << node.c_str() << "\nwarning: " << warning.c_str() << "\nblocking: " << blocking_str.c_str() << - "\ndescr: " << fault_config_.faults[i].description.c_str() << - "\nCommand:\n\t" << (*tmp_cmd)); + "\ndescr: " << fault_config_.faults[i].description.c_str() ); //<< + // "\nCommand:\n\t" << (*tmp_cmd).to_yaml()); // HACK ANA } // Output heartbeat tables second - NODELET_DEBUG("Heartbeat faults: "); + FF_DEBUG("Heartbeat faults: "); typedef std::map::iterator it_type; for (it_type it = watch_dogs_.begin(); it != watch_dogs_.end(); it++) { - NODELET_DEBUG_STREAM("hb fault: " << + FF_DEBUG_STREAM("hb fault: " << "\nid: " << it->second->fault_id() << "\nnode name: " << it->first << "\nnode type: " << it->second->nodelet_type() << @@ -487,48 +483,48 @@ void SysMonitor::OutputFaultTables() { } } -void SysMonitor::PublishCmd(ff_msgs::CommandStampedPtr cmd) { - cmd->cmd_id = "sys_monitor" + std::to_string(ros::Time::now().sec); - pub_cmd_.publish(cmd); +void SysMonitor::PublishCmd(std::shared_ptr cmd) { + cmd->cmd_id = "sys_monitor" + std::to_string(GetTimeNow().seconds()); + pub_cmd_->publish(*cmd); } void SysMonitor::PublishFaultConfig() { - fault_config_.header.stamp = ros::Time::now(); - pub_fault_config_.publish(fault_config_); + fault_config_.header.stamp = GetTimeNow(); + pub_fault_config_->publish(fault_config_); } void SysMonitor::PublishFaultState() { - fault_state_.header.stamp = ros::Time::now(); - pub_fault_state_.publish(fault_state_); + fault_state_.header.stamp = GetTimeNow(); + pub_fault_state_->publish(fault_state_); } void SysMonitor::PublishFaultResponse(unsigned int fault_id) { if (all_faults_.count(fault_id)) { // Don't publish command if it is a warning if (!all_faults_.at(fault_id)->warning) { - ff_msgs::CommandStampedConstPtr cmd = all_faults_.at(fault_id)->response; + std::shared_ptr cmd = all_faults_.at(fault_id)->response; // Check for unload since it doesn't to send the command to the executive // just to have the executive call the system monitor service - if (cmd->cmd_name == ff_msgs::CommandConstants::CMD_NAME_UNLOAD_NODELET) { + if (cmd->cmd_name == ff_msgs::msg::CommandConstants::CMD_NAME_UNLOAD_NODELET) { // Need to check the command is formatted correctly if (cmd->args.size() != 2 || - cmd->args[0].data_type != ff_msgs::CommandArg::DATA_TYPE_STRING || - cmd->args[1].data_type != ff_msgs::CommandArg::DATA_TYPE_STRING) { - NODELET_ERROR("Malformed unload command for fault %i.", fault_id); + cmd->args[0].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_STRING || + cmd->args[1].data_type != ff_msgs::msg::CommandArg::DATA_TYPE_STRING) { + FF_ERROR("Malformed unload command for fault %i.", fault_id); } else { UnloadNodelet(cmd->args[0].s, cmd->args[1].s); } - } else if (cmd->cmd_name != ff_msgs::CommandConstants::CMD_NAME_NO_OP) { + } else if (cmd->cmd_name != ff_msgs::msg::CommandConstants::CMD_NAME_NO_OP) { // Don't publish command if it is a noop PublishCmd(all_faults_.at(fault_id)->response); } } } else { - NODELET_FATAL("Fault id %i unrecognized!", fault_id); + FF_FATAL("Fault id %i unrecognized!", fault_id); } } -void SysMonitor::PublishHeartbeatCallback(ros::TimerEvent const& te) { +void SysMonitor::PublishHeartbeatCallback() { PublishHeartbeat(); } @@ -539,12 +535,12 @@ void SysMonitor::PublishHeartbeat(bool initialization_fault) { // occurs before the fault ids are read in from the config files. Also the // fault id is not being sent to the ground. Thus we are just going to set // the initialization fault id to zero. - + FF_WARN("ANA -- SysMonitor init failed"); // Check to make sure the fault wasn't already added if (heartbeat_.faults.size() == 0) { - ff_msgs::Fault fault; + ff_msgs::msg::Fault fault; fault.id = 0; - fault.time_of_fault = ros::Time::now(); + fault.time_of_fault = GetTimeNow(); heartbeat_.faults.push_back(fault); } @@ -553,27 +549,27 @@ void SysMonitor::PublishHeartbeat(bool initialization_fault) { heartbeat_timer_.stop(); } - heartbeat_.header.stamp = ros::Time::now(); - pub_heartbeat_.publish(heartbeat_); + heartbeat_.header.stamp = GetTimeNow(); + pub_heartbeat_->publish(heartbeat_); } -void SysMonitor::StartupTimerCallback(ros::TimerEvent const& te) { - ff_msgs::UnloadLoadNodelet::Request load_req; +void SysMonitor::StartupTimerCallback() { + std::shared_ptr load_req; int load_result; for (auto it = watch_dogs_.begin(); it != watch_dogs_.end(); ++it) { // Try restarting nodelet if we never received a heartbeat from it if (!it->second->heartbeat_started()) { - ROS_WARN_STREAM(it->first << + FF_WARN_STREAM(it->first << " never published a heartbeat! Trying to start it manually."); // Only need to set the name. Everything else will be looked up from the // config - load_req.name = it->first; + load_req->name = it->first; load_result = LoadNodelet(load_req); // Check if loading the nodelet successed. Later checks will make sure the // nodelet started up successfully. - if (load_result != ff_msgs::UnloadLoadNodelet::Response::SUCCESSFUL) { + if (load_result != ff_msgs::srv::UnloadLoadNodelet::Response::SUCCESSFUL) { AddFault(it->second->fault_id(), ("Never received heartbeat from and unable to load " + it->first)); PublishFaultResponse(it->second->fault_id()); @@ -590,13 +586,13 @@ void SysMonitor::StartupTimerCallback(ros::TimerEvent const& te) { // If a critical fault occurred before the startup timer is triggered, the // state might be set to fault or blocked. In this case, we don't want to // set the state to functional - if (fault_state_.state == ff_msgs::FaultState::STARTING_UP) { - fault_state_.state = ff_msgs::FaultState::FUNCTIONAL; + if (fault_state_.state == ff_msgs::msg::FaultState::STARTING_UP) { + fault_state_.state = ff_msgs::msg::FaultState::FUNCTIONAL; fault_state_.hr_state = "Functional"; } } else { reload_nodelet_timer_.start(); - fault_state_.state = ff_msgs::FaultState::RELOADING_NODELETS; + fault_state_.state = ff_msgs::msg::FaultState::RELOADING_NODELETS; fault_state_.hr_state = "The following nodelets died on startup and are "; fault_state_.hr_state += "being reloaded:"; for (unsigned int i = 0; i < reloaded_nodelets_.size(); i++) { @@ -613,7 +609,7 @@ void SysMonitor::StartupTimerCallback(ros::TimerEvent const& te) { // This function is meant to catch any nodes that died on startup, were reloaded // and died or became unresponsive after reload. -void SysMonitor::ReloadNodeletTimerCallback(ros::TimerEvent const& te) { +void SysMonitor::ReloadNodeletTimerCallback() { std::string nodelets_not_running = ""; for (unsigned int i = 0; i < reloaded_nodelets_.size(); i++) { WatchdogPtr wd = watch_dogs_.at(reloaded_nodelets_[i]); @@ -630,8 +626,8 @@ void SysMonitor::ReloadNodeletTimerCallback(ros::TimerEvent const& te) { // If the system monitor state is still reloading nodelets, that means either // all the nodelets were reloaded successfully or all of the critical nodelets // are running. So the state can be set to functional. - if (fault_state_.state == ff_msgs::FaultState::RELOADING_NODELETS) { - fault_state_.state = ff_msgs::FaultState::FUNCTIONAL; + if (fault_state_.state == ff_msgs::msg::FaultState::RELOADING_NODELETS) { + fault_state_.state = ff_msgs::msg::FaultState::FUNCTIONAL; fault_state_.hr_state = "Functional"; if (nodelets_not_running != "") { fault_state_.hr_state += " but the following nodelets aren't running"; @@ -651,7 +647,7 @@ bool SysMonitor::ReadParams() { // Read config files into lua if (!config_params_.ReadFiles()) { - NODELET_ERROR("Error loading system monitor parameters."); + FF_ERROR("Error loading system monitor parameters."); PublishHeartbeat(true); return false; } @@ -659,7 +655,7 @@ bool SysMonitor::ReadParams() { // Get startup time. Used to check if all the heartbeats have started within // the specified startup time. if (!config_params_.GetUInt("startup_time_sec", &startup_time_)) { - NODELET_WARN("Unable to read startup time."); + FF_WARN("Unable to read startup time."); startup_time_ = 60; } @@ -667,23 +663,23 @@ bool SysMonitor::ReadParams() { // restarted if it died on startup if (!config_params_.GetUInt("reload_nodelet_timeout_sec", &reload_nodelet_timeout_)) { - NODELET_WARN("Unable to read reload nodelet timeout."); + FF_WARN("Unable to read reload nodelet timeout."); reload_nodelet_timeout_ = 20; } if (!config_params_.GetUInt("heartbeat_pub_rate_sec", &heartbeat_pub_rate_)) { - NODELET_WARN("Unable to read heartbeat pub rate."); + FF_WARN("Unable to read heartbeat pub rate."); heartbeat_pub_rate_ = 5; } if (!config_params_.GetStr("time_diff_node", &time_diff_node_)) { - NODELET_WARN("Unable to read time diff node name."); + FF_WARN("Unable to read time diff node name."); time_diff_node_ = "imu_aug"; } if (!config_params_.GetPosReal("time_drift_thres_sec", &time_drift_thres_sec_)) { - NODELET_WARN("Unable to read time drift threshold."); + FF_WARN("Unable to read time drift threshold."); time_drift_thres_sec_ = 0.25; } @@ -704,7 +700,7 @@ bool SysMonitor::ReadParams() { // Read in all faults // Check fault table exists if (!config_params_.CheckValExists("subsystems")) { - NODELET_ERROR("Unable to find/read the fault table."); + FF_ERROR("Unable to find/read the fault table."); PublishHeartbeat(true); return false; } @@ -723,7 +719,7 @@ bool SysMonitor::ReadParams() { for (i = 1; i < subsystems_tbl_size; i++) { config_reader::ConfigReader::Table subsystem_tbl(&subsystems_tbl, i); if (!subsystem_tbl.GetStr("name", &subsys_name)) { - NODELET_ERROR("Unable to read name at %i in subsys table", i); + FF_ERROR("Unable to read name at %i in subsys table", i); PublishHeartbeat(true); return false; } @@ -733,7 +729,7 @@ bool SysMonitor::ReadParams() { // Check nodes table exists if (!subsystem_tbl.CheckValExists("nodes")) { - NODELET_ERROR("Unable to find nodes table in %s's table.", + FF_ERROR("Unable to find nodes table in %s's table.", subsys_name.c_str()); PublishHeartbeat(true); return false; @@ -746,7 +742,7 @@ bool SysMonitor::ReadParams() { for (j = 1; j < nodes_tbl_size; j++) { config_reader::ConfigReader::Table node_tbl(&nodes_tbl, j); if (!node_tbl.GetStr("name", &node_name)) { - NODELET_ERROR("Unable to read name at %i in %s's node table.", + FF_ERROR("Unable to read name at %i in %s's node table.", j, subsys_name.c_str()); PublishHeartbeat(true); return false; @@ -769,7 +765,7 @@ bool SysMonitor::ReadParams() { // Check faults table exists if (!node_tbl.CheckValExists("faults")) { - NODELET_ERROR("Unable to read the fault table for node %s.", + FF_ERROR("Unable to read the fault table for node %s.", node_name.c_str()); PublishHeartbeat(true); return false; @@ -782,14 +778,14 @@ bool SysMonitor::ReadParams() { for (k = 1; k < faults_tbl_size; k++) { config_reader::ConfigReader::Table fault_entry(&faults_tbl, k); - ff_msgs::CommandStampedPtr response_cmd(new ff_msgs::CommandStamped()); + std::shared_ptr response_cmd(new ff_msgs::msg::CommandStamped()); if (fault_entry.GetUInt("id", &fault_id) && fault_entry.GetBool("blocking", &blocking) && fault_entry.GetBool("warning", &warning) && fault_entry.GetStr("description", &fault_description) && ReadCommand(&fault_entry, response_cmd)) { - ff_msgs::FaultInfo fault_info; + ff_msgs::msg::FaultInfo fault_info; // subsystem was added to vector at back which corresponds to i - 1 fault_info.subsystem = i - 1; @@ -816,16 +812,16 @@ bool SysMonitor::ReadParams() { "heartbeat"); if (heartbeat.GetReal("timeout_sec", &timeout) && heartbeat.GetUInt("misses", &misses)) { - AddWatchDog(ros::Duration(timeout), node_name, misses, fault_id); + AddWatchDog(timeout, node_name, misses, fault_id); } else { - NODELET_ERROR("Unable to add heartbeat for node %s.", + FF_ERROR("Unable to add heartbeat for node %s.", node_name.c_str()); PublishHeartbeat(true); return false; } } } else { - NODELET_ERROR("Unable to read fault at %i in %s's table.", + FF_ERROR("Unable to read fault at %i in %s's table.", k, node_name.c_str()); PublishHeartbeat(true); return false; @@ -843,17 +839,17 @@ bool SysMonitor::ReadParams() { for (i = 1; i < types_tbl_size; i++) { config_reader::ConfigReader::Table type_entry(&types_tbl, i); if (!type_entry.GetStr("name", &node_name)) { - NODELET_WARN("Name not found at %i in nodelet info table.", i); + FF_WARN("Name not found at %i in nodelet info table.", i); continue; } if (!type_entry.GetStr("type", &type)) { - NODELET_WARN("Type not found at %i in nodelet info table.", i); + FF_WARN("Type not found at %i in nodelet info table.", i); continue; } if (!type_entry.GetStr("manager", &manager)) { - NODELET_WARN("Manager not found at %i in nodelet info table.", i); + FF_WARN("Manager not found at %i in nodelet info table.", i); continue; } @@ -874,7 +870,7 @@ bool SysMonitor::ReadParams() { watch_dogs_.at(node_name)->nodelet_type(type); watch_dogs_.at(node_name)->nodelet_manager(manager); } else { - NODELET_WARN("Couldn't add type, %s wasn't in fault table.", + FF_WARN("Couldn't add type, %s wasn't in fault table.", node_name.c_str()); } } @@ -884,11 +880,11 @@ bool SysMonitor::ReadParams() { } bool SysMonitor::ReadCommand(config_reader::ConfigReader::Table *entry, - ff_msgs::CommandStampedPtr cmd) { + std::shared_ptr cmd) { std::string cmd_name; config_reader::ConfigReader::Table command(entry, "response"); if (!command.GetStr("name", &cmd_name)) { - NODELET_FATAL("Command name not specified."); + FF_FATAL("Command name not specified."); return false; } @@ -907,7 +903,7 @@ bool SysMonitor::ReadCommand(config_reader::ConfigReader::Table *entry, config_reader::ConfigReader::Table arg(&args, (i + 1)); // First element in table is the type if (!arg.GetUInt(1, &type)) { - NODELET_FATAL("First command argument value is not a uint"); + FF_FATAL("First command argument value is not a uint"); return false; } @@ -917,7 +913,7 @@ bool SysMonitor::ReadCommand(config_reader::ConfigReader::Table *entry, { bool val; if (!arg.GetBool(2, &val)) { - NODELET_FATAL("Expected command argument to be a bool!"); + FF_FATAL("Expected command argument to be a bool!"); return false; } cmd->args[i].data_type = CommandArg::DATA_TYPE_BOOL; @@ -928,7 +924,7 @@ bool SysMonitor::ReadCommand(config_reader::ConfigReader::Table *entry, { double val; if (!arg.GetReal(2, &val)) { - NODELET_FATAL("Expected command argument to be a double"); + FF_FATAL("Expected command argument to be a double"); return false; } cmd->args[i].data_type = CommandArg::DATA_TYPE_DOUBLE; @@ -939,7 +935,7 @@ bool SysMonitor::ReadCommand(config_reader::ConfigReader::Table *entry, { float val; if (!arg.GetReal(2, &val)) { - NODELET_FATAL("Expected command argument to be a float."); + FF_FATAL("Expected command argument to be a float."); return false; } cmd->args[i].data_type = CommandArg::DATA_TYPE_FLOAT; @@ -950,7 +946,7 @@ bool SysMonitor::ReadCommand(config_reader::ConfigReader::Table *entry, { int val; if (!arg.GetInt(2, &val)) { - NODELET_FATAL("Expected command argument to be an int."); + FF_FATAL("Expected command argument to be an int."); return false; } cmd->args[i].data_type = CommandArg::DATA_TYPE_INT; @@ -961,7 +957,7 @@ bool SysMonitor::ReadCommand(config_reader::ConfigReader::Table *entry, { int64_t val; if (!arg.GetLongLong(2, &val)) { - NODELET_FATAL("Expected command argument to be an int."); + FF_FATAL("Expected command argument to be an int."); return false; } cmd->args[i].data_type = CommandArg::DATA_TYPE_LONGLONG; @@ -972,37 +968,37 @@ bool SysMonitor::ReadCommand(config_reader::ConfigReader::Table *entry, { std::string val; if (!arg.GetStr(2, &val)) { - NODELET_FATAL("Expected command argument to be a string"); + FF_FATAL("Expected command argument to be a string"); return false; } cmd->args[i].data_type = CommandArg::DATA_TYPE_STRING; cmd->args[i].s = val; } break; - case CommandArg::DATA_TYPE_VEC3d: + case CommandArg::DATA_TYPE_VEC3D: { int j; double val; - cmd->args[i].data_type = CommandArg::DATA_TYPE_VEC3d; + cmd->args[i].data_type = CommandArg::DATA_TYPE_VEC3D; for (j = 0; j < 3; ++j) { // Index to get vector values in table starts at 2 if (!arg.GetReal((j + 2), &val)) { - NODELET_FATAL("Expected command argument to be a double."); + FF_FATAL("Expected command argument to be a double."); return false; } cmd->args[i].vec3d[j] = val; } } break; - case CommandArg::DATA_TYPE_MAT33f: + case CommandArg::DATA_TYPE_MAT33F: { int j; float val; - cmd->args[i].data_type = CommandArg::DATA_TYPE_MAT33f; + cmd->args[i].data_type = CommandArg::DATA_TYPE_MAT33F; for (j = 0; j < 9; ++j) { // Index in get matrix values in table starts at 2 if (!arg.GetReal((j + 2), &val)) { - NODELET_FATAL("Expected command argument to be a float."); + FF_FATAL("Expected command argument to be a float."); return false; } cmd->args[i].mat33f[j] = val; @@ -1010,7 +1006,7 @@ bool SysMonitor::ReadCommand(config_reader::ConfigReader::Table *entry, } break; default: - NODELET_FATAL("SysMonitor: Type for command argument unrecognized!"); + FF_FATAL("SysMonitor: Type for command argument unrecognized!"); return false; } } @@ -1019,77 +1015,80 @@ bool SysMonitor::ReadCommand(config_reader::ConfigReader::Table *entry, return true; } -bool SysMonitor::NodeletService(ff_msgs::UnloadLoadNodelet::Request &req, - ff_msgs::UnloadLoadNodelet::Response &res) { - if (req.load) { - res.result = LoadNodelet(req); +bool SysMonitor::NodeletService(const std::shared_ptr req, + std::shared_ptr res) { + if (req->load) { + res->result = LoadNodelet(req); } else { - res.result = UnloadNodelet(req.name, req.manager_name); + res->result = UnloadNodelet(req->name, req->manager_name); } return true; } -int SysMonitor::LoadNodelet(ff_msgs::UnloadLoadNodelet::Request &req) { +int SysMonitor::LoadNodelet(std::shared_ptr req) { std::string platform = GetPlatform(); if (platform == "") { - load_service_.request.name = "/" + req.name; + load_service_.request->node_name = "/" + req->name; } else { - load_service_.request.name = "/" + platform + "/" + req.name; + load_service_.request->node_name = "/" + platform + "/" + req->name; } - load_service_.request.remap_source_args = req.remap_source_args; - load_service_.request.remap_target_args = req.remap_target_args; - load_service_.request.my_argv = req.my_argv; - load_service_.request.bond_id = req.bond_id; + load_service_.request->remap_rules = req->remap_source_args; + //load_service_.request->remap_target_args = req->remap_target_args; + //load_service_.request->my_argv = req->my_argv; + //load_service_.request->bond_id = req->bond_id; // NOT SURE WHAT THESE LINES DO -- Ana // Extract manager from watchdog map if not specified in service std::string manager; - if (req.manager_name == "") { + if (req->manager_name == "") { // Check if node name was added to the heartbeat map - if (watch_dogs_.count(req.name) > 0) { - manager = watch_dogs_.at(req.name)->nodelet_manager(); + if (watch_dogs_.count(req->name) > 0) { + manager = watch_dogs_.at(req->name)->nodelet_manager(); if (manager == "") { - return ff_msgs::UnloadLoadNodelet::Response::MANAGER_NAME_MISSING; + return ff_msgs::srv::UnloadLoadNodelet::Response::MANAGER_NAME_MISSING; } } else { - return ff_msgs::UnloadLoadNodelet::Response::NODE_NOT_IN_MAP; + return ff_msgs::srv::UnloadLoadNodelet::Response::NODE_NOT_IN_MAP; } } else { - manager = req.manager_name; + manager = req->manager_name; } // Extract type from watchdog map if not specified in service - if (req.type == "") { + /*if (req->type == "") { // Check if node name was added to the heartbeat map - if (watch_dogs_.count(req.name) > 0) { - load_service_.request.type = watch_dogs_.at(req.name)->nodelet_type(); - if (load_service_.request.type == "") { - return ff_msgs::UnloadLoadNodelet::Response::TYPE_MISSING; + if (watch_dogs_.count(req->name) > 0) { + load_service_.request->type = watch_dogs_.at(req->name)->nodelet_type(); + if (load_service_.request->type == "") { + return ff_msgs::srv::UnloadLoadNodelet::Response::TYPE_MISSING; } } else { - return ff_msgs::UnloadLoadNodelet::Response::NODE_NOT_IN_MAP; + return ff_msgs::srv::UnloadLoadNodelet::Response::NODE_NOT_IN_MAP; } } else { - load_service_.request.type = req.type; - } - - if (!ros::service::call(manager + "/load_nodelet", load_service_)) { - NODELET_FATAL("Unable to load nodelet %s in %s manager.", - req.name.c_str(), manager.c_str()); - return ff_msgs::UnloadLoadNodelet::Response::ROS_SERVICE_FAILED; + load_service_.request->type = req->type; + }*/ + + ff_util::FreeFlyerServiceClient load_client; + load_client.Create(nh_, manager + "/container/load_node"); + if (!load_client.Call(load_service_)) { + FF_FATAL("Unable to load nodelet %s in %s manager.", + req->name.c_str(), manager.c_str()); + return ff_msgs::srv::UnloadLoadNodelet::Response::ROS_SERVICE_FAILED; } - return ff_msgs::UnloadLoadNodelet::Response::SUCCESSFUL; + return ff_msgs::srv::UnloadLoadNodelet::Response::SUCCESSFUL; } int SysMonitor::UnloadNodelet(std::string const& nodelet, std::string const& manager) { std::string platform = GetPlatform(); - if (platform == "") { - unload_service_.request.name = "/" + nodelet; + // unload_service_.request.unique_id = ; // How do we get the id? + /*if (platform == "") { + unload_service_.request->node_name = "/" + nodelet; } else { - unload_service_.request.name = "/" + platform + "/" + nodelet; - } + unload_service_.request->node_name = "/" + platform + "/" + nodelet; + }*/ std::string manager_name = ""; // Check if node name was added to the heartbeat map @@ -1098,19 +1097,21 @@ int SysMonitor::UnloadNodelet(std::string const& nodelet, if (manager == "") { manager_name = watch_dogs_.at(nodelet)->nodelet_manager(); if (manager_name == "") { - return ff_msgs::UnloadLoadNodelet::Response::MANAGER_NAME_MISSING; + return ff_msgs::srv::UnloadLoadNodelet::Response::MANAGER_NAME_MISSING; } } else { manager_name = manager; } } else { - return ff_msgs::UnloadLoadNodelet::Response::NODE_NOT_IN_MAP; + return ff_msgs::srv::UnloadLoadNodelet::Response::NODE_NOT_IN_MAP; } - if (!ros::service::call(manager_name + "/unload_nodelet", unload_service_)) { - NODELET_FATAL("Unable to unload nodet %s in %s manager.", + ff_util::FreeFlyerServiceClient unload_client; + unload_client.Create(nh_, manager_name + "/container/unload_node"); + if (!unload_client.Call(unload_service_)) { + FF_FATAL("Unable to unload nodet %s in %s manager.", nodelet.c_str(), manager.c_str()); - return ff_msgs::UnloadLoadNodelet::Response::ROS_SERVICE_FAILED; + return ff_msgs::srv::UnloadLoadNodelet::Response::ROS_SERVICE_FAILED; } // Stop timer so we don't get a heartbeat missing fault @@ -1119,13 +1120,13 @@ int SysMonitor::UnloadNodelet(std::string const& nodelet, // faults watch_dogs_.at(nodelet)->unloaded(true); - return ff_msgs::UnloadLoadNodelet::Response::SUCCESSFUL; + return ff_msgs::srv::UnloadLoadNodelet::Response::SUCCESSFUL; } /**************************** Watchdog Functions *****************************/ SysMonitor::Watchdog::Watchdog(SysMonitor *const sys_monitor, std::string const& nodelet_name, - ros::Duration const& timeout, + double const& timeout, uint const allowed_misses, uint const fault_id) : monitor_(sys_monitor), @@ -1139,8 +1140,8 @@ SysMonitor::Watchdog::Watchdog(SysMonitor *const sys_monitor, nodelet_name_(nodelet_name), nodelet_type_(""), previous_hb_() { - timer_ = monitor_->nh_.createTimer(timeout, - &SysMonitor::Watchdog::TimerCallBack, this, false, false); + timer_.createTimer(timeout, + std::bind(&SysMonitor::Watchdog::TimerCallBack, this), sys_monitor->nh_, false, false); } uint SysMonitor::Watchdog::fault_id() { @@ -1151,7 +1152,7 @@ uint SysMonitor::Watchdog::misses_allowed() { return misses_allowed_; } -ff_msgs::HeartbeatConstPtr SysMonitor::Watchdog::previous_hb() { +ff_msgs::msg::Heartbeat::SharedPtr SysMonitor::Watchdog::previous_hb() { return previous_hb_; } @@ -1213,11 +1214,11 @@ void SysMonitor::Watchdog::StopTimer() { timer_.stop(); } -void SysMonitor::Watchdog::previous_hb(ff_msgs::HeartbeatConstPtr hb) { +void SysMonitor::Watchdog::previous_hb(ff_msgs::msg::Heartbeat::SharedPtr hb) { previous_hb_ = hb; } -void SysMonitor::Watchdog::TimerCallBack(ros::TimerEvent const& te) { +void SysMonitor::Watchdog::TimerCallBack() { if (missed_count_++ >= misses_allowed_) { std::string err_msg = "Didn't receive a heartbeat from " + nodelet_name_; monitor_->AddFault(fault_id_, err_msg); @@ -1232,7 +1233,7 @@ void SysMonitor::Watchdog::TimerCallBack(ros::TimerEvent const& te) { SysMonitor::Fault::Fault(std::string const& node_name_in, bool const blocking_in, bool const warning_in, - ff_msgs::CommandStampedPtr response_in) : + std::shared_ptr response_in) : node_name(node_name_in), blocking(blocking_in), warning(warning_in), @@ -1240,4 +1241,6 @@ SysMonitor::Fault::Fault(std::string const& node_name_in, } } // namespace sys_monitor -PLUGINLIB_EXPORT_CLASS(sys_monitor::SysMonitor, nodelet::Nodelet) +#include "rclcpp_components/register_node_macro.hpp" + +RCLCPP_COMPONENTS_REGISTER_NODE(sys_monitor::SysMonitor) diff --git a/mobility/choreographer/src/choreographer_component.cc b/mobility/choreographer/src/choreographer_component.cc index 34ec5ab601..0e20264d92 100644 --- a/mobility/choreographer/src/choreographer_component.cc +++ b/mobility/choreographer/src/choreographer_component.cc @@ -64,6 +64,9 @@ #include #include #include +#include + +using namespace std::chrono_literals; /** * \ingroup mobility @@ -606,7 +609,7 @@ class ChoreographerComponent : public ff_util::FreeFlyerComponent { UpdateCallback(fsm_.GetState(), MANUAL_STATE_SET); } - // Called on registration of aplanner + // Called on registration of a planner void SetInertiaCallback(const std::shared_ptr req, std::shared_ptr res) { // TODO(Andrew) Potentially sanity check inertia? @@ -847,8 +850,9 @@ class ChoreographerComponent : public ff_util::FreeFlyerComponent { if (!GetPlatform().empty()) child_frame = GetPlatform() + "/" + child_frame; try { + rclcpp::Time now = GetTimeNow(); geometry_msgs::TransformStamped tf = tf_buffer_->lookupTransform( - std::string(FRAME_NAME_WORLD), child_frame, ros::Time(0)); + std::string(FRAME_NAME_WORLD), child_frame, now, 50ms); pose.header = tf.header; pose.pose = msg_conversions::ros_transform_to_ros_pose(tf.transform); } diff --git a/mobility/planner_trapezoidal/package.xml b/mobility/planner_trapezoidal/package.xml index 093ff12ed2..0244a2602a 100644 --- a/mobility/planner_trapezoidal/package.xml +++ b/mobility/planner_trapezoidal/package.xml @@ -17,8 +17,9 @@ ament_cmake rclcpp - + rclcpp_components + + choreographer tf2_geometry_msgs ff_util @@ -28,15 +29,15 @@ rclcpp rclcpp_components - choreographer tf2_geometry_msgs ff_util ff_msgs ff_common mapper + ament_cmake - \ No newline at end of file + + diff --git a/scripts/docker/ros2/ros2_rolling_deb.Dockerfile b/scripts/docker/ros2/ros2_rolling_deb.Dockerfile index b38659ffa7..f1964d4043 100644 --- a/scripts/docker/ros2/ros2_rolling_deb.Dockerfile +++ b/scripts/docker/ros2/ros2_rolling_deb.Dockerfile @@ -4,7 +4,12 @@ ARG REMOTE=astrobee FROM ${REMOTE}/astrobee:latest-rolling_base-ubuntu${UBUNTU_VERSION} # install ros rolling + gazebo and dependencies +RUN /bin/bash -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' \ + && wget https://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add - + RUN apt-get update && apt-get install -q -y --fix-missing \ + gazebo11 \ + libgazebo11-dev \ ros-rolling-desktop \ binutils \ mesa-utils \ diff --git a/shared/ff_util/include/ff_util/config_client.h b/shared/ff_util/include/ff_util/config_client.h index f4aee066b6..a85eb32db3 100644 --- a/shared/ff_util/include/ff_util/config_client.h +++ b/shared/ff_util/include/ff_util/config_client.h @@ -33,7 +33,7 @@ class ConfigClient { explicit ConfigClient(NodeHandle & platform_nh, const std::string &node); virtual ~ConfigClient(); // Call a reconfigure with all set variables - bool Reconfigure(); + bool Reconfigure() { return true; }; // Getters and Setters template bool Get(const std::string &name, T &value) { diff --git a/shared/ff_util/include/ff_util/ff_component.h b/shared/ff_util/include/ff_util/ff_component.h index 3efa86cd4f..5e2f4e927b 100644 --- a/shared/ff_util/include/ff_util/ff_component.h +++ b/shared/ff_util/include/ff_util/ff_component.h @@ -72,7 +72,7 @@ class FreeFlyerComponent { // Use default name from freeflyer - crashes in ROS2 // explicit FreeFlyerComponent(const rclcpp::NodeOptions & options, bool autostart_hb_timer = true); - // Explicitly specift the name + // Explicitly specify the name explicit FreeFlyerComponent(const rclcpp::NodeOptions& options, std::string const& name, bool autostart_hb_timer = true); // Explicitly specify the node from gazebo diff --git a/shared/ff_util/include/ff_util/ff_service.h b/shared/ff_util/include/ff_util/ff_service.h index b993295327..aabb62605a 100644 --- a/shared/ff_util/include/ff_util/ff_service.h +++ b/shared/ff_util/include/ff_util/ff_service.h @@ -130,7 +130,7 @@ class FreeFlyerServiceClient { } bool call(FreeFlyerService & service) { - return call(service.request.get(), service.response); + return call(service.request, service.response); } bool exists() { diff --git a/simulation/launch/spawn_astrobee.launch.py b/simulation/launch/spawn_astrobee.launch.py index 91c253e8ca..26991aa270 100644 --- a/simulation/launch/spawn_astrobee.launch.py +++ b/simulation/launch/spawn_astrobee.launch.py @@ -32,32 +32,35 @@ def read_pose(context, *args, **kwargs): DeclareLaunchArgument('Y', default_value=pose[5]), ] -def generate_launch_description(): - return LaunchDescription([ - DeclareLaunchArgument("ns", default_value=""), # Robot namespace - DeclareLaunchArgument("pose"), # Robot pose - OpaqueFunction(function=read_pose), - DeclareLaunchArgument("robot_description"), # Robot description +def launch_setup(context, *args, **kwargs): + ns = str( (LaunchConfiguration("ns").perform(context)) ) + + topic = "/robot_description" + entity = "bsharp" - Node( + if ns: + topic = "/" + ns + topic + entity = ns + + spawn_entity = Node( package='gazebo_ros', executable='spawn_entity.py', name='spawn_astrobee', output='screen', - arguments=["-topic", "/robot_description", "-entity", "bsharp", "-timeout", "30.0", + arguments=["-topic", topic, "-entity", entity, "-timeout", "30.0", "-x", LaunchConfiguration("x"), "-y", LaunchConfiguration("y"), "-z", LaunchConfiguration("z"), - "-R", LaunchConfiguration("R"), "-P", LaunchConfiguration("P"), "-Y", LaunchConfiguration("Y")], - condition=LaunchConfigurationEquals("ns", "") - ), - Node( - package='gazebo_ros', - executable='spawn_entity.py', - name='spawn_astrobee', - output='screen', - arguments=["-topic", "/robot_description", "-entity", LaunchConfiguration("ns"), "-timeout", "30.0", - "-x", LaunchConfiguration("x"), "-y", LaunchConfiguration("y"), "-z", LaunchConfiguration("z"), - "-R", LaunchConfiguration("R"), "-P", LaunchConfiguration("P"), "-Y", LaunchConfiguration("Y")], - condition=LaunchConfigurationNotEquals("ns", "") + "-R", LaunchConfiguration("R"), "-P", LaunchConfiguration("P"), "-Y", LaunchConfiguration("Y"), + "-robot_namespace", LaunchConfiguration("ns")] ) + + return [spawn_entity] +def generate_launch_description(): + + return LaunchDescription([ + DeclareLaunchArgument("ns", default_value=""), # Robot namespace + DeclareLaunchArgument("pose"), # Robot pose + OpaqueFunction(function=read_pose), + DeclareLaunchArgument("robot_description"), # Robot description + OpaqueFunction(function=launch_setup) ])