From 64da04f4c097135bf459c2c629ef87cbf7ea255e Mon Sep 17 00:00:00 2001 From: Tomas Baca Date: Sat, 27 Jan 2024 11:37:24 +0100 Subject: [PATCH] ControlManager: moved bumper diagnostics to controlmgr diag --- src/control_manager/control_manager.cpp | 10 +--------- 1 file changed, 1 insertion(+), 9 deletions(-) diff --git a/src/control_manager/control_manager.cpp b/src/control_manager/control_manager.cpp index 0778c9a..9e7ebea 100644 --- a/src/control_manager/control_manager.cpp +++ b/src/control_manager/control_manager.cpp @@ -422,7 +422,6 @@ class ControlManager : public nodelet::Nodelet { mrs_lib::PublisherHandler ph_safety_area_markers_; mrs_lib::PublisherHandler ph_safety_area_coordinates_markers_; mrs_lib::PublisherHandler ph_disturbances_markers_; - mrs_lib::PublisherHandler ph_bumper_status_; mrs_lib::PublisherHandler ph_current_constraints_; mrs_lib::PublisherHandler ph_heading_; mrs_lib::PublisherHandler ph_speed_; @@ -637,7 +636,6 @@ class ControlManager : public nodelet::Nodelet { bool callbackEnableCallbacks(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res); bool callbackEnableBumper(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res); bool callbackUseSafetyArea(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res); - bool callbackBumperSetParams(mrs_msgs::BumperParamsSrv::Request& req, mrs_msgs::BumperParamsSrv::Response& res); bool callbackGetMinZ(mrs_msgs::GetFloat64::Request& req, mrs_msgs::GetFloat64::Response& res); @@ -1769,7 +1767,6 @@ void ControlManager::initialize(void) { ph_safety_area_markers_ = mrs_lib::PublisherHandler(nh_, "safety_area_markers_out", 1, true, 1.0); ph_safety_area_coordinates_markers_ = mrs_lib::PublisherHandler(nh_, "safety_area_coordinates_markers_out", 1, true, 1.0); ph_disturbances_markers_ = mrs_lib::PublisherHandler(nh_, "disturbances_markers_out", 1, false, 10.0); - ph_bumper_status_ = mrs_lib::PublisherHandler(nh_, "bumper_status_out", 1); ph_current_constraints_ = mrs_lib::PublisherHandler(nh_, "current_constraints_out", 1); ph_heading_ = mrs_lib::PublisherHandler(nh_, "heading_out", 1); ph_speed_ = mrs_lib::PublisherHandler(nh_, "speed_out", 1, false, 10.0); @@ -6238,7 +6235,7 @@ void ControlManager::publishDiagnostics(void) { diagnostics_msg.output_enabled = output_enabled_; - diagnostics_msg.rc_mode = rc_goto_active_; + diagnostics_msg.joystick_active = rc_goto_active_; { std::scoped_lock lock(mutex_tracker_list_, mutex_controller_list_); @@ -6879,11 +6876,6 @@ void ControlManager::bumperPushFromObstacle(void) { bumper_repulsing_ = true; - mrs_msgs::BumperStatus bumper_status; - bumper_status.repulsing = bumper_repulsing_; - - ph_bumper_status_.publish(bumper_status); - callbacks_enabled_ = false; mrs_msgs::ReferenceSrvResponse::ConstPtr tracker_response;