diff --git a/astrobee/launch/robot/MLP.launch.py b/astrobee/launch/robot/MLP.launch.py index efb535109b..9514129a30 100644 --- a/astrobee/launch/robot/MLP.launch.py +++ b/astrobee/launch/robot/MLP.launch.py @@ -235,16 +235,16 @@ def generate_launch_description(): package='rclcpp_components', executable='component_container', 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', + extra_arguments=[{'use_intra_process_comms': False}]), + ComposableNode( + package='access_control', + plugin='access_control::AccessControl', + name='access_control', + extra_arguments=[{'use_intra_process_comms': False}]), ] ), ComposableNodeContainer( 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..34fb6c3210 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,19 @@ 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}) +ament_target_dependencies(sequencer rclcpp rclcpp_components ff_common ff_msgs ff_hw_msgs config_reader ff_util jsonloader) # 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 +71,93 @@ 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_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 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 +# )") # 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() \ No newline at end of file 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..75ee04c4c7 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(rclcpp::NodeOptions const& 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,201 @@ 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_; + + 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..4cca6ee983 100644 --- a/management/executive/src/executive.cc +++ b/management/executive/src/executive.cc @@ -22,13 +22,15 @@ #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(rclcpp::NodeOptions const& options) : + ff_util::FreeFlyerComponent(options, NODE_EXECUTIVE), 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()), dock_state_(NULL), fault_state_(NULL), guest_science_config_(NULL), @@ -58,44 +60,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,7 +107,7 @@ 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; } @@ -115,73 +118,77 @@ void Executive::CmdCallback(ff_msgs::CommandStampedPtr const& cmd) { 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; } @@ -1179,7 +1197,7 @@ bool Executive::ConfigureMobility(bool move_to_start, std::string& err_msg) { // Initialize choreographer config client if it hasn't been initialized if (!choreographer_cfg_) { choreographer_cfg_ = - std::make_shared(&nh_, NODE_CHOREOGRAPHER); + std::make_shared(nh_, NODE_CHOREOGRAPHER); } // Set values for configuring, these values will persist until changed @@ -1213,8 +1231,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 +1248,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 +1259,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 +1274,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 +1283,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 +1292,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 +1303,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 +1320,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 +1328,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 +1364,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 +1385,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 +1430,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 +1441,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 +1489,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 +1500,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 +1523,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 +1535,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 +1560,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 +1570,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 +1595,27 @@ void Executive::StopWaitTimer() { wait_timer_.stop(); } +/****************************** Output functions ******************************/ +void Debug(std::string output) { + FF_DEBUG_STREAM(output); +} +void Error(std::string output) { + FF_ERROR_STREAM(output); +} + +void Info(std::string output) { + FF_INFO_STREAM(output); +} + +void 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 +1623,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 +1670,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 +1694,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 +1708,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 +1731,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 +1758,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 +1787,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 +1809,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 +1826,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 +1836,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 +1874,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 +1902,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 +1916,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 +1924,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 +1967,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 +2040,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 +2082,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 +2125,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 +2134,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 +2142,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 +2151,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 +2159,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 +2168,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 +2176,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 +2185,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 +2194,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 +2209,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 +2247,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 +2261,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 +2276,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 +2290,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 +2310,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 +2348,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 +2362,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 +2377,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 +2391,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 +2411,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 +2433,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 +2455,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 +2470,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 +2482,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 +2490,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 +2523,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 +2543,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 +2561,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 +2580,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 +2603,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 +2624,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 +2645,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 +2720,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 +2741,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 +2802,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 +2854,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 +2888,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 +2912,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 +2927,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 +2998,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 +3012,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 +3024,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 +3033,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 +3066,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 +3090,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 +3099,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 +3113,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 +3121,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 +3144,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 +3229,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 +3241,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 +3258,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 +3267,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 +3293,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 +3315,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 +3333,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 +3352,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 +3378,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 +3396,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 +3414,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 +3452,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 +3466,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 +3540,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 +3582,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 +3616,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 +3631,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 +3658,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,199 +3710,201 @@ 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()); + FF_ERROR("%s", err_msg.c_str()); this->AssertFault(ff_util::INITIALIZATION_FAILED, err_msg); return; } else { @@ -3815,73 +3920,70 @@ void Executive::Initialize(ros::NodeHandle *nh) { 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,93 +3999,93 @@ 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()); + FF_ERROR("%s", err_msg.c_str()); this->AssertFault(ff_util::INITIALIZATION_FAILED, err_msg); 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()); + FF_ERROR("%s", err_msg.c_str()); this->AssertFault(ff_util::INITIALIZATION_FAILED, err_msg); 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()); + FF_ERROR("%s", err_msg.c_str()); this->AssertFault(ff_util::INITIALIZATION_FAILED, err_msg); return false; } @@ -3993,7 +4095,7 @@ 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()); + FF_ERROR("%s", err_msg.c_str()); this->AssertFault(ff_util::INITIALIZATION_FAILED, err_msg); return false; } @@ -4001,14 +4103,14 @@ bool Executive::ReadParams() { 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()); + FF_ERROR("%s", err_msg.c_str()); this->AssertFault(ff_util::INITIALIZATION_FAILED, err_msg); 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()); + FF_ERROR("%s", err_msg.c_str()); this->AssertFault(ff_util::INITIALIZATION_FAILED, err_msg); return false; } @@ -4018,7 +4120,7 @@ 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()); + FF_ERROR("%s", err_msg.c_str()); this->AssertFault(ff_util::INITIALIZATION_FAILED, err_msg); return false; } @@ -4026,7 +4128,7 @@ bool Executive::ReadParams() { 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()); + FF_ERROR("%s", err_msg.c_str()); this->AssertFault(ff_util::INITIALIZATION_FAILED, err_msg); return false; } @@ -4040,7 +4142,7 @@ 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()); + FF_ERROR("%s", err_msg.c_str()); this->AssertFault(ff_util::INITIALIZATION_FAILED, err_msg); return false; } @@ -4050,7 +4152,7 @@ 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()); + FF_ERROR("%s", err_msg.c_str()); this->AssertFault(ff_util::INITIALIZATION_FAILED, err_msg); return false; } @@ -4058,12 +4160,12 @@ bool Executive::ReadMapperParams() { // 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,7 +4174,7 @@ 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()); + FF_ERROR("%s", err_msg.c_str()); this->AssertFault(ff_util::INITIALIZATION_FAILED, err_msg); return false; } @@ -4086,7 +4188,7 @@ 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()); + FF_ERROR("%s", err_msg.c_str()); this->AssertFault(ff_util::INITIALIZATION_FAILED, err_msg); return false; } @@ -4095,10 +4197,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 +4219,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 +4325,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 +4335,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..d8fcdc5c65 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,34 +800,42 @@ 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; } @@ -819,7 +848,7 @@ int main(int argc, char** argv) { } // 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/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() {