From d1987cb96d5761f8bfd35d14d9afe99ed61c6992 Mon Sep 17 00:00:00 2001 From: FabianEP11 Date: Mon, 4 Sep 2023 14:30:03 -0500 Subject: [PATCH 01/12] Adding nav feature as plugin base --- CMakeLists.txt | 1 + .../custom_datastructures.h | 64 ----- .../temoto_robot_manager/custom_plugin_base.h | 2 +- .../navigation_plugin_base.h | 23 ++ .../navigation_plugin_helper.h | 69 ++++++ .../temoto_robot_manager/rm_datastructures.h | 78 ++++++ include/temoto_robot_manager/robot.h | 7 +- include/temoto_robot_manager/robot_manager.h | 2 +- src/custom_plugin_helper.cpp | 1 + src/navigation_plugin_helper.cpp | 226 ++++++++++++++++++ src/robot.cpp | 161 ++++++++++--- src/robot_common_procedures.cpp | 2 +- src/robot_features.cpp | 30 ++- src/robot_manager.cpp | 2 +- 14 files changed, 553 insertions(+), 115 deletions(-) delete mode 100644 include/temoto_robot_manager/custom_datastructures.h create mode 100644 include/temoto_robot_manager/navigation_plugin_base.h create mode 100644 include/temoto_robot_manager/navigation_plugin_helper.h create mode 100644 include/temoto_robot_manager/rm_datastructures.h create mode 100644 src/navigation_plugin_helper.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 4e6e47a..d964550 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -85,6 +85,7 @@ add_executable(temoto_robot_manager src/robot_features.cpp src/robot_common_procedures.cpp src/custom_plugin_helper.cpp + src/navigation_plugin_helper.cpp ) add_dependencies(temoto_robot_manager ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS}) target_link_libraries(temoto_robot_manager diff --git a/include/temoto_robot_manager/custom_datastructures.h b/include/temoto_robot_manager/custom_datastructures.h deleted file mode 100644 index 607fbc2..0000000 --- a/include/temoto_robot_manager/custom_datastructures.h +++ /dev/null @@ -1,64 +0,0 @@ -#ifndef TEMOTO_ROBOT_MANAGER__CUSTOM_DATASTRUCTURES_H -#define TEMOTO_ROBOT_MANAGER__CUSTOM_DATASTRUCTURES_H - -#include -#include - -namespace temoto_robot_manager -{ - -struct RmCustomRequest -{ - struct Header - { - std::string frame_id; - uint64_t timestamp; - uint64_t sequence_id; - }; - - struct Position - { - double x; - double y; - double z; - }; - - struct Orientation - { - double x; - double y; - double z; - double w; - }; - - struct Pose - { - Position position; - Orientation orientation; - }; - - struct PoseStamped - { - Header header; - Pose pose; - }; - - std::string data_str; - std::vector data_str_array; - - double data_num; - std::vector data_num_array; - - PoseStamped data_pose; - std::vector data_pose_array; -}; - -struct RmCustomFeedback -{ - uint8_t status; - double progress; -}; - -} // temoto_robot_manager - -#endif \ No newline at end of file diff --git a/include/temoto_robot_manager/custom_plugin_base.h b/include/temoto_robot_manager/custom_plugin_base.h index 9ece19f..23f8faf 100644 --- a/include/temoto_robot_manager/custom_plugin_base.h +++ b/include/temoto_robot_manager/custom_plugin_base.h @@ -1,7 +1,7 @@ #ifndef TEMOTO_ROBOT_MANAGER__CUSTOM_PLUGIN_BASE_H #define TEMOTO_ROBOT_MANAGER__CUSTOM_PLUGIN_BASE_H -#include "temoto_robot_manager/custom_datastructures.h" +#include "temoto_robot_manager/rm_datastructures.h" #include namespace temoto_robot_manager diff --git a/include/temoto_robot_manager/navigation_plugin_base.h b/include/temoto_robot_manager/navigation_plugin_base.h new file mode 100644 index 0000000..4944410 --- /dev/null +++ b/include/temoto_robot_manager/navigation_plugin_base.h @@ -0,0 +1,23 @@ +#ifndef TEMOTO_ROBOT_MANAGER__NAVIGATION_PLUGIN_BASE_H +#define TEMOTO_ROBOT_MANAGER__NAVIGATION_PLUGIN_BASE_H + +#include "temoto_robot_manager/rm_datastructures.h" +#include + +namespace temoto_robot_manager +{ + +class NavigationPluginBase +{ +public: + virtual bool initialize() = 0; + virtual bool sendGoal(RmNavigationGoal goal) = 0; + virtual std::optional getFeedback() = 0; + virtual bool cancelGoal() = 0; + virtual bool deinitialize() = 0; + virtual ~NavigationPluginBase(){}; +}; + +} // temoto_robot_manager + +#endif \ No newline at end of file diff --git a/include/temoto_robot_manager/navigation_plugin_helper.h b/include/temoto_robot_manager/navigation_plugin_helper.h new file mode 100644 index 0000000..12dbeed --- /dev/null +++ b/include/temoto_robot_manager/navigation_plugin_helper.h @@ -0,0 +1,69 @@ +#ifndef TEMOTO_ROBOT_MANAGER__NAVIGATION_PLUGIN_HELPER_H +#define TEMOTO_ROBOT_MANAGER__NAVIGATION_PLUGIN_HELPER_H + +#include "class_loader/class_loader.hpp" +#include "temoto_robot_manager/navigation_plugin_base.h" +#include +#include + +namespace temoto_robot_manager +{ + +struct RmNavigationFeedbackWrap : RmNavigationFeedback +{ + std::string robot_name; + std::string navigation_feature_name; + std::string request_id; +}; + +struct RmNavigationRequestWrap : RmNavigationGoal +{ + std::string robot_name; + std::string navigation_feature_name; + std::string request_id; +}; + +class NavigationPluginHelper; // Forward declaration + +typedef std::shared_ptr NavigationPluginHelperPtr; +typedef std::function NavigationFeatureUpdateCb; + +class NavigationPluginHelper +{ +public: + enum class State + { + NOT_LOADED, + UNINITIALIZED, + INITIALIZED, + PROCESSING, + FINISHED, + STOPPING, + ERROR + }; + + NavigationPluginHelper(const std::string& plugin_path, NavigationFeatureUpdateCb update_cb); + ~NavigationPluginHelper(); + void initialize(); + void sendGoal(const RmNavigationRequestWrap& request); + void sendUpdate() const; + void cancelGoal(); + void deinitialize(); + +private: + State getState() const; + void setState(State state); + + std::shared_ptr plugin; + std::shared_ptr class_loader; + std::thread exec_thread_; + std::string plugin_path_; + + State state_; + mutable std::mutex mutex_state_; + + std::optional current_request_; + NavigationFeatureUpdateCb update_cb_; +}; +} // temoto_robot_manager namespace +#endif \ No newline at end of file diff --git a/include/temoto_robot_manager/rm_datastructures.h b/include/temoto_robot_manager/rm_datastructures.h new file mode 100644 index 0000000..789fde1 --- /dev/null +++ b/include/temoto_robot_manager/rm_datastructures.h @@ -0,0 +1,78 @@ +#ifndef TEMOTO_ROBOT_MANAGER__CUSTOM_DATASTRUCTURES_H +#define TEMOTO_ROBOT_MANAGER__CUSTOM_DATASTRUCTURES_H + +#include +#include + +namespace temoto_robot_manager +{ + +struct Header +{ + std::string frame_id; + uint64_t timestamp; + uint64_t sequence_id; +}; + +struct Position +{ + double x; + double y; + double z; +}; + +struct Orientation +{ + double x; + double y; + double z; + double w; +}; + +struct Pose +{ + Position position; + Orientation orientation; +}; + +struct PoseStamped +{ + Header header; + Pose pose; +}; + +struct RmCustomRequest +{ + Header header; + Position position; + Orientation orientation; + Pose pose; + PoseStamped poseStamped; + std::string data_str; + std::vector data_str_array; + double data_num; + std::vector data_num_array; + PoseStamped data_pose; + std::vector data_pose_array; +}; + +struct RmCustomFeedback +{ + uint8_t status; + double progress; +}; + +struct RmNavigationGoal +{ + PoseStamped goal_pose; +}; + +struct RmNavigationFeedback +{ + uint8_t status; + PoseStamped base_position; +}; + +} // temoto_robot_manager + +#endif \ No newline at end of file diff --git a/include/temoto_robot_manager/robot.h b/include/temoto_robot_manager/robot.h index d360f68..9c81431 100644 --- a/include/temoto_robot_manager/robot.h +++ b/include/temoto_robot_manager/robot.h @@ -24,6 +24,7 @@ #include "temoto_robot_manager/robot_common_procedures.h" #include "temoto_robot_manager/GripperControl.h" #include "temoto_robot_manager/custom_plugin_helper.h" +#include "temoto_robot_manager/navigation_plugin_helper.h" #include #include #include @@ -138,7 +139,11 @@ class Robot typedef actionlib::SimpleActionClient MoveBaseClient; ros::Subscriber localized_pose_sub_; geometry_msgs::PoseWithCovarianceStamped current_pose_navigation_; - + + NavigationPluginHelperPtr navigation_feature_plugin_; + mutable std::mutex navigation_feature_plugins_mutex_; + NavigationFeatureUpdateCb navigation_feature_update_cb_; + // Custom related std::map custom_feature_plugins_; mutable std::mutex custom_feature_plugins_mutex_; diff --git a/include/temoto_robot_manager/robot_manager.h b/include/temoto_robot_manager/robot_manager.h index b4a2c9b..f7ba9a7 100644 --- a/include/temoto_robot_manager/robot_manager.h +++ b/include/temoto_robot_manager/robot_manager.h @@ -22,7 +22,7 @@ #include "temoto_core/trr/config_synchronizer.h" #include "temoto_core/ConfigSync.h" #include "temoto_process_manager/process_manager_services.hpp" -#include "temoto_robot_manager/custom_datastructures.h" +#include "temoto_robot_manager/rm_datastructures.h" #include "temoto_robot_manager/robot_manager_services.h" #include "temoto_robot_manager/robot.h" #include "temoto_robot_manager/robot_config.h" diff --git a/src/custom_plugin_helper.cpp b/src/custom_plugin_helper.cpp index ad7109d..2611fc6 100644 --- a/src/custom_plugin_helper.cpp +++ b/src/custom_plugin_helper.cpp @@ -13,6 +13,7 @@ CustomPluginHelper::CustomPluginHelper(const std::string& plugin_path, CustomFea { try { + TEMOTO_INFO_STREAM_("\n ====== [Custom plug Helper] try ============" + plugin_path_); class_loader = std::make_shared(plugin_path_, false); if (class_loader->getAvailableClasses().empty()) diff --git a/src/navigation_plugin_helper.cpp b/src/navigation_plugin_helper.cpp new file mode 100644 index 0000000..e58fc56 --- /dev/null +++ b/src/navigation_plugin_helper.cpp @@ -0,0 +1,226 @@ +#include "temoto_robot_manager/navigation_plugin_helper.h" +#include "temoto_resource_registrar/temoto_error.h" +#include + +#include + +namespace temoto_robot_manager +{ + +NavigationPluginHelper::NavigationPluginHelper(const std::string& plugin_path, NavigationFeatureUpdateCb update_cb) +: plugin_path_(plugin_path) +, state_(State::NOT_LOADED) +, update_cb_(update_cb) +, current_request_{} +{ +try +{ + TEMOTO_INFO_("\n ====== [Nav plug Helper] try ============"); + TEMOTO_INFO_STREAM_("\n ====== [Nav plug Helper] try ============" + plugin_path_); + class_loader = std::make_shared(plugin_path_, false); + + if (class_loader->getAvailableClasses().empty()) + { + setState(State::ERROR); + throw TEMOTO_ERRSTACK("Library contains no plugins, check if the path is correct: '" + plugin_path_ + "'"); + } + + std::string plugin_name = class_loader->getAvailableClasses().front(); + plugin = class_loader->createSharedInstance(plugin_name); + TEMOTO_INFO_("\n ====== [Nav plug Helper] createSharedInstance() ============"); + TEMOTO_INFO_(plugin_name); + if (!class_loader->isLibraryLoaded()) + { + setState(State::ERROR); + throw TEMOTO_ERRSTACK("Unable to load plugin '" + plugin_path_ + "'"); + } + + setState(State::UNINITIALIZED); + TEMOTO_INFO_("\n ====== [Nav plug Helper] End Constructor ============"); +} +catch(class_loader::ClassLoaderException & e) +{ + throw TEMOTO_ERRSTACK(e.what()); +} +} + +NavigationPluginHelper::~NavigationPluginHelper() +{ + if (getState() == State::PROCESSING) + { + plugin->cancelGoal(); + + while (!exec_thread_.joinable()) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + exec_thread_.join(); + plugin->deinitialize(); + } + + else if (getState() == State::STOPPING) + { + while (!exec_thread_.joinable()) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + exec_thread_.join(); + plugin->deinitialize(); + } + + else if(getState() == State::INITIALIZED || getState() == State::FINISHED || getState() == State::ERROR) + { + if (exec_thread_.joinable()) + { + exec_thread_.join(); + } + + plugin->deinitialize(); + } + + plugin.reset(); +} + +void NavigationPluginHelper::initialize() +try +{ + TEMOTO_INFO_("====== [Nav plug Helper] Try Initialize () ============"); + State enumValue = getState(); + + std::cout << "Enum value: " << static_cast(enumValue) << std::endl; + + if (getState() != State::UNINITIALIZED && getState() != State::FINISHED) + { + TEMOTO_INFO_("\n ====== [Nav plug Helper] if getState() ============"); + setState(State::ERROR); + TEMOTO_INFO_("\n ====== [Nav plug Helper] setState() ============"); + throw TEMOTO_ERRSTACK("Cannot initalize the plugin. It has to be in 'UNINITIALIZED' state for that"); + } + TEMOTO_INFO_("====== [Nav plug Helper] getState () ============"); + if (!plugin->initialize()) + { + setState(State::ERROR); + plugin.reset(); + throw TEMOTO_ERRSTACK("Unable to initialize the plugin"); + } + TEMOTO_INFO_("====== [Nav plug Helper] set State Initialized ============"); + setState(State::INITIALIZED); +} +catch(class_loader::ClassLoaderException & e) +{ + throw TEMOTO_ERRSTACK(e.what()); +} + +void NavigationPluginHelper::sendGoal(const RmNavigationRequestWrap& request) +{ + TEMOTO_INFO_("================= [Nav plug Helper] sendGoal - Initialize=================="); + + initialize(); + + if (getState() != State::INITIALIZED) + { + setState(State::ERROR); + throw TEMOTO_ERRSTACK("Cannot send navigation goal. The plugin has to be in 'INITIALIZED' state for that"); + } + + if (exec_thread_.joinable()) + { + exec_thread_.join(); + } + + current_request_ = request; + exec_thread_ = std::thread( + [&] + { + if (plugin->sendGoal(request)) + { + setState(State::FINISHED); + } + else + { + setState(State::ERROR); + //throw TEMOTO_ERRSTACK("Unable to invoke the plugin"); + } + + sendUpdate(); + }); + + setState(State::PROCESSING); + sendUpdate(); +} + +void NavigationPluginHelper::cancelGoal() +{ + if (getState() != State::PROCESSING) + { + setState(State::ERROR); + throw TEMOTO_ERRSTACK("Cannot cancel the goal. Plugin has to be in 'PROCESSING' state for that"); + } + + if (!plugin->cancelGoal()) + { + setState(State::ERROR); + throw TEMOTO_ERRSTACK("Unable to cancel goal"); + } + + setState(State::STOPPING); + sendUpdate(); +} + +void NavigationPluginHelper::deinitialize() +try +{ + if (getState() != State::INITIALIZED) + { + setState(State::ERROR); + throw TEMOTO_ERRSTACK("Cannot deinitialize the plugin. It has to be in 'INITIALIZED' state for that"); + } + + if (!plugin->deinitialize()) + { + setState(State::ERROR); + throw TEMOTO_ERRSTACK("Unable to deinitialize the plugin"); + } + + setState(State::UNINITIALIZED); +} +catch(...) +{ + throw TEMOTO_ERRSTACK("Unable to deinitialize the plugin"); +} + +NavigationPluginHelper::State NavigationPluginHelper::getState() const +{ + TEMOTO_INFO_("\n ====== [getState] ============"); + std::lock_guard l(mutex_state_); + TEMOTO_INFO_("\n ====== [getState] before return ============"); + std::cout << "Enum value: " << static_cast(state_) << std::endl; + return state_; +} + +void NavigationPluginHelper::setState(State state) +{ + std::lock_guard l(mutex_state_); + state_ = state; +} + +void NavigationPluginHelper::sendUpdate() const +{ + auto fb = plugin->getFeedback(); + if (fb.has_value()) + { + RmNavigationFeedbackWrap fbw; + + fbw.robot_name = current_request_->robot_name; + fbw.navigation_feature_name = current_request_->navigation_feature_name; + fbw.request_id = current_request_->request_id; + fbw.status = uint8_t(state_); + fbw.base_position = fb->base_position; + + update_cb_(fbw); + } +} + +} \ No newline at end of file diff --git a/src/robot.cpp b/src/robot.cpp index d7dbfd9..32321d6 100644 --- a/src/robot.cpp +++ b/src/robot.cpp @@ -17,6 +17,7 @@ #include "ros/package.h" #include "temoto_robot_manager/robot.h" #include "temoto_robot_manager/custom_plugin_base.h" +#include "temoto_robot_manager/navigation_plugin_base.h" #include "temoto_resource_registrar/temoto_error.h" namespace temoto_robot_manager @@ -384,29 +385,93 @@ void Robot::loadManipulationDriver() // Load Move Base void Robot::loadNavigationController() { + TEMOTO_INFO_("0 ================= Loading Navigation contoller =================="); if (config_->getFeatureNavigation().isLoaded()) { return; // Return if already loaded. } - + TEMOTO_INFO_("1 ================= Loading Navigation contoller =================="); try { FeatureNavigation& ftr = config_->getFeatureNavigation(); - rosExecute(ftr.getPackageName(), ftr.getExecutable(), ftr.getArgs()); - - // wait for command velocity to be published - std::string cmd_vel_topic = "/" + config_->getAbsRobotNamespace() + "/" + ftr.getCmdVelTopic(); - waitForTopic(cmd_vel_topic); + TEMOTO_INFO_("2 ================= Loading Navigation contoller =================="); + if (ftr.getExecutableType() == "ros") + { + // Previous Implementation + rosExecute(ftr.getPackageName(), ftr.getExecutable(), ftr.getArgs()); + // wait for command velocity to be published + std::string cmd_vel_topic = "/" + config_->getAbsRobotNamespace() + "/" + ftr.getCmdVelTopic(); + waitForTopic(cmd_vel_topic); - // Subscribe to the pose messages - if (!ftr.getPoseTopic().empty()) + // Subscribe to the pose messages + if (!ftr.getPoseTopic().empty()) + { + localized_pose_sub_ = nh_.subscribe("/" + config_->getAbsRobotNamespace() + "/" + ftr.getPoseTopic() + , 1 + , &Robot::robotPoseCallback + , this); + } + } + else if (ftr.getExecutableType() == "lib") { - localized_pose_sub_ = nh_.subscribe("/" + config_->getAbsRobotNamespace() + "/" + ftr.getPoseTopic() - , 1 - , &Robot::robotPoseCallback - , this); + TEMOTO_INFO_("Navigation contoller lib"); + try + { + const std::string& plugin_path = ftr.getExecutable(); + TEMOTO_INFO_("Executable"); + TEMOTO_INFO_(plugin_path); + NavigationPluginHelperPtr plugin_helper = std::make_shared(plugin_path, navigation_feature_update_cb_); + + std::lock_guard l(navigation_feature_plugins_mutex_); + // ftr.setLoaded(true); + + + // /* + // * Start the custom feature feedback thread + // */ + // if (navigation_feature_feedback_thread_running_) + // { + // return; + // } + + // navigation_feature_feedback_thread_running_ = true; + // navigation_feature_feedback_thread_ = std::thread( + // [&] + // { + // TEMOTO_DEBUG_("Custom feature feedback thread running"); + + // while (navigation_feature_feedback_thread_running_) + // { + // std::lock_guard l(custom_feature_plugins_mutex_); + + // for (const auto& cfp : custom_feature_plugins_) + // { + // cfp.second->sendUpdate(); + // std::this_thread::sleep_for(std::chrono::milliseconds(50)); + // } + + // std::this_thread::sleep_for(std::chrono::milliseconds(200)); + // } + + // TEMOTO_DEBUG_("Custom feature feedback thread finished"); + // }); + } + catch(resource_registrar::TemotoErrorStack& error_stack) + { + throw FWD_TEMOTO_ERRSTACK(error_stack); + } + catch(std::exception& e) + { + throw TEMOTO_ERRSTACK(e.what()); + } + catch(...) + { + throw TEMOTO_ERRSTACK("Could not load navigation feature"); + } } + TEMOTO_INFO_("3 ================= Loadng Navigation contoller =================="); + ros::Duration(5).sleep(); ftr.setLoaded(true); TEMOTO_DEBUG_("Feature 'Navigation Controller' loaded."); @@ -430,6 +495,9 @@ void Robot::loadNavigationDriver() FeatureNavigation& ftr = config_->getFeatureNavigation(); rosExecute(ftr.getDriverPackageName(), ftr.getDriverExecutable(), ftr.getDriverArgs()); std::string odom_topic = "/" + config_->getAbsRobotNamespace() + "/" + ftr.getOdomTopic(); + TEMOTO_INFO_(" ===== Loadng Navigation driver ===== Waitinf for topic ====="); + TEMOTO_INFO_(odom_topic); + waitForTopic(odom_topic); ftr.setDriverLoaded(true); TEMOTO_DEBUG_("Feature 'Navigation Driver' loaded."); @@ -955,41 +1023,56 @@ std::vector Robot::getNamedTargetPoses(const std::string& planning_ void Robot::goalNavigation(const geometry_msgs::PoseStamped& target_pose) { + TEMOTO_INFO_("================= [robot.cpp 1026] goalNavigation =================="); if (!isRobotOperational()) { throw TEMOTO_ERRSTACK("Could not navigate the robot because robot is not operational"); } - + TEMOTO_INFO_("================= [robot.cpp 1031] getFeatureNavigation =================="); FeatureNavigation& ftr = config_->getFeatureNavigation(); - std::string act_rob_ns = "/" + config_->getAbsRobotNamespace() + "/move_base"; - MoveBaseClient ac(act_rob_ns, true); + RmNavigationRequestWrap request; + request.goal_pose.header.frame_id = target_pose.header.frame_id; + request.goal_pose.pose.position.x = target_pose.pose.position.x; + request.goal_pose.pose.position.y = target_pose.pose.position.y; + request.goal_pose.pose.position.z = target_pose.pose.position.z; + request.goal_pose.pose.orientation.x = target_pose.pose.orientation.x; + request.goal_pose.pose.orientation.y = target_pose.pose.orientation.y; + request.goal_pose.pose.orientation.z = target_pose.pose.orientation.z; + request.goal_pose.pose.orientation.w = target_pose.pose.orientation.w; + TEMOTO_INFO_("================= Before send Goal =================="); + navigation_feature_plugin_->sendGoal(request); + + // std::string act_rob_ns = "/" + config_->getAbsRobotNamespace() + "/move_base"; + // MoveBaseClient ac(act_rob_ns, true); - if (!ac.waitForServer(ros::Duration(5.0))) - { - TEMOTO_ERRSTACK("The move_base action server did not come up"); - } + // if (!ac.waitForServer(ros::Duration(5.0))) + // { + // TEMOTO_ERRSTACK("The move_base action server did not come up"); + // } + + // move_base_msgs::MoveBaseGoal goal; + // goal.target_pose = target_pose; + // goal.target_pose.header.stamp = ros::Time::now(); + // ac.sendGoal(goal); + + // // Wait until either the goal is finished or robot has encountered a system issue + // while((ac.getState() == actionlib::SimpleClientGoalState::PENDING || ac.getState() == actionlib::SimpleClientGoalState::ACTIVE) + // && isRobotOperational()) + // { + // ros::Duration(1).sleep(); + // } + + // if (!isRobotOperational()) + // { + // ac.cancelGoal(); + // throw TEMOTO_ERRSTACK("Could not finish the navigation goal because the robot is not operational"); + // } + // else if(ac.getState() != actionlib::SimpleClientGoalState::SUCCEEDED) + // { + // throw TEMOTO_ERRSTACK("The base failed to move"); + // } - move_base_msgs::MoveBaseGoal goal; - goal.target_pose = target_pose; - goal.target_pose.header.stamp = ros::Time::now(); - ac.sendGoal(goal); - // Wait until either the goal is finished or robot has encountered a system issue - while((ac.getState() == actionlib::SimpleClientGoalState::PENDING || ac.getState() == actionlib::SimpleClientGoalState::ACTIVE) - && isRobotOperational()) - { - ros::Duration(1).sleep(); - } - - if (!isRobotOperational()) - { - ac.cancelGoal(); - throw TEMOTO_ERRSTACK("Could not finish the navigation goal because the robot is not operational"); - } - else if(ac.getState() != actionlib::SimpleClientGoalState::SUCCEEDED) - { - throw TEMOTO_ERRSTACK("The base failed to move"); - } } void Robot::controlGripper(const std::string& robot_name,const float position) diff --git a/src/robot_common_procedures.cpp b/src/robot_common_procedures.cpp index 6f0d488..569480b 100644 --- a/src/robot_common_procedures.cpp +++ b/src/robot_common_procedures.cpp @@ -23,7 +23,7 @@ CommonProcedure::CommonProcedure(const std::string& name, const YAML::Node& comm { this->executable_ = common_conf["executable"].as(); this->executable_type_ = common_conf["executable_type"].as(); - // setFromConfig(common_conf["executable_type"], this->executable_type_); + if (common_conf["args"]) { this->args_ = common_conf["args"].as(); diff --git a/src/robot_features.cpp b/src/robot_features.cpp index 33c8c8f..9e4d9fe 100644 --- a/src/robot_features.cpp +++ b/src/robot_features.cpp @@ -106,9 +106,17 @@ FeatureNavigation::FeatureNavigation(const YAML::Node& nav_conf) * Get the controller configuration. Not required */ if (nav_conf["controller"].IsDefined()) - { - this->feature_enabled_ = setFromConfig(nav_conf["controller"]["package_name"], this->package_name_) - && setFromConfig(nav_conf["controller"]["executable"], this->executable_); + { + setFromConfig(nav_conf["controller"]["executable"], this->executable_); + setFromConfig(nav_conf["controller"]["executable_type"], this->executable_type_); + + if (executable_type_ == "ros") + { + setFromConfig(nav_conf["controller"]["package_name"], this->package_name_); + } + + this->feature_enabled_ = true; + // Optional parameters if (this->feature_enabled_) { @@ -116,16 +124,23 @@ FeatureNavigation::FeatureNavigation(const YAML::Node& nav_conf) setFromConfig(nav_conf["controller"]["global_planner"], this->global_planner_); setFromConfig(nav_conf["controller"]["local_planner"], this->local_planner_); setFromConfig(nav_conf["controller"]["pose_topic"], this->pose_topic_); - } + } } /* * Get the driver configuration. */ if (nav_conf["driver"].IsDefined()) - { - this->driver_enabled_ = setFromConfig(nav_conf["driver"]["package_name"], this->driver_package_name_) - && setFromConfig(nav_conf["driver"]["executable"], this->driver_executable_); + { + setFromConfig(nav_conf["driver"]["executable"], this->driver_executable_); + setFromConfig(nav_conf["driver"]["executable_type"], this->driver_executable_type_); + + if (driver_executable_type_ == "ros") + { + setFromConfig(nav_conf["driver"]["package_name"], this->driver_package_name_); + } + + this->driver_enabled_ = true; // Optional parameters if (this->driver_enabled_) { @@ -133,6 +148,7 @@ FeatureNavigation::FeatureNavigation(const YAML::Node& nav_conf) setFromConfig(nav_conf["driver"]["odom_topic"], this->odom_topic_); setFromConfig(nav_conf["driver"]["cmd_vel_topic"], this->cmd_vel_topic_); } + } } diff --git a/src/robot_manager.cpp b/src/robot_manager.cpp index 73ba048..e0d1e39 100644 --- a/src/robot_manager.cpp +++ b/src/robot_manager.cpp @@ -193,7 +193,7 @@ try req_rm.data_pose.pose.orientation.z = req.data_pose.pose.orientation.z; req_rm.data_pose.pose.orientation.w = req.data_pose.pose.orientation.w; - req_rm.data_pose_array = std::vector{}; // TODO + req_rm.data_pose_array = std::vector{}; // TODO // TODO: Add pose and pose array loaded_robot->invokeCustomFeature(req.custom_feature_name, req_rm); From a78b7aac47c2dac490dbeb89671abd97d5e2ab21 Mon Sep 17 00:00:00 2001 From: FabianEP11 Date: Wed, 6 Sep 2023 18:28:31 -0500 Subject: [PATCH 02/12] Testing feedback --- CMakeLists.txt | 1 + .../custom_datastructures.h | 32 +++++++++ .../temoto_robot_manager/custom_plugin_base.h | 2 +- .../navigation_datastructures.h | 23 +++++++ .../navigation_plugin_base.h | 2 +- .../navigation_plugin_helper.h | 8 +-- .../temoto_robot_manager/rm_datastructures.h | 36 +--------- include/temoto_robot_manager/robot.h | 5 +- include/temoto_robot_manager/robot_manager.h | 12 +++- .../robot_manager_interface.h | 66 ++++++++++++++++++ .../robot_manager_services.h | 3 + msg/NavigationFeedback.msg | 12 ++++ src/custom_plugin_helper.cpp | 1 - src/navigation_plugin_helper.cpp | 30 ++------ src/robot.cpp | 69 ++++++++++++++----- src/robot_manager.cpp | 43 +++++++++++- 16 files changed, 257 insertions(+), 88 deletions(-) create mode 100644 include/temoto_robot_manager/custom_datastructures.h create mode 100644 include/temoto_robot_manager/navigation_datastructures.h create mode 100644 msg/NavigationFeedback.msg diff --git a/CMakeLists.txt b/CMakeLists.txt index d964550..386cb67 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -33,6 +33,7 @@ find_package(class_loader REQUIRED) add_message_files(FILES CustomFeedback.msg + NavigationFeedback.msg ) add_service_files(FILES diff --git a/include/temoto_robot_manager/custom_datastructures.h b/include/temoto_robot_manager/custom_datastructures.h new file mode 100644 index 0000000..86346db --- /dev/null +++ b/include/temoto_robot_manager/custom_datastructures.h @@ -0,0 +1,32 @@ +#ifndef TEMOTO_ROBOT_MANAGER__CUSTOM_DATASTRUCTURES_H +#define TEMOTO_ROBOT_MANAGER__CUSTOM_DATASTRUCTURES_H + +#include "temoto_robot_manager/rm_datastructures.h" + +namespace temoto_robot_manager +{ + +struct RmCustomRequest +{ + Header header; + Position position; + Orientation orientation; + Pose pose; + PoseStamped poseStamped; + std::string data_str; + std::vector data_str_array; + double data_num; + std::vector data_num_array; + PoseStamped data_pose; + std::vector data_pose_array; +}; + +struct RmCustomFeedback +{ + uint8_t status; + double progress; +}; + +} // temoto_robot_manager + +#endif \ No newline at end of file diff --git a/include/temoto_robot_manager/custom_plugin_base.h b/include/temoto_robot_manager/custom_plugin_base.h index 23f8faf..9ece19f 100644 --- a/include/temoto_robot_manager/custom_plugin_base.h +++ b/include/temoto_robot_manager/custom_plugin_base.h @@ -1,7 +1,7 @@ #ifndef TEMOTO_ROBOT_MANAGER__CUSTOM_PLUGIN_BASE_H #define TEMOTO_ROBOT_MANAGER__CUSTOM_PLUGIN_BASE_H -#include "temoto_robot_manager/rm_datastructures.h" +#include "temoto_robot_manager/custom_datastructures.h" #include namespace temoto_robot_manager diff --git a/include/temoto_robot_manager/navigation_datastructures.h b/include/temoto_robot_manager/navigation_datastructures.h new file mode 100644 index 0000000..47c07ea --- /dev/null +++ b/include/temoto_robot_manager/navigation_datastructures.h @@ -0,0 +1,23 @@ +#ifndef TEMOTO_ROBOT_MANAGER__NAVIGATION_DATASTRUCTURES_H +#define TEMOTO_ROBOT_MANAGER__NAVIGATION_DATASTRUCTURES_H + +#include "temoto_robot_manager/rm_datastructures.h" + +namespace temoto_robot_manager +{ + +struct RmNavigationGoal +{ + PoseStamped goal_pose; +}; + +struct RmNavigationFeedback +{ + uint8_t status; + double progress; + PoseStamped base_position; +}; + +} // temoto_robot_manager + +#endif \ No newline at end of file diff --git a/include/temoto_robot_manager/navigation_plugin_base.h b/include/temoto_robot_manager/navigation_plugin_base.h index 4944410..ed64019 100644 --- a/include/temoto_robot_manager/navigation_plugin_base.h +++ b/include/temoto_robot_manager/navigation_plugin_base.h @@ -1,7 +1,7 @@ #ifndef TEMOTO_ROBOT_MANAGER__NAVIGATION_PLUGIN_BASE_H #define TEMOTO_ROBOT_MANAGER__NAVIGATION_PLUGIN_BASE_H -#include "temoto_robot_manager/rm_datastructures.h" +#include "temoto_robot_manager/navigation_datastructures.h" #include namespace temoto_robot_manager diff --git a/include/temoto_robot_manager/navigation_plugin_helper.h b/include/temoto_robot_manager/navigation_plugin_helper.h index 12dbeed..33af4ab 100644 --- a/include/temoto_robot_manager/navigation_plugin_helper.h +++ b/include/temoto_robot_manager/navigation_plugin_helper.h @@ -12,15 +12,15 @@ namespace temoto_robot_manager struct RmNavigationFeedbackWrap : RmNavigationFeedback { std::string robot_name; - std::string navigation_feature_name; - std::string request_id; + // std::string navigation_feature_name; + // std::string request_id; }; struct RmNavigationRequestWrap : RmNavigationGoal { std::string robot_name; - std::string navigation_feature_name; - std::string request_id; + // std::string navigation_feature_name; + // std::string request_id; }; class NavigationPluginHelper; // Forward declaration diff --git a/include/temoto_robot_manager/rm_datastructures.h b/include/temoto_robot_manager/rm_datastructures.h index 789fde1..0a6a212 100644 --- a/include/temoto_robot_manager/rm_datastructures.h +++ b/include/temoto_robot_manager/rm_datastructures.h @@ -1,5 +1,5 @@ -#ifndef TEMOTO_ROBOT_MANAGER__CUSTOM_DATASTRUCTURES_H -#define TEMOTO_ROBOT_MANAGER__CUSTOM_DATASTRUCTURES_H +#ifndef TEMOTO_ROBOT_MANAGER__RM_DATASTRUCTURES_H +#define TEMOTO_ROBOT_MANAGER__RM_DATASTRUCTURES_H #include #include @@ -41,38 +41,6 @@ struct PoseStamped Pose pose; }; -struct RmCustomRequest -{ - Header header; - Position position; - Orientation orientation; - Pose pose; - PoseStamped poseStamped; - std::string data_str; - std::vector data_str_array; - double data_num; - std::vector data_num_array; - PoseStamped data_pose; - std::vector data_pose_array; -}; - -struct RmCustomFeedback -{ - uint8_t status; - double progress; -}; - -struct RmNavigationGoal -{ - PoseStamped goal_pose; -}; - -struct RmNavigationFeedback -{ - uint8_t status; - PoseStamped base_position; -}; - } // temoto_robot_manager #endif \ No newline at end of file diff --git a/include/temoto_robot_manager/robot.h b/include/temoto_robot_manager/robot.h index 9c81431..9836960 100644 --- a/include/temoto_robot_manager/robot.h +++ b/include/temoto_robot_manager/robot.h @@ -44,7 +44,8 @@ class Robot Robot(RobotConfigPtr config_ , const std::string& resource_id , temoto_resource_registrar::ResourceRegistrarRos1& resource_registrar - , CustomFeatureUpdateCb custom_feature_update_cb); + , CustomFeatureUpdateCb custom_feature_update_cb + , NavigationFeatureUpdateCb navigation_feature_update_cb); virtual ~Robot(); void load(); @@ -143,6 +144,8 @@ class Robot NavigationPluginHelperPtr navigation_feature_plugin_; mutable std::mutex navigation_feature_plugins_mutex_; NavigationFeatureUpdateCb navigation_feature_update_cb_; + std::thread navigation_feature_feedback_thread_; + bool navigation_feature_feedback_thread_running_; // Custom related std::map custom_feature_plugins_; diff --git a/include/temoto_robot_manager/robot_manager.h b/include/temoto_robot_manager/robot_manager.h index f7ba9a7..e268aa5 100644 --- a/include/temoto_robot_manager/robot_manager.h +++ b/include/temoto_robot_manager/robot_manager.h @@ -22,7 +22,8 @@ #include "temoto_core/trr/config_synchronizer.h" #include "temoto_core/ConfigSync.h" #include "temoto_process_manager/process_manager_services.hpp" -#include "temoto_robot_manager/rm_datastructures.h" +#include "temoto_robot_manager/custom_datastructures.h" +// #include "temoto_robot_manager/navigation_datastructures.h" #include "temoto_robot_manager/robot_manager_services.h" #include "temoto_robot_manager/robot.h" #include "temoto_robot_manager/robot_config.h" @@ -109,6 +110,8 @@ class RobotManager : public temoto_core::BaseSubsystem void customFeatureUpdateCb(const RmCustomFeedbackWrap& feedback); + void navigationFeatureUpdateCb(const RmNavigationFeedbackWrap& feedback); + RobotConfigs parseRobotConfigs(const YAML::Node& config); RobotConfigPtr findRobot(const std::string& robot_name, const RobotConfigs& robot_infos); @@ -153,6 +156,13 @@ class RobotManager : public temoto_core::BaseSubsystem ros::ServiceClient client_navigation_goal_; ros::ServiceClient client_gripper_control_position_; + // DO I NEED THIS? + RobotNavigationGoal ongoing_navigation_requests_; + std::mutex mutex_ongoing_navigation_requests_; + + ros::Publisher pub_navigation_feature_feedback_; + std::mutex mutex_pub_navigation_feature_feedback_; + /* * CUSTOM FEATURE */ diff --git a/include/temoto_robot_manager/robot_manager_interface.h b/include/temoto_robot_manager/robot_manager_interface.h index 58a7a5c..66b50c3 100644 --- a/include/temoto_robot_manager/robot_manager_interface.h +++ b/include/temoto_robot_manager/robot_manager_interface.h @@ -73,6 +73,7 @@ class RobotManagerInterface nh_.serviceClient(channels::custom::PREEMPT); custom_feedback_ = nh_.subscribe(channels::custom::FEEDBACK, 1, &RobotManagerInterface::customFeedback, this); + navigation_feedback_ = nh_.subscribe(NAVIGATION_FEEDBACK, 1, &RobotManagerInterface::navigationFeedbackCb, this); initialized_ = true; } else @@ -383,6 +384,53 @@ class RobotManagerInterface } } + bool navigationGoal(RobotNavigationGoal& goal) + { + if (goal.request.target_pose.header.frame_id.empty()) + { + throw TEMOTO_ERRSTACK("Reference frame is not defined"); + } + + if (!client_navigation_goal_.call(goal)) + { + throw TEMOTO_ERRSTACK("Unable to reach robot_manager"); + } + + if (!goal.response.success) + { + throw TEMOTO_ERRSTACK("Unsuccessful attempt to invoke 'navigationGoal'"); + } + return goal.response.success; + } + + std::optional getNavigationFeedback(const std::string& robot_name) + { + std::lock_guard lock(navigation_queries_mutex_); + auto ongoing_query_it = ongoing_navigation_queries_.find(robot_name); + + if (ongoing_query_it == ongoing_navigation_queries_.end()) + { + return {}; + //throw TEMOTO_ERRSTACK("Could not find the request in the list of ongoing requests"); + } + + if (ongoing_query_it->second.status == NavigationFeedback::FINISHED) + { + auto feedback = ongoing_query_it->second; + ongoing_navigation_queries_.erase(ongoing_query_it); + return feedback; + } + else + { + return ongoing_query_it->second; + } + } + + bool cancelNavigationGoal(const std::string& robot_name) + { + + } + void controlGripperPosition(const std::string& robot_name, const float& position) { temoto_robot_manager::RobotGripperControlPosition msg; @@ -479,6 +527,20 @@ class RobotManagerInterface return; } + void navigationFeedbackCb(const NavigationFeedback& msg) + { + TEMOTO_INFO_STREAM_("Nav feedback" << msg); + std::lock_guard lock(custom_queries_mutex_); + auto ongoing_nav_query_it = ongoing_navigation_queries_.find(msg.robot_name); + + if (ongoing_nav_query_it != ongoing_navigation_queries_.end()) + { + ongoing_nav_query_it->second = msg; + } + + return; + } + std::string rr_name_; std::string unique_suffix_; bool initialized_; @@ -502,6 +564,10 @@ class RobotManagerInterface std::mutex custom_queries_mutex_; std::map ongoing_custom_queries_; + ros::Subscriber navigation_feedback_; + std::mutex navigation_queries_mutex_; + std::map ongoing_navigation_queries_; + std::unique_ptr resource_registrar_; }; diff --git a/include/temoto_robot_manager/robot_manager_services.h b/include/temoto_robot_manager/robot_manager_services.h index daeca23..1396e20 100644 --- a/include/temoto_robot_manager/robot_manager_services.h +++ b/include/temoto_robot_manager/robot_manager_services.h @@ -31,6 +31,7 @@ #include "temoto_robot_manager/CustomRequest.h" #include "temoto_robot_manager/CustomRequestPreempt.h" #include "temoto_robot_manager/CustomFeedback.h" +#include "temoto_robot_manager/NavigationFeedback.h" #include @@ -54,6 +55,8 @@ const std::string SERVER_SET_MODE = MANAGER + "/" + "set_mode"; const std::string SERVER_GRIPPER_CONTROL_POSITION = MANAGER + "/" + "gripper_control_position"; } +const std::string NAVIGATION_FEEDBACK = srv_name::MANAGER + "/" + "navigation_feedback"; + namespace channels { namespace custom diff --git a/msg/NavigationFeedback.msg b/msg/NavigationFeedback.msg new file mode 100644 index 0000000..f3a5bc2 --- /dev/null +++ b/msg/NavigationFeedback.msg @@ -0,0 +1,12 @@ +Header header +string robot_name +# string request_id + +uint8 status +uint8 IDLE=2 +uint8 RUNNING=3 +uint8 FINISHED=4 +uint8 CANCELLED=5 + +float64 progress +geometry_msgs/PoseStamped base_position \ No newline at end of file diff --git a/src/custom_plugin_helper.cpp b/src/custom_plugin_helper.cpp index 2611fc6..ad7109d 100644 --- a/src/custom_plugin_helper.cpp +++ b/src/custom_plugin_helper.cpp @@ -13,7 +13,6 @@ CustomPluginHelper::CustomPluginHelper(const std::string& plugin_path, CustomFea { try { - TEMOTO_INFO_STREAM_("\n ====== [Custom plug Helper] try ============" + plugin_path_); class_loader = std::make_shared(plugin_path_, false); if (class_loader->getAvailableClasses().empty()) diff --git a/src/navigation_plugin_helper.cpp b/src/navigation_plugin_helper.cpp index e58fc56..c3e53fd 100644 --- a/src/navigation_plugin_helper.cpp +++ b/src/navigation_plugin_helper.cpp @@ -15,8 +15,6 @@ NavigationPluginHelper::NavigationPluginHelper(const std::string& plugin_path, N { try { - TEMOTO_INFO_("\n ====== [Nav plug Helper] try ============"); - TEMOTO_INFO_STREAM_("\n ====== [Nav plug Helper] try ============" + plugin_path_); class_loader = std::make_shared(plugin_path_, false); if (class_loader->getAvailableClasses().empty()) @@ -27,16 +25,13 @@ try std::string plugin_name = class_loader->getAvailableClasses().front(); plugin = class_loader->createSharedInstance(plugin_name); - TEMOTO_INFO_("\n ====== [Nav plug Helper] createSharedInstance() ============"); - TEMOTO_INFO_(plugin_name); if (!class_loader->isLibraryLoaded()) { setState(State::ERROR); throw TEMOTO_ERRSTACK("Unable to load plugin '" + plugin_path_ + "'"); } - setState(State::UNINITIALIZED); - TEMOTO_INFO_("\n ====== [Nav plug Helper] End Constructor ============"); + setState(State::UNINITIALIZED); } catch(class_loader::ClassLoaderException & e) { @@ -86,19 +81,11 @@ NavigationPluginHelper::~NavigationPluginHelper() void NavigationPluginHelper::initialize() try { - TEMOTO_INFO_("====== [Nav plug Helper] Try Initialize () ============"); - State enumValue = getState(); - - std::cout << "Enum value: " << static_cast(enumValue) << std::endl; - if (getState() != State::UNINITIALIZED && getState() != State::FINISHED) { - TEMOTO_INFO_("\n ====== [Nav plug Helper] if getState() ============"); setState(State::ERROR); - TEMOTO_INFO_("\n ====== [Nav plug Helper] setState() ============"); throw TEMOTO_ERRSTACK("Cannot initalize the plugin. It has to be in 'UNINITIALIZED' state for that"); } - TEMOTO_INFO_("====== [Nav plug Helper] getState () ============"); if (!plugin->initialize()) { setState(State::ERROR); @@ -115,8 +102,6 @@ catch(class_loader::ClassLoaderException & e) void NavigationPluginHelper::sendGoal(const RmNavigationRequestWrap& request) { - TEMOTO_INFO_("================= [Nav plug Helper] sendGoal - Initialize=================="); - initialize(); if (getState() != State::INITIALIZED) @@ -193,10 +178,7 @@ catch(...) NavigationPluginHelper::State NavigationPluginHelper::getState() const { - TEMOTO_INFO_("\n ====== [getState] ============"); std::lock_guard l(mutex_state_); - TEMOTO_INFO_("\n ====== [getState] before return ============"); - std::cout << "Enum value: " << static_cast(state_) << std::endl; return state_; } @@ -212,13 +194,15 @@ void NavigationPluginHelper::sendUpdate() const if (fb.has_value()) { RmNavigationFeedbackWrap fbw; - + fbw.robot_name = current_request_->robot_name; - fbw.navigation_feature_name = current_request_->navigation_feature_name; - fbw.request_id = current_request_->request_id; + // fbw.navigation_feature_name = current_request_->navigation_feature_name; + // fbw.request_id = current_request_->request_id; fbw.status = uint8_t(state_); + fbw.progress = fb->progress; fbw.base_position = fb->base_position; - + std::cout << "sendUpdate: progress -->" << fb->progress << " " << fbw.progress << std::endl; + std::cout << "Robot Name -->" << current_request_->robot_name << " " << fbw.robot_name << std::endl; update_cb_(fbw); } } diff --git a/src/robot.cpp b/src/robot.cpp index 32321d6..203789d 100644 --- a/src/robot.cpp +++ b/src/robot.cpp @@ -25,16 +25,19 @@ namespace temoto_robot_manager Robot::Robot(RobotConfigPtr config , const std::string& resource_id , temoto_resource_registrar::ResourceRegistrarRos1& resource_registrar -, CustomFeatureUpdateCb custom_feature_update_cb) +, CustomFeatureUpdateCb custom_feature_update_cb +, NavigationFeatureUpdateCb navigation_feature_update_cb) : config_(config) , robot_resource_id_(resource_id) , resource_registrar_(resource_registrar) , custom_feature_update_cb_(custom_feature_update_cb) +, navigation_feature_update_cb_(navigation_feature_update_cb) , is_plan_valid_(false) , robot_operational_(true) , state_in_error_(false) , robot_loaded_(false) , custom_feature_feedback_thread_running_(false) +, navigation_feature_feedback_thread_running_(false) {} Robot::~Robot() @@ -73,9 +76,18 @@ Robot::~Robot() config_->getFeatureManipulation().setDriverLoaded(false); } + // Stop Thread + navigation_feature_feedback_thread_running_ = false; + while (!navigation_feature_feedback_thread_.joinable()) + { + std::this_thread::sleep_for(std::chrono::milliseconds(5)); + } + navigation_feature_feedback_thread_.join(); + if (config_->getFeatureNavigation().isLoaded()) { - TEMOTO_DEBUG_("Unloading Navigation Feature."); + TEMOTO_DEBUG_("Unloading Navigation Feature."); + navigation_feature_plugin_.reset(); config_->getFeatureNavigation().setLoaded(false); } @@ -385,16 +397,13 @@ void Robot::loadManipulationDriver() // Load Move Base void Robot::loadNavigationController() { - TEMOTO_INFO_("0 ================= Loading Navigation contoller =================="); if (config_->getFeatureNavigation().isLoaded()) { return; // Return if already loaded. } - TEMOTO_INFO_("1 ================= Loading Navigation contoller =================="); try { FeatureNavigation& ftr = config_->getFeatureNavigation(); - TEMOTO_INFO_("2 ================= Loading Navigation contoller =================="); if (ftr.getExecutableType() == "ros") { // Previous Implementation @@ -418,16 +427,15 @@ void Robot::loadNavigationController() try { const std::string& plugin_path = ftr.getExecutable(); - TEMOTO_INFO_("Executable"); - TEMOTO_INFO_(plugin_path); NavigationPluginHelperPtr plugin_helper = std::make_shared(plugin_path, navigation_feature_update_cb_); std::lock_guard l(navigation_feature_plugins_mutex_); + navigation_feature_plugin_ = plugin_helper; // ftr.setLoaded(true); // /* - // * Start the custom feature feedback thread + // * Start the navigation feature feedback thread // */ // if (navigation_feature_feedback_thread_running_) // { @@ -438,22 +446,17 @@ void Robot::loadNavigationController() // navigation_feature_feedback_thread_ = std::thread( // [&] // { - // TEMOTO_DEBUG_("Custom feature feedback thread running"); + // TEMOTO_DEBUG_("Navigation feature feedback thread running"); // while (navigation_feature_feedback_thread_running_) // { - // std::lock_guard l(custom_feature_plugins_mutex_); - - // for (const auto& cfp : custom_feature_plugins_) - // { - // cfp.second->sendUpdate(); - // std::this_thread::sleep_for(std::chrono::milliseconds(50)); - // } + // std::lock_guard l(navigation_feature_plugins_mutex_); + // navigation_feature_plugin_->sendUpdate(); // std::this_thread::sleep_for(std::chrono::milliseconds(200)); // } - // TEMOTO_DEBUG_("Custom feature feedback thread finished"); + // TEMOTO_DEBUG_("Navigation feature feedback thread finished"); // }); } catch(resource_registrar::TemotoErrorStack& error_stack) @@ -470,8 +473,6 @@ void Robot::loadNavigationController() } } - TEMOTO_INFO_("3 ================= Loadng Navigation contoller =================="); - ros::Duration(5).sleep(); ftr.setLoaded(true); TEMOTO_DEBUG_("Feature 'Navigation Controller' loaded."); @@ -1031,17 +1032,47 @@ void Robot::goalNavigation(const geometry_msgs::PoseStamped& target_pose) TEMOTO_INFO_("================= [robot.cpp 1031] getFeatureNavigation =================="); FeatureNavigation& ftr = config_->getFeatureNavigation(); RmNavigationRequestWrap request; + request.robot_name = config_->getName(); request.goal_pose.header.frame_id = target_pose.header.frame_id; request.goal_pose.pose.position.x = target_pose.pose.position.x; request.goal_pose.pose.position.y = target_pose.pose.position.y; request.goal_pose.pose.position.z = target_pose.pose.position.z; + TEMOTO_INFO_STREAM_("position z: " << request.goal_pose.pose.position.z); request.goal_pose.pose.orientation.x = target_pose.pose.orientation.x; request.goal_pose.pose.orientation.y = target_pose.pose.orientation.y; request.goal_pose.pose.orientation.z = target_pose.pose.orientation.z; request.goal_pose.pose.orientation.w = target_pose.pose.orientation.w; TEMOTO_INFO_("================= Before send Goal =================="); + + + navigation_feature_plugin_->sendGoal(request); + /* + * Start the navigation feature feedback thread + */ + if (navigation_feature_feedback_thread_running_) + { + return; + } + + navigation_feature_feedback_thread_running_ = true; + navigation_feature_feedback_thread_ = std::thread( + [&] + { + TEMOTO_DEBUG_("Navigation feature feedback thread running"); + + while (navigation_feature_feedback_thread_running_) + { + std::lock_guard l(navigation_feature_plugins_mutex_); + + navigation_feature_plugin_->sendUpdate(); + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + } + + TEMOTO_DEBUG_("Navigation feature feedback thread finished"); + }); + // std::string act_rob_ns = "/" + config_->getAbsRobotNamespace() + "/move_base"; // MoveBaseClient ac(act_rob_ns, true); diff --git a/src/robot_manager.cpp b/src/robot_manager.cpp index e0d1e39..e0f5e93 100644 --- a/src/robot_manager.cpp +++ b/src/robot_manager.cpp @@ -127,6 +127,7 @@ RobotManager::RobotManager(const std::string& config_base_path, bool restore_fro &RobotManager::customFeaturePreemptCb, this); pub_custom_feature_feedback_ = nh_.advertise(channels::custom::FEEDBACK, 10); + pub_navigation_feature_feedback_ = nh_.advertise(NAVIGATION_FEEDBACK, 10); TEMOTO_INFO_("Robot manager is ready.\n"); } @@ -295,6 +296,31 @@ void RobotManager::customFeatureUpdateCb(const RmCustomFeedbackWrap& feedback) pub_custom_feature_feedback_.publish(msg); } +void RobotManager::navigationFeatureUpdateCb(const RmNavigationFeedbackWrap& feedback) +{ + NavigationFeedback msg; + + msg.header.stamp = ros::Time::now(); + msg.robot_name = feedback.robot_name; + // msg.request_id = feedback.request_id; + msg.status = feedback.status; + msg.progress = feedback.progress; + + // Convert from RM/PoseStamped to geomety_msgs/PoseStamped + msg.base_position.header.frame_id = feedback.base_position.header.frame_id; + msg.base_position.header.stamp = ros::Time::now(); + msg.base_position.pose.position.x = feedback.base_position.pose.position.x; + msg.base_position.pose.position.y = feedback.base_position.pose.position.y; + msg.base_position.pose.position.z = feedback.base_position.pose.position.z; + msg.base_position.pose.orientation.x = feedback.base_position.pose.orientation.x; + msg.base_position.pose.orientation.y = feedback.base_position.pose.orientation.y; + msg.base_position.pose.orientation.z = feedback.base_position.pose.orientation.z; + msg.base_position.pose.orientation.w = feedback.base_position.pose.orientation.w; + + std::lock_guard l(mutex_pub_navigation_feature_feedback_); + pub_navigation_feature_feedback_.publish(msg); +} + void RobotManager::findRobotDescriptionFiles(boost::filesystem::path current_dir) { if (std::string(current_dir.c_str()).empty()) @@ -348,7 +374,8 @@ void RobotManager::loadCb(RobotLoad::Request& req, RobotLoad::Response& res) try { auto loaded_robot = std::make_shared(config, res.temoto_metadata.request_id, resource_registrar_ - , std::bind(&RobotManager::customFeatureUpdateCb, this, std::placeholders::_1)); + , std::bind(&RobotManager::customFeatureUpdateCb, this, std::placeholders::_1) + , std::bind(&RobotManager::navigationFeatureUpdateCb, this, std::placeholders::_1)); loaded_robot->load(); loaded_robots_.push_back(loaded_robot); @@ -386,7 +413,8 @@ void RobotManager::loadCb(RobotLoad::Request& req, RobotLoad::Response& res) TEMOTO_INFO_("Call to remote RobotManager was sucessful."); auto loaded_robot = std::make_shared(config, res.temoto_metadata.request_id, resource_registrar_ - , std::bind(&RobotManager::customFeatureUpdateCb, this, std::placeholders::_1)); + , std::bind(&RobotManager::customFeatureUpdateCb, this, std::placeholders::_1) + , std::bind(&RobotManager::navigationFeatureUpdateCb, this, std::placeholders::_1)); loaded_robots_.push_back(loaded_robot); return; @@ -753,15 +781,23 @@ catch(resource_registrar::TemotoErrorStack& e) bool RobotManager::goalNavigationCb(RobotNavigationGoal::Request& req, RobotNavigationGoal::Response& res) try { + TEMOTO_INFO_("Received a goal Navigation request"); + TEMOTO_DEBUG_STREAM_("Request:\n" << req); + std::lock_guard l(mutex_ongoing_navigation_requests_); + + + TEMOTO_INFO_STREAM_("Robot name: " << req.robot_name); RobotPtr loaded_robot = findLoadedRobot(req.robot_name); if (loaded_robot->isLocal()) { + TEMOTO_INFO_STREAM_(" Loaded Robot, it is local "); TEMOTO_DEBUG_STREAM_("Navigating '" << req.robot_name << " to pose: " << req.target_pose << " ..."); loaded_robot->goalNavigation(req.target_pose); // The robot would move with respect to the coordinate frame defined in the header res.success = true; } else { + TEMOTO_INFO_STREAM_(" Loaded Robot, it is remote "); std::string topic = "/" + loaded_robot->getConfig()->getTemotoNamespace() + "/" + srv_name::SERVER_NAVIGATION_GOAL; TEMOTO_DEBUG_STREAM_("Forwarding the request to remote robot manager at '" << topic << "'."); @@ -993,7 +1029,8 @@ void RobotManager::restoreState() continue; } auto robot = std::make_shared(robot_config, query.response.temoto_metadata.request_id, resource_registrar_ - , std::bind(&RobotManager::customFeatureUpdateCb, this, std::placeholders::_1)); + , std::bind(&RobotManager::customFeatureUpdateCb, this, std::placeholders::_1) + , std::bind(&RobotManager::navigationFeatureUpdateCb, this, std::placeholders::_1)); robot->recover(query.response.temoto_metadata.request_id); loaded_robots_.push_back(robot); } From 13af5928b1f839bddc7dce5f4386f2a55a3d76ce Mon Sep 17 00:00:00 2001 From: FabianEP11 Date: Wed, 6 Sep 2023 21:30:42 -0500 Subject: [PATCH 03/12] cancel goal --- CMakeLists.txt | 1 + include/temoto_robot_manager/robot.h | 1 + include/temoto_robot_manager/robot_manager.h | 5 +- .../robot_manager_interface.h | 26 ++++++++-- .../robot_manager_services.h | 7 ++- src/navigation_plugin_helper.cpp | 5 +- src/robot.cpp | 11 ++++ src/robot_manager.cpp | 51 ++++++++++++++++++- srv/RobotCancelNavigationGoal.srv | 6 +++ 9 files changed, 101 insertions(+), 12 deletions(-) create mode 100644 srv/RobotCancelNavigationGoal.srv diff --git a/CMakeLists.txt b/CMakeLists.txt index 386cb67..7b30d64 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -51,6 +51,7 @@ add_service_files(FILES RobotGetConfig.srv RobotNavigationGoal.srv RobotGripperControlPosition.srv + RobotCancelNavigationGoal.srv ) generate_messages(DEPENDENCIES diff --git a/include/temoto_robot_manager/robot.h b/include/temoto_robot_manager/robot.h index 9836960..498cedd 100644 --- a/include/temoto_robot_manager/robot.h +++ b/include/temoto_robot_manager/robot.h @@ -63,6 +63,7 @@ class Robot std::vector getNamedTargetPoses(const std::string& planning_group_name); void goalNavigation(const geometry_msgs::PoseStamped& target_pose); + void cancelNavigationGoal(); void controlGripper(const std::string& robot_name, const float position); void invokeCustomFeature(const std::string& custom_feature_name, const RmCustomRequestWrap& request); diff --git a/include/temoto_robot_manager/robot_manager.h b/include/temoto_robot_manager/robot_manager.h index e268aa5..60b432e 100644 --- a/include/temoto_robot_manager/robot_manager.h +++ b/include/temoto_robot_manager/robot_manager.h @@ -112,6 +112,8 @@ class RobotManager : public temoto_core::BaseSubsystem void navigationFeatureUpdateCb(const RmNavigationFeedbackWrap& feedback); + bool cancelNavigationGoalCb(RobotCancelNavigationGoal::Request& req, RobotCancelNavigationGoal::Response& res); + RobotConfigs parseRobotConfigs(const YAML::Node& config); RobotConfigPtr findRobot(const std::string& robot_name, const RobotConfigs& robot_infos); @@ -145,6 +147,7 @@ class RobotManager : public temoto_core::BaseSubsystem ros::ServiceServer server_navigation_goal_; ros::ServiceServer server_gripper_control_position_; ros::ServiceServer server_get_robot_config_; + ros::ServiceServer server_cancel_navigation_goal_; ros::ServiceClient client_plan_; ros::ServiceClient client_exec_; @@ -155,8 +158,8 @@ class RobotManager : public temoto_core::BaseSubsystem ros::ServiceClient client_set_mode_; ros::ServiceClient client_navigation_goal_; ros::ServiceClient client_gripper_control_position_; + ros::ServiceClient client_cancel_navigation_goal_; - // DO I NEED THIS? RobotNavigationGoal ongoing_navigation_requests_; std::mutex mutex_ongoing_navigation_requests_; diff --git a/include/temoto_robot_manager/robot_manager_interface.h b/include/temoto_robot_manager/robot_manager_interface.h index 66b50c3..f242478 100644 --- a/include/temoto_robot_manager/robot_manager_interface.h +++ b/include/temoto_robot_manager/robot_manager_interface.h @@ -66,6 +66,9 @@ class RobotManagerInterface nh_.serviceClient(srv_name::SERVER_GRIPPER_CONTROL_POSITION); client_get_robot_config_ = nh_.serviceClient(srv_name::SERVER_GET_CONFIG); + + client_cancel_navigation_goal_ = + nh_.serviceClient(srv_name::SERVER_CANCEL_NAVIGATION_GOAL); client_custom_request_ = nh_.serviceClient(channels::custom::REQUEST); @@ -73,7 +76,7 @@ class RobotManagerInterface nh_.serviceClient(channels::custom::PREEMPT); custom_feedback_ = nh_.subscribe(channels::custom::FEEDBACK, 1, &RobotManagerInterface::customFeedback, this); - navigation_feedback_ = nh_.subscribe(NAVIGATION_FEEDBACK, 1, &RobotManagerInterface::navigationFeedbackCb, this); + navigation_feedback_ = nh_.subscribe(srv_name::NAVIGATION_FEEDBACK, 1, &RobotManagerInterface::navigationFeedbackCb, this); initialized_ = true; } else @@ -428,7 +431,20 @@ class RobotManagerInterface bool cancelNavigationGoal(const std::string& robot_name) { - + temoto_robot_manager::RobotCancelNavigationGoal msg; + msg.request.robot_name = robot_name; + + if (!client_cancel_navigation_goal_.call(msg)) + { + throw TEMOTO_ERRSTACK("Unable to reach the CancelNavigationGoal server"); + } + + if (!msg.response.result) + { + throw TEMOTO_ERRSTACK("Unsuccessful attempt to invoke 'CancelNavigationGoal'"); + } + + return msg.response.result; } void controlGripperPosition(const std::string& robot_name, const float& position) @@ -500,6 +516,7 @@ class RobotManagerInterface client_set_manipulation_target_.shutdown(); client_get_manipulation_target_.shutdown(); client_navigation_goal_.shutdown(); + client_cancel_navigation_goal_.shutdown(); client_gripper_control_position_.shutdown(); TEMOTO_DEBUG_("RobotManagerInterface destroyed."); @@ -523,13 +540,11 @@ class RobotManagerInterface { ongoing_query_it->second.feedback = msg; } - - return; } void navigationFeedbackCb(const NavigationFeedback& msg) { - TEMOTO_INFO_STREAM_("Nav feedback" << msg); + TEMOTO_INFO_STREAM_("progress: " << msg.progress << " " << static_cast(msg.status)); std::lock_guard lock(custom_queries_mutex_); auto ongoing_nav_query_it = ongoing_navigation_queries_.find(msg.robot_name); @@ -555,6 +570,7 @@ class RobotManagerInterface ros::ServiceClient client_get_manipulation_target_; ros::ServiceClient client_get_manipulation_named_targets_; ros::ServiceClient client_navigation_goal_; + ros::ServiceClient client_cancel_navigation_goal_; ros::ServiceClient client_gripper_control_position_; ros::ServiceClient client_get_robot_config_; diff --git a/include/temoto_robot_manager/robot_manager_services.h b/include/temoto_robot_manager/robot_manager_services.h index 1396e20..b039de9 100644 --- a/include/temoto_robot_manager/robot_manager_services.h +++ b/include/temoto_robot_manager/robot_manager_services.h @@ -32,6 +32,7 @@ #include "temoto_robot_manager/CustomRequestPreempt.h" #include "temoto_robot_manager/CustomFeedback.h" #include "temoto_robot_manager/NavigationFeedback.h" +#include "temoto_robot_manager/RobotCancelNavigationGoal.h" #include @@ -53,9 +54,13 @@ const std::string SERVER_GET_MANIPULATION_NAMED_TARGETS = MANAGER + "/" + "get_m const std::string SERVER_NAVIGATION_GOAL = MANAGER + "/" + "navigation_goal"; const std::string SERVER_SET_MODE = MANAGER + "/" + "set_mode"; const std::string SERVER_GRIPPER_CONTROL_POSITION = MANAGER + "/" + "gripper_control_position"; +const std::string SERVER_CANCEL_NAVIGATION_GOAL = MANAGER + "/" + "cancel_navigation_goal"; + +const std::string NAVIGATION_FEEDBACK = MANAGER + "/" + "navigation_feedback"; + } -const std::string NAVIGATION_FEEDBACK = srv_name::MANAGER + "/" + "navigation_feedback"; + namespace channels { diff --git a/src/navigation_plugin_helper.cpp b/src/navigation_plugin_helper.cpp index c3e53fd..befe89e 100644 --- a/src/navigation_plugin_helper.cpp +++ b/src/navigation_plugin_helper.cpp @@ -92,7 +92,6 @@ try plugin.reset(); throw TEMOTO_ERRSTACK("Unable to initialize the plugin"); } - TEMOTO_INFO_("====== [Nav plug Helper] set State Initialized ============"); setState(State::INITIALIZED); } catch(class_loader::ClassLoaderException & e) @@ -201,8 +200,8 @@ void NavigationPluginHelper::sendUpdate() const fbw.status = uint8_t(state_); fbw.progress = fb->progress; fbw.base_position = fb->base_position; - std::cout << "sendUpdate: progress -->" << fb->progress << " " << fbw.progress << std::endl; - std::cout << "Robot Name -->" << current_request_->robot_name << " " << fbw.robot_name << std::endl; + std::cout << "sendUpdate: progress -->" << fb->progress << std::endl; + std::cout << "status: -->" << fbw.status << std::endl; update_cb_(fbw); } } diff --git a/src/robot.cpp b/src/robot.cpp index 203789d..0da4658 100644 --- a/src/robot.cpp +++ b/src/robot.cpp @@ -1106,6 +1106,17 @@ void Robot::goalNavigation(const geometry_msgs::PoseStamped& target_pose) } +void Robot::cancelNavigationGoal() +try +{ + navigation_feature_plugin_->cancelGoal(); +} +catch(resource_registrar::TemotoErrorStack& e) +{ + std::string message = "Unable to cancel navigation goal of robot '" + config_->getName() + "'."; + throw FWD_TEMOTO_ERRSTACK_WMSG(e, message); +} + void Robot::controlGripper(const std::string& robot_name,const float position) try { diff --git a/src/robot_manager.cpp b/src/robot_manager.cpp index e0f5e93..ef2e6c9 100644 --- a/src/robot_manager.cpp +++ b/src/robot_manager.cpp @@ -114,6 +114,10 @@ RobotManager::RobotManager(const std::string& config_base_path, bool restore_fro srv_name::SERVER_GET_CONFIG, &RobotManager::getRobotConfigCb, this); + server_cancel_navigation_goal_ = nh_.advertiseService( + srv_name::SERVER_CANCEL_NAVIGATION_GOAL, + &RobotManager::cancelNavigationGoalCb, + this); /* * Set up the Custom Feature channel @@ -127,7 +131,7 @@ RobotManager::RobotManager(const std::string& config_base_path, bool restore_fro &RobotManager::customFeaturePreemptCb, this); pub_custom_feature_feedback_ = nh_.advertise(channels::custom::FEEDBACK, 10); - pub_navigation_feature_feedback_ = nh_.advertise(NAVIGATION_FEEDBACK, 10); + pub_navigation_feature_feedback_ = nh_.advertise(srv_name::NAVIGATION_FEEDBACK, 10); TEMOTO_INFO_("Robot manager is ready.\n"); } @@ -184,7 +188,6 @@ try req_rm.data_num = req.data_num; req_rm.data_num_array = req.data_num_array; - // req_rm.data_pose = RmCustomRequest::PoseStamped{}; // TODO req_rm.data_pose.header.frame_id = req.data_pose.header.frame_id; req_rm.data_pose.pose.position.x = req.data_pose.pose.position.x; req_rm.data_pose.pose.position.y = req.data_pose.pose.position.y; @@ -824,6 +827,50 @@ catch(resource_registrar::TemotoErrorStack& e) return true; } +bool RobotManager::cancelNavigationGoalCb(RobotCancelNavigationGoal::Request& req, RobotCancelNavigationGoal::Response& res) +try +{ + TEMOTO_INFO_("Cancel Navigation goal request"); + TEMOTO_DEBUG_STREAM_("Request:\n" << req); + std::lock_guard l(mutex_ongoing_navigation_requests_); + + TEMOTO_INFO_STREAM_("Robot name: " << req.robot_name); + RobotPtr loaded_robot = findLoadedRobot(req.robot_name); + if (loaded_robot->isLocal()) + { + TEMOTO_INFO_STREAM_(" Loaded Robot, it is local "); + loaded_robot->cancelNavigationGoal(); + res.result = true; + } + else + { + TEMOTO_INFO_STREAM_(" Loaded Robot, it is remote "); + std::string topic = "/" + loaded_robot->getConfig()->getTemotoNamespace() + "/" + + srv_name::SERVER_CANCEL_NAVIGATION_GOAL; + TEMOTO_DEBUG_STREAM_("Forwarding the request to remote robot manager at '" << topic << "'."); + + ros::ServiceClient client_cancel_navigation_goal_ = nh_.serviceClient(topic); + RobotCancelNavigationGoal fwd_cancel_goal_srvc; + fwd_cancel_goal_srvc.request = req; + fwd_cancel_goal_srvc.response = res; + if (client_cancel_navigation_goal_.call(fwd_cancel_goal_srvc)) + { + res = fwd_cancel_goal_srvc.response; + } + else + { + throw TEMOTO_ERRSTACK("Call to remote RobotManager service failed."); + } + } + res.result = true; + return true; +} +catch(resource_registrar::TemotoErrorStack& e) +{ + res.result = false; + return true; +} + void RobotManager::resourceStatusCb(RobotLoad srv_msg, temoto_resource_registrar::Status status_msg) { TEMOTO_DEBUG_("status info was received"); diff --git a/srv/RobotCancelNavigationGoal.srv b/srv/RobotCancelNavigationGoal.srv new file mode 100644 index 0000000..8cb5a0c --- /dev/null +++ b/srv/RobotCancelNavigationGoal.srv @@ -0,0 +1,6 @@ +string robot_name + +--- + +bool result +string message From 9a6e98ea63f1ff2571bef08ff55fc7948d5b25c2 Mon Sep 17 00:00:00 2001 From: FabianEP11 Date: Fri, 15 Sep 2023 18:45:18 -0500 Subject: [PATCH 04/12] Working version --- .../navigation_plugin_base.h | 2 +- .../navigation_plugin_helper.h | 5 +- include/temoto_robot_manager/robot_features.h | 6 + .../robot_manager_interface.h | 11 +- src/navigation_plugin_helper.cpp | 34 +-- src/robot.cpp | 199 ++++++++---------- src/robot_features.cpp | 25 +-- src/robot_manager.cpp | 13 +- 8 files changed, 141 insertions(+), 154 deletions(-) diff --git a/include/temoto_robot_manager/navigation_plugin_base.h b/include/temoto_robot_manager/navigation_plugin_base.h index ed64019..64914ae 100644 --- a/include/temoto_robot_manager/navigation_plugin_base.h +++ b/include/temoto_robot_manager/navigation_plugin_base.h @@ -10,7 +10,7 @@ namespace temoto_robot_manager class NavigationPluginBase { public: - virtual bool initialize() = 0; + virtual bool initialize(const std::string& robot_ns) = 0; virtual bool sendGoal(RmNavigationGoal goal) = 0; virtual std::optional getFeedback() = 0; virtual bool cancelGoal() = 0; diff --git a/include/temoto_robot_manager/navigation_plugin_helper.h b/include/temoto_robot_manager/navigation_plugin_helper.h index 33af4ab..b60b6d4 100644 --- a/include/temoto_robot_manager/navigation_plugin_helper.h +++ b/include/temoto_robot_manager/navigation_plugin_helper.h @@ -42,22 +42,23 @@ class NavigationPluginHelper ERROR }; - NavigationPluginHelper(const std::string& plugin_path, NavigationFeatureUpdateCb update_cb); + NavigationPluginHelper(const std::string& plugin_path, const std::string& robot_ns, NavigationFeatureUpdateCb update_cb); ~NavigationPluginHelper(); void initialize(); void sendGoal(const RmNavigationRequestWrap& request); void sendUpdate() const; void cancelGoal(); void deinitialize(); + State getState() const; private: - State getState() const; void setState(State state); std::shared_ptr plugin; std::shared_ptr class_loader; std::thread exec_thread_; std::string plugin_path_; + std::string robot_ns_; State state_; mutable std::mutex mutex_state_; diff --git a/include/temoto_robot_manager/robot_features.h b/include/temoto_robot_manager/robot_features.h index c766f6f..b56d3e4 100644 --- a/include/temoto_robot_manager/robot_features.h +++ b/include/temoto_robot_manager/robot_features.h @@ -240,12 +240,18 @@ class FeatureNavigation : public FeatureWithDriver return pose_topic_; } + const std::string& getControllerInterface() const + { + return controller_interface_; + } + private: std::string global_planner_; std::string local_planner_; std::string odom_topic_; std::string cmd_vel_topic_; std::string pose_topic_; + std::string controller_interface_; }; class FeatureGripper : public FeatureWithDriver diff --git a/include/temoto_robot_manager/robot_manager_interface.h b/include/temoto_robot_manager/robot_manager_interface.h index f242478..1023acf 100644 --- a/include/temoto_robot_manager/robot_manager_interface.h +++ b/include/temoto_robot_manager/robot_manager_interface.h @@ -380,7 +380,7 @@ class RobotManagerInterface { throw TEMOTO_ERRSTACK("Unable to reach robot_manager"); } - + if (!msg.response.success) { throw TEMOTO_ERRSTACK("Unsuccessful attempt to invoke 'navigationGoal'"); @@ -389,6 +389,9 @@ class RobotManagerInterface bool navigationGoal(RobotNavigationGoal& goal) { + std::cout << "\033[1;35m [RMI] navigationGoal\033[0m\n" < End of cancelNavigationGoal"); return msg.response.result; } @@ -544,7 +547,6 @@ class RobotManagerInterface void navigationFeedbackCb(const NavigationFeedback& msg) { - TEMOTO_INFO_STREAM_("progress: " << msg.progress << " " << static_cast(msg.status)); std::lock_guard lock(custom_queries_mutex_); auto ongoing_nav_query_it = ongoing_navigation_queries_.find(msg.robot_name); @@ -552,7 +554,6 @@ class RobotManagerInterface { ongoing_nav_query_it->second = msg; } - return; } diff --git a/src/navigation_plugin_helper.cpp b/src/navigation_plugin_helper.cpp index befe89e..9842183 100644 --- a/src/navigation_plugin_helper.cpp +++ b/src/navigation_plugin_helper.cpp @@ -7,11 +7,12 @@ namespace temoto_robot_manager { -NavigationPluginHelper::NavigationPluginHelper(const std::string& plugin_path, NavigationFeatureUpdateCb update_cb) +NavigationPluginHelper::NavigationPluginHelper(const std::string& plugin_path, const std::string& robot_ns, NavigationFeatureUpdateCb update_cb) : plugin_path_(plugin_path) , state_(State::NOT_LOADED) , update_cb_(update_cb) , current_request_{} +, robot_ns_(robot_ns) { try { @@ -86,7 +87,7 @@ try setState(State::ERROR); throw TEMOTO_ERRSTACK("Cannot initalize the plugin. It has to be in 'UNINITIALIZED' state for that"); } - if (!plugin->initialize()) + if (!plugin->initialize(robot_ns_)) { setState(State::ERROR); plugin.reset(); @@ -137,20 +138,29 @@ void NavigationPluginHelper::sendGoal(const RmNavigationRequestWrap& request) void NavigationPluginHelper::cancelGoal() { - if (getState() != State::PROCESSING) + + if (getState() == State::FINISHED) { - setState(State::ERROR); - throw TEMOTO_ERRSTACK("Cannot cancel the goal. Plugin has to be in 'PROCESSING' state for that"); + std::cout << "\033[1;32m [Plugin helper] Nothing to Cancel. Goal finished already\033[0m\n" <cancelGoal()) + if (getState() == State::PROCESSING) { - setState(State::ERROR); - throw TEMOTO_ERRSTACK("Unable to cancel goal"); - } + // setState(State::ERROR); + std::cout << "\033[1;32m [Plugin helper] State = processing\033[0m\n" <cancelGoal()) + { + std::cout << "\033[1;32m [Plugin helper] !plugin->cancelGoal \033[0m\n" <progress; fbw.base_position = fb->base_position; - std::cout << "sendUpdate: progress -->" << fb->progress << std::endl; - std::cout << "status: -->" << fbw.status << std::endl; update_cb_(fbw); } } diff --git a/src/robot.cpp b/src/robot.cpp index 0da4658..0228176 100644 --- a/src/robot.cpp +++ b/src/robot.cpp @@ -404,74 +404,46 @@ void Robot::loadNavigationController() try { FeatureNavigation& ftr = config_->getFeatureNavigation(); - if (ftr.getExecutableType() == "ros") - { - // Previous Implementation - rosExecute(ftr.getPackageName(), ftr.getExecutable(), ftr.getArgs()); - // wait for command velocity to be published - std::string cmd_vel_topic = "/" + config_->getAbsRobotNamespace() + "/" + ftr.getCmdVelTopic(); - waitForTopic(cmd_vel_topic); + + rosExecute(ftr.getPackageName(), ftr.getExecutable(), ftr.getArgs()); + // wait for command velocity to be published + std::string cmd_vel_topic = "/" + config_->getAbsRobotNamespace() + "/" + ftr.getCmdVelTopic(); + waitForTopic(cmd_vel_topic); - // Subscribe to the pose messages - if (!ftr.getPoseTopic().empty()) - { - localized_pose_sub_ = nh_.subscribe("/" + config_->getAbsRobotNamespace() + "/" + ftr.getPoseTopic() - , 1 - , &Robot::robotPoseCallback - , this); - } + // Subscribe to the pose messages + if (!ftr.getPoseTopic().empty()) + { + localized_pose_sub_ = nh_.subscribe("/" + config_->getAbsRobotNamespace() + "/" + ftr.getPoseTopic() + , 1 + , &Robot::robotPoseCallback + , this); } - else if (ftr.getExecutableType() == "lib") + + + TEMOTO_INFO_("Load Navigation contoller lib"); + try { - TEMOTO_INFO_("Navigation contoller lib"); - try - { - const std::string& plugin_path = ftr.getExecutable(); - NavigationPluginHelperPtr plugin_helper = std::make_shared(plugin_path, navigation_feature_update_cb_); + const std::string& plugin_path = ftr.getControllerInterface(); + std::string act_rob_ns = "/" + config_->getAbsRobotNamespace(); - std::lock_guard l(navigation_feature_plugins_mutex_); - navigation_feature_plugin_ = plugin_helper; - // ftr.setLoaded(true); - - - // /* - // * Start the navigation feature feedback thread - // */ - // if (navigation_feature_feedback_thread_running_) - // { - // return; - // } - - // navigation_feature_feedback_thread_running_ = true; - // navigation_feature_feedback_thread_ = std::thread( - // [&] - // { - // TEMOTO_DEBUG_("Navigation feature feedback thread running"); - - // while (navigation_feature_feedback_thread_running_) - // { - // std::lock_guard l(navigation_feature_plugins_mutex_); - - // navigation_feature_plugin_->sendUpdate(); - // std::this_thread::sleep_for(std::chrono::milliseconds(200)); - // } - - // TEMOTO_DEBUG_("Navigation feature feedback thread finished"); - // }); - } - catch(resource_registrar::TemotoErrorStack& error_stack) - { - throw FWD_TEMOTO_ERRSTACK(error_stack); - } - catch(std::exception& e) - { - throw TEMOTO_ERRSTACK(e.what()); - } - catch(...) - { - throw TEMOTO_ERRSTACK("Could not load navigation feature"); - } + NavigationPluginHelperPtr plugin_helper = std::make_shared(plugin_path, act_rob_ns, navigation_feature_update_cb_); + + std::lock_guard l(navigation_feature_plugins_mutex_); + navigation_feature_plugin_ = plugin_helper; + } + catch(resource_registrar::TemotoErrorStack& error_stack) + { + throw FWD_TEMOTO_ERRSTACK(error_stack); + } + catch(std::exception& e) + { + throw TEMOTO_ERRSTACK(e.what()); + } + catch(...) + { + throw TEMOTO_ERRSTACK("Could not load navigation feature"); } + ros::Duration(5).sleep(); ftr.setLoaded(true); @@ -496,7 +468,6 @@ void Robot::loadNavigationDriver() FeatureNavigation& ftr = config_->getFeatureNavigation(); rosExecute(ftr.getDriverPackageName(), ftr.getDriverExecutable(), ftr.getDriverArgs()); std::string odom_topic = "/" + config_->getAbsRobotNamespace() + "/" + ftr.getOdomTopic(); - TEMOTO_INFO_(" ===== Loadng Navigation driver ===== Waitinf for topic ====="); TEMOTO_INFO_(odom_topic); waitForTopic(odom_topic); @@ -1024,12 +995,10 @@ std::vector Robot::getNamedTargetPoses(const std::string& planning_ void Robot::goalNavigation(const geometry_msgs::PoseStamped& target_pose) { - TEMOTO_INFO_("================= [robot.cpp 1026] goalNavigation =================="); if (!isRobotOperational()) { throw TEMOTO_ERRSTACK("Could not navigate the robot because robot is not operational"); } - TEMOTO_INFO_("================= [robot.cpp 1031] getFeatureNavigation =================="); FeatureNavigation& ftr = config_->getFeatureNavigation(); RmNavigationRequestWrap request; request.robot_name = config_->getName(); @@ -1037,78 +1006,86 @@ void Robot::goalNavigation(const geometry_msgs::PoseStamped& target_pose) request.goal_pose.pose.position.x = target_pose.pose.position.x; request.goal_pose.pose.position.y = target_pose.pose.position.y; request.goal_pose.pose.position.z = target_pose.pose.position.z; - TEMOTO_INFO_STREAM_("position z: " << request.goal_pose.pose.position.z); request.goal_pose.pose.orientation.x = target_pose.pose.orientation.x; request.goal_pose.pose.orientation.y = target_pose.pose.orientation.y; request.goal_pose.pose.orientation.z = target_pose.pose.orientation.z; request.goal_pose.pose.orientation.w = target_pose.pose.orientation.w; - TEMOTO_INFO_("================= Before send Goal =================="); - - - + std::cout << "\033[1;32m [R] goalNavigation\033[0m\n" <sendGoal(request); /* * Start the navigation feature feedback thread */ - if (navigation_feature_feedback_thread_running_) + if (!navigation_feature_feedback_thread_running_) { - return; - } + navigation_feature_feedback_thread_running_ = true; + navigation_feature_feedback_thread_ = std::thread( + [&] + { + TEMOTO_DEBUG_("Navigation feature feedback thread running"); - navigation_feature_feedback_thread_running_ = true; - navigation_feature_feedback_thread_ = std::thread( - [&] - { - TEMOTO_DEBUG_("Navigation feature feedback thread running"); + while (navigation_feature_feedback_thread_running_) + { + std::lock_guard l(navigation_feature_plugins_mutex_); - while (navigation_feature_feedback_thread_running_) - { - std::lock_guard l(navigation_feature_plugins_mutex_); + navigation_feature_plugin_->sendUpdate(); + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + } - navigation_feature_plugin_->sendUpdate(); - std::this_thread::sleep_for(std::chrono::milliseconds(200)); - } + TEMOTO_DEBUG_("Navigation feature feedback thread finished"); + }); + } - TEMOTO_DEBUG_("Navigation feature feedback thread finished"); - }); + // std::string act_rob_ns = "/" + config_->getAbsRobotNamespace() + "/move_base"; - // MoveBaseClient ac(act_rob_ns, true); - // if (!ac.waitForServer(ros::Duration(5.0))) - // { - // TEMOTO_ERRSTACK("The move_base action server did not come up"); - // } - // move_base_msgs::MoveBaseGoal goal; - // goal.target_pose = target_pose; - // goal.target_pose.header.stamp = ros::Time::now(); - // ac.sendGoal(goal); - // // Wait until either the goal is finished or robot has encountered a system issue - // while((ac.getState() == actionlib::SimpleClientGoalState::PENDING || ac.getState() == actionlib::SimpleClientGoalState::ACTIVE) - // && isRobotOperational()) - // { - // ros::Duration(1).sleep(); - // } - // if (!isRobotOperational()) - // { - // ac.cancelGoal(); - // throw TEMOTO_ERRSTACK("Could not finish the navigation goal because the robot is not operational"); - // } - // else if(ac.getState() != actionlib::SimpleClientGoalState::SUCCEEDED) - // { - // throw TEMOTO_ERRSTACK("The base failed to move"); - // } + ///////////////////////////////////////////////////////////////////// + + // Wait until either the goal is finished or robot has encountered a system issue + + // NOT_LOADED, + // UNINITIALIZED, + // INITIALIZED, + // PROCESSING, + // FINISHED, + // STOPPING, + // ERROR + + while((navigation_feature_plugin_->getState() == temoto_robot_manager::NavigationPluginHelper::State::PROCESSING) + && isRobotOperational()) + { + ros::Duration(1).sleep(); + } + if (!isRobotOperational()) + { + cancelNavigationGoal(); + throw TEMOTO_ERRSTACK("Could not finish the navigation goal because the robot is not operational"); + } + else if(navigation_feature_plugin_->getState() != temoto_robot_manager::NavigationPluginHelper::State::FINISHED) + { + throw TEMOTO_ERRSTACK("The base failed to move"); + } + // DO I NEED TO STOP THE FEEDBACK THREAD? + // navigation_feature_feedback_thread_running_ = false; + // while (!navigation_feature_feedback_thread_.joinable()) + // { + // std::this_thread::sleep_for(std::chrono::milliseconds(5)); + // } + // navigation_feature_feedback_thread_.join(); } void Robot::cancelNavigationGoal() try { + std::cout << "\033[1;32m [R] Cancel goalNavigation\033[0m\n" <cancelGoal(); } catch(resource_registrar::TemotoErrorStack& e) diff --git a/src/robot_features.cpp b/src/robot_features.cpp index 9e4d9fe..78a0bce 100644 --- a/src/robot_features.cpp +++ b/src/robot_features.cpp @@ -107,16 +107,9 @@ FeatureNavigation::FeatureNavigation(const YAML::Node& nav_conf) */ if (nav_conf["controller"].IsDefined()) { - setFromConfig(nav_conf["controller"]["executable"], this->executable_); - setFromConfig(nav_conf["controller"]["executable_type"], this->executable_type_); - - if (executable_type_ == "ros") - { - setFromConfig(nav_conf["controller"]["package_name"], this->package_name_); - } - - this->feature_enabled_ = true; - + this->feature_enabled_ = setFromConfig(nav_conf["controller"]["package_name"], this->package_name_) + && setFromConfig(nav_conf["controller"]["executable"], this->executable_) + && setFromConfig(nav_conf["controller"]["controller_interface"], this->controller_interface_); // Optional parameters if (this->feature_enabled_) { @@ -132,15 +125,8 @@ FeatureNavigation::FeatureNavigation(const YAML::Node& nav_conf) */ if (nav_conf["driver"].IsDefined()) { - setFromConfig(nav_conf["driver"]["executable"], this->driver_executable_); - setFromConfig(nav_conf["driver"]["executable_type"], this->driver_executable_type_); - - if (driver_executable_type_ == "ros") - { - setFromConfig(nav_conf["driver"]["package_name"], this->driver_package_name_); - } - - this->driver_enabled_ = true; + this->driver_enabled_ = setFromConfig(nav_conf["driver"]["package_name"], this->driver_package_name_) + && setFromConfig(nav_conf["driver"]["executable"], this->driver_executable_); // Optional parameters if (this->driver_enabled_) { @@ -148,7 +134,6 @@ FeatureNavigation::FeatureNavigation(const YAML::Node& nav_conf) setFromConfig(nav_conf["driver"]["odom_topic"], this->odom_topic_); setFromConfig(nav_conf["driver"]["cmd_vel_topic"], this->cmd_vel_topic_); } - } } diff --git a/src/robot_manager.cpp b/src/robot_manager.cpp index ef2e6c9..ccd9b4b 100644 --- a/src/robot_manager.cpp +++ b/src/robot_manager.cpp @@ -786,11 +786,18 @@ try { TEMOTO_INFO_("Received a goal Navigation request"); TEMOTO_DEBUG_STREAM_("Request:\n" << req); - std::lock_guard l(mutex_ongoing_navigation_requests_); + + // I have commented out this line because I need to cancel the goal. If Uncommented, cancel works after sendGoal() finishes its execution + // std::lock_guard l(mutex_ongoing_navigation_requests_); TEMOTO_INFO_STREAM_("Robot name: " << req.robot_name); RobotPtr loaded_robot = findLoadedRobot(req.robot_name); + + std::cout << "\033[1;32m [RM] goalNavigationCb\033[0m\n" <isLocal()) { TEMOTO_INFO_STREAM_(" Loaded Robot, it is local "); @@ -832,7 +839,9 @@ try { TEMOTO_INFO_("Cancel Navigation goal request"); TEMOTO_DEBUG_STREAM_("Request:\n" << req); - std::lock_guard l(mutex_ongoing_navigation_requests_); + + // I have commented out this line because I need to cancel the goal. If Uncommented, cancel works after sendGoal() finishes its execution + // std::lock_guard l(mutex_ongoing_navigation_requests_); TEMOTO_INFO_STREAM_("Robot name: " << req.robot_name); RobotPtr loaded_robot = findLoadedRobot(req.robot_name); From 8a3065a7b4a4fa48da3c7a3ae9113dc098d133be Mon Sep 17 00:00:00 2001 From: FabianEP11 Date: Thu, 21 Sep 2023 16:26:04 -0500 Subject: [PATCH 05/12] Clean up code --- .../navigation_plugin_helper.h | 6 +--- .../robot_manager_interface.h | 7 ++--- src/navigation_plugin_helper.cpp | 31 +++++++++---------- src/robot.cpp | 23 -------------- src/robot_manager.cpp | 13 +------- 5 files changed, 18 insertions(+), 62 deletions(-) diff --git a/include/temoto_robot_manager/navigation_plugin_helper.h b/include/temoto_robot_manager/navigation_plugin_helper.h index b60b6d4..8c084ac 100644 --- a/include/temoto_robot_manager/navigation_plugin_helper.h +++ b/include/temoto_robot_manager/navigation_plugin_helper.h @@ -12,18 +12,14 @@ namespace temoto_robot_manager struct RmNavigationFeedbackWrap : RmNavigationFeedback { std::string robot_name; - // std::string navigation_feature_name; - // std::string request_id; }; struct RmNavigationRequestWrap : RmNavigationGoal { std::string robot_name; - // std::string navigation_feature_name; - // std::string request_id; }; -class NavigationPluginHelper; // Forward declaration +class NavigationPluginHelper; typedef std::shared_ptr NavigationPluginHelperPtr; typedef std::function NavigationFeatureUpdateCb; diff --git a/include/temoto_robot_manager/robot_manager_interface.h b/include/temoto_robot_manager/robot_manager_interface.h index 1023acf..9fb9d82 100644 --- a/include/temoto_robot_manager/robot_manager_interface.h +++ b/include/temoto_robot_manager/robot_manager_interface.h @@ -363,6 +363,7 @@ class RobotManagerInterface return msg.response.named_target_poses; } + // TODO: Erase? void navigationGoal(const std::string& robot_name , const geometry_msgs::PoseStamped& pose) { @@ -389,9 +390,6 @@ class RobotManagerInterface bool navigationGoal(RobotNavigationGoal& goal) { - std::cout << "\033[1;35m [RMI] navigationGoal\033[0m\n" < End of cancelNavigationGoal"); return msg.response.result; } diff --git a/src/navigation_plugin_helper.cpp b/src/navigation_plugin_helper.cpp index 9842183..7abf154 100644 --- a/src/navigation_plugin_helper.cpp +++ b/src/navigation_plugin_helper.cpp @@ -141,26 +141,25 @@ void NavigationPluginHelper::cancelGoal() if (getState() == State::FINISHED) { - std::cout << "\033[1;32m [Plugin helper] Nothing to Cancel. Goal finished already\033[0m\n" <cancelGoal()) - { - std::cout << "\033[1;32m [Plugin helper] !plugin->cancelGoal \033[0m\n" <cancelGoal()) + { + setState(State::ERROR); + throw TEMOTO_ERRSTACK("Unable to cancel goal"); + } - std::cout << "\033[1;32m [Plugin helper] end cancelgoal \033[0m\n" <robot_name; - // fbw.navigation_feature_name = current_request_->navigation_feature_name; - // fbw.request_id = current_request_->request_id; fbw.status = uint8_t(state_); fbw.progress = fb->progress; fbw.base_position = fb->base_position; diff --git a/src/robot.cpp b/src/robot.cpp index 0228176..68a6823 100644 --- a/src/robot.cpp +++ b/src/robot.cpp @@ -1010,9 +1010,6 @@ void Robot::goalNavigation(const geometry_msgs::PoseStamped& target_pose) request.goal_pose.pose.orientation.y = target_pose.pose.orientation.y; request.goal_pose.pose.orientation.z = target_pose.pose.orientation.z; request.goal_pose.pose.orientation.w = target_pose.pose.orientation.w; - std::cout << "\033[1;32m [R] goalNavigation\033[0m\n" <sendGoal(request); /* @@ -1038,25 +1035,6 @@ void Robot::goalNavigation(const geometry_msgs::PoseStamped& target_pose) }); } - - - // std::string act_rob_ns = "/" + config_->getAbsRobotNamespace() + "/move_base"; - - - - - ///////////////////////////////////////////////////////////////////// - - // Wait until either the goal is finished or robot has encountered a system issue - - // NOT_LOADED, - // UNINITIALIZED, - // INITIALIZED, - // PROCESSING, - // FINISHED, - // STOPPING, - // ERROR - while((navigation_feature_plugin_->getState() == temoto_robot_manager::NavigationPluginHelper::State::PROCESSING) && isRobotOperational()) { @@ -1085,7 +1063,6 @@ void Robot::goalNavigation(const geometry_msgs::PoseStamped& target_pose) void Robot::cancelNavigationGoal() try { - std::cout << "\033[1;32m [R] Cancel goalNavigation\033[0m\n" <cancelGoal(); } catch(resource_registrar::TemotoErrorStack& e) diff --git a/src/robot_manager.cpp b/src/robot_manager.cpp index ccd9b4b..8131ccf 100644 --- a/src/robot_manager.cpp +++ b/src/robot_manager.cpp @@ -787,27 +787,19 @@ try TEMOTO_INFO_("Received a goal Navigation request"); TEMOTO_DEBUG_STREAM_("Request:\n" << req); - // I have commented out this line because I need to cancel the goal. If Uncommented, cancel works after sendGoal() finishes its execution + // I have commented out this line because I need to cancel the goal. If Uncommented, cancel cmd is triggered after sendGoal() finishes its execution // std::lock_guard l(mutex_ongoing_navigation_requests_); - - TEMOTO_INFO_STREAM_("Robot name: " << req.robot_name); RobotPtr loaded_robot = findLoadedRobot(req.robot_name); - std::cout << "\033[1;32m [RM] goalNavigationCb\033[0m\n" <isLocal()) { - TEMOTO_INFO_STREAM_(" Loaded Robot, it is local "); TEMOTO_DEBUG_STREAM_("Navigating '" << req.robot_name << " to pose: " << req.target_pose << " ..."); loaded_robot->goalNavigation(req.target_pose); // The robot would move with respect to the coordinate frame defined in the header res.success = true; } else { - TEMOTO_INFO_STREAM_(" Loaded Robot, it is remote "); std::string topic = "/" + loaded_robot->getConfig()->getTemotoNamespace() + "/" + srv_name::SERVER_NAVIGATION_GOAL; TEMOTO_DEBUG_STREAM_("Forwarding the request to remote robot manager at '" << topic << "'."); @@ -843,17 +835,14 @@ try // I have commented out this line because I need to cancel the goal. If Uncommented, cancel works after sendGoal() finishes its execution // std::lock_guard l(mutex_ongoing_navigation_requests_); - TEMOTO_INFO_STREAM_("Robot name: " << req.robot_name); RobotPtr loaded_robot = findLoadedRobot(req.robot_name); if (loaded_robot->isLocal()) { - TEMOTO_INFO_STREAM_(" Loaded Robot, it is local "); loaded_robot->cancelNavigationGoal(); res.result = true; } else { - TEMOTO_INFO_STREAM_(" Loaded Robot, it is remote "); std::string topic = "/" + loaded_robot->getConfig()->getTemotoNamespace() + "/" + srv_name::SERVER_CANCEL_NAVIGATION_GOAL; TEMOTO_DEBUG_STREAM_("Forwarding the request to remote robot manager at '" << topic << "'."); From dc04753de254ae47770e54dec8c555187d95a21a Mon Sep 17 00:00:00 2001 From: FabianEP11 Date: Tue, 10 Oct 2023 23:29:35 -0500 Subject: [PATCH 06/12] wait for finish state on RMI --- include/temoto_robot_manager/robot.h | 2 +- .../temoto_robot_manager/robot_manager_interface.h | 10 ++++++++++ src/robot.cpp | 14 +++++++------- src/robot_manager.cpp | 4 ++-- 4 files changed, 20 insertions(+), 10 deletions(-) diff --git a/include/temoto_robot_manager/robot.h b/include/temoto_robot_manager/robot.h index 498cedd..1c585fb 100644 --- a/include/temoto_robot_manager/robot.h +++ b/include/temoto_robot_manager/robot.h @@ -143,7 +143,7 @@ class Robot geometry_msgs::PoseWithCovarianceStamped current_pose_navigation_; NavigationPluginHelperPtr navigation_feature_plugin_; - mutable std::mutex navigation_feature_plugins_mutex_; + mutable std::mutex navigation_feature_plugin_mutex_; NavigationFeatureUpdateCb navigation_feature_update_cb_; std::thread navigation_feature_feedback_thread_; bool navigation_feature_feedback_thread_running_; diff --git a/include/temoto_robot_manager/robot_manager_interface.h b/include/temoto_robot_manager/robot_manager_interface.h index 9fb9d82..c07c943 100644 --- a/include/temoto_robot_manager/robot_manager_interface.h +++ b/include/temoto_robot_manager/robot_manager_interface.h @@ -404,6 +404,16 @@ class RobotManagerInterface { // throw TEMOTO_ERRSTACK("Unsuccessful attempt to invoke 'navigationGoal'"); } + + // wait + std::cout << getNavigationFeedback(goal.request.robot_name)->status << std::endl; + + while (getNavigationFeedback(goal.request.robot_name)->status != NavigationFeedback::FINISHED + || getNavigationFeedback(goal.request.robot_name)->status != NavigationFeedback::CANCELLED) + { + ros::Duration(1).sleep(); + } + return goal.response.success; } diff --git a/src/robot.cpp b/src/robot.cpp index 68a6823..fbdd40d 100644 --- a/src/robot.cpp +++ b/src/robot.cpp @@ -428,7 +428,7 @@ void Robot::loadNavigationController() NavigationPluginHelperPtr plugin_helper = std::make_shared(plugin_path, act_rob_ns, navigation_feature_update_cb_); - std::lock_guard l(navigation_feature_plugins_mutex_); + std::lock_guard l(navigation_feature_plugin_mutex_); navigation_feature_plugin_ = plugin_helper; } catch(resource_registrar::TemotoErrorStack& error_stack) @@ -1025,7 +1025,7 @@ void Robot::goalNavigation(const geometry_msgs::PoseStamped& target_pose) while (navigation_feature_feedback_thread_running_) { - std::lock_guard l(navigation_feature_plugins_mutex_); + std::lock_guard l(navigation_feature_plugin_mutex_); navigation_feature_plugin_->sendUpdate(); std::this_thread::sleep_for(std::chrono::milliseconds(200)); @@ -1035,11 +1035,11 @@ void Robot::goalNavigation(const geometry_msgs::PoseStamped& target_pose) }); } - while((navigation_feature_plugin_->getState() == temoto_robot_manager::NavigationPluginHelper::State::PROCESSING) - && isRobotOperational()) - { - ros::Duration(1).sleep(); - } + // while((navigation_feature_plugin_->getState() == temoto_robot_manager::NavigationPluginHelper::State::PROCESSING) + // && isRobotOperational()) + // { + // ros::Duration(1).sleep(); + // } if (!isRobotOperational()) { diff --git a/src/robot_manager.cpp b/src/robot_manager.cpp index 8131ccf..4189e11 100644 --- a/src/robot_manager.cpp +++ b/src/robot_manager.cpp @@ -788,7 +788,7 @@ try TEMOTO_DEBUG_STREAM_("Request:\n" << req); // I have commented out this line because I need to cancel the goal. If Uncommented, cancel cmd is triggered after sendGoal() finishes its execution - // std::lock_guard l(mutex_ongoing_navigation_requests_); + std::lock_guard l(mutex_ongoing_navigation_requests_); RobotPtr loaded_robot = findLoadedRobot(req.robot_name); @@ -833,7 +833,7 @@ try TEMOTO_DEBUG_STREAM_("Request:\n" << req); // I have commented out this line because I need to cancel the goal. If Uncommented, cancel works after sendGoal() finishes its execution - // std::lock_guard l(mutex_ongoing_navigation_requests_); + std::lock_guard l(mutex_ongoing_navigation_requests_); RobotPtr loaded_robot = findLoadedRobot(req.robot_name); if (loaded_robot->isLocal()) From c266da1c5fbf195f970377b648be27b5318937e5 Mon Sep 17 00:00:00 2001 From: FabianEP11 Date: Thu, 12 Oct 2023 09:46:09 -0500 Subject: [PATCH 07/12] Fixed RMI - wait for feedback --- .../robot_manager_interface.h | 42 +++++++++++-------- src/robot.cpp | 6 --- 2 files changed, 25 insertions(+), 23 deletions(-) diff --git a/include/temoto_robot_manager/robot_manager_interface.h b/include/temoto_robot_manager/robot_manager_interface.h index c07c943..47094f1 100644 --- a/include/temoto_robot_manager/robot_manager_interface.h +++ b/include/temoto_robot_manager/robot_manager_interface.h @@ -405,14 +405,25 @@ class RobotManagerInterface // throw TEMOTO_ERRSTACK("Unsuccessful attempt to invoke 'navigationGoal'"); } - // wait - std::cout << getNavigationFeedback(goal.request.robot_name)->status << std::endl; - + ongoing_navigation_queries_.insert({goal.request.robot_name, NavigationQuery(goal)}); + + // wait to finish execution while (getNavigationFeedback(goal.request.robot_name)->status != NavigationFeedback::FINISHED - || getNavigationFeedback(goal.request.robot_name)->status != NavigationFeedback::CANCELLED) + && getNavigationFeedback(goal.request.robot_name)->status != NavigationFeedback::CANCELLED) { ros::Duration(1).sleep(); } + + auto ongoing_query_it = ongoing_navigation_queries_.find(goal.request.robot_name); + if (ongoing_query_it != ongoing_navigation_queries_.end()) + { + ongoing_navigation_queries_.erase(ongoing_query_it); + } + + if (getNavigationFeedback(goal.request.robot_name)->status == NavigationFeedback::CANCELLED) + { + return false; + } return goal.response.success; } @@ -428,16 +439,7 @@ class RobotManagerInterface //throw TEMOTO_ERRSTACK("Could not find the request in the list of ongoing requests"); } - if (ongoing_query_it->second.status == NavigationFeedback::FINISHED) - { - auto feedback = ongoing_query_it->second; - ongoing_navigation_queries_.erase(ongoing_query_it); - return feedback; - } - else - { - return ongoing_query_it->second; - } + return ongoing_query_it->second.feedback; } bool cancelNavigationGoal(const std::string& robot_name) @@ -541,6 +543,13 @@ class RobotManagerInterface CustomFeedback feedback; }; + struct NavigationQuery + { + NavigationQuery(const RobotNavigationGoal& nr) : request{nr}{} + RobotNavigationGoal request; + NavigationFeedback feedback; + }; + void customFeedback(const CustomFeedback& msg) { std::lock_guard lock(custom_queries_mutex_); @@ -556,10 +565,9 @@ class RobotManagerInterface { std::lock_guard lock(custom_queries_mutex_); auto ongoing_nav_query_it = ongoing_navigation_queries_.find(msg.robot_name); - if (ongoing_nav_query_it != ongoing_navigation_queries_.end()) { - ongoing_nav_query_it->second = msg; + ongoing_nav_query_it->second.feedback = msg; } return; } @@ -590,7 +598,7 @@ class RobotManagerInterface ros::Subscriber navigation_feedback_; std::mutex navigation_queries_mutex_; - std::map ongoing_navigation_queries_; + std::map ongoing_navigation_queries_; std::unique_ptr resource_registrar_; }; diff --git a/src/robot.cpp b/src/robot.cpp index fbdd40d..9e379c6 100644 --- a/src/robot.cpp +++ b/src/robot.cpp @@ -1035,12 +1035,6 @@ void Robot::goalNavigation(const geometry_msgs::PoseStamped& target_pose) }); } - // while((navigation_feature_plugin_->getState() == temoto_robot_manager::NavigationPluginHelper::State::PROCESSING) - // && isRobotOperational()) - // { - // ros::Duration(1).sleep(); - // } - if (!isRobotOperational()) { cancelNavigationGoal(); From 8ce3b82c95bbc6fd0579c275a6854411137a4566 Mon Sep 17 00:00:00 2001 From: FabianEP11 Date: Wed, 25 Oct 2023 18:25:46 -0500 Subject: [PATCH 08/12] changes on nav requests & Add priority --- include/temoto_robot_manager/robot_manager.h | 2 +- .../robot_manager_interface.h | 14 +++- src/navigation_plugin_helper.cpp | 1 + src/robot.cpp | 10 --- src/robot_manager.cpp | 72 +++++++++++++++++-- srv/RobotCancelNavigationGoal.srv | 1 + srv/RobotNavigationGoal.srv | 1 + 7 files changed, 83 insertions(+), 18 deletions(-) diff --git a/include/temoto_robot_manager/robot_manager.h b/include/temoto_robot_manager/robot_manager.h index 60b432e..6fc0e7f 100644 --- a/include/temoto_robot_manager/robot_manager.h +++ b/include/temoto_robot_manager/robot_manager.h @@ -160,7 +160,7 @@ class RobotManager : public temoto_core::BaseSubsystem ros::ServiceClient client_gripper_control_position_; ros::ServiceClient client_cancel_navigation_goal_; - RobotNavigationGoal ongoing_navigation_requests_; + std::map ongoing_navigation_requests_; std::mutex mutex_ongoing_navigation_requests_; ros::Publisher pub_navigation_feature_feedback_; diff --git a/include/temoto_robot_manager/robot_manager_interface.h b/include/temoto_robot_manager/robot_manager_interface.h index 47094f1..838a7af 100644 --- a/include/temoto_robot_manager/robot_manager_interface.h +++ b/include/temoto_robot_manager/robot_manager_interface.h @@ -402,7 +402,7 @@ class RobotManagerInterface if (!goal.response.success) { - // throw TEMOTO_ERRSTACK("Unsuccessful attempt to invoke 'navigationGoal'"); + TEMOTO_INFO_("Goal response not success"); } ongoing_navigation_queries_.insert({goal.request.robot_name, NavigationQuery(goal)}); @@ -424,7 +424,6 @@ class RobotManagerInterface { return false; } - return goal.response.success; } @@ -435,11 +434,20 @@ class RobotManagerInterface if (ongoing_query_it == ongoing_navigation_queries_.end()) { + TEMOTO_INFO_STREAM_("There's no ongoing query"); return {}; //throw TEMOTO_ERRSTACK("Could not find the request in the list of ongoing requests"); } - return ongoing_query_it->second.feedback; + if (ongoing_query_it->second.feedback.status == NavigationFeedback::FINISHED) + { + auto feedback = ongoing_query_it->second.feedback; + return feedback; + } + else + { + return ongoing_query_it->second.feedback; + } } bool cancelNavigationGoal(const std::string& robot_name) diff --git a/src/navigation_plugin_helper.cpp b/src/navigation_plugin_helper.cpp index 7abf154..054e57c 100644 --- a/src/navigation_plugin_helper.cpp +++ b/src/navigation_plugin_helper.cpp @@ -142,6 +142,7 @@ void NavigationPluginHelper::cancelGoal() if (getState() == State::FINISHED) { std::cout << "\033[1;32m [Plugin helper] There is no goal to cancel.\033[0m\n" <getState() != temoto_robot_manager::NavigationPluginHelper::State::FINISHED) - { - throw TEMOTO_ERRSTACK("The base failed to move"); - } - // DO I NEED TO STOP THE FEEDBACK THREAD? // navigation_feature_feedback_thread_running_ = false; // while (!navigation_feature_feedback_thread_.joinable()) diff --git a/src/robot_manager.cpp b/src/robot_manager.cpp index 4189e11..08ef11e 100644 --- a/src/robot_manager.cpp +++ b/src/robot_manager.cpp @@ -786,16 +786,56 @@ try { TEMOTO_INFO_("Received a goal Navigation request"); TEMOTO_DEBUG_STREAM_("Request:\n" << req); - - // I have commented out this line because I need to cancel the goal. If Uncommented, cancel cmd is triggered after sendGoal() finishes its execution std::lock_guard l(mutex_ongoing_navigation_requests_); + auto navigation_request_it = std::find_if( + ongoing_navigation_requests_.begin() + , ongoing_navigation_requests_.end() + , [&](const auto& ongoing_req) + { + return req == ongoing_req.second.request; + // return req.robot_name == ongoing_req.second.request.robot_name; + }); + + /* + * Check if that request is already processed by a client with higher priority + */ + if (navigation_request_it != ongoing_navigation_requests_.end() && + navigation_request_it->second.request.priority > req.priority) + { + TEMOTO_WARN_STREAM_("Request declined as it is already in process by client with higher priority" << std::endl); + return true; + } RobotPtr loaded_robot = findLoadedRobot(req.robot_name); + /* + * Pre-empt the lower priority request + */ + if (navigation_request_it != ongoing_navigation_requests_.end()) + { + TEMOTO_INFO_("This request is already in process under a lower priority, preempting."); + loaded_robot->cancelNavigationGoal(); + ongoing_navigation_requests_.erase(navigation_request_it); + } + if (loaded_robot->isLocal()) { TEMOTO_DEBUG_STREAM_("Navigating '" << req.robot_name << " to pose: " << req.target_pose << " ..."); loaded_robot->goalNavigation(req.target_pose); // The robot would move with respect to the coordinate frame defined in the header + + // ongoing_navigation_requests_.insert({req.robot_name + // , [&] + // { + // RobotNavigationGoal goal; + // goal.request = req; + // goal.response = res; + // return goal; + // }()}); + RobotNavigationGoal goal; + goal.request = req; + goal.response = res; + ongoing_navigation_requests_.insert({req.robot_name, goal}); + res.success = true; } else @@ -818,7 +858,7 @@ try } } res.success = true; - return true; + return true; } catch(resource_registrar::TemotoErrorStack& e) { @@ -832,13 +872,37 @@ try TEMOTO_INFO_("Cancel Navigation goal request"); TEMOTO_DEBUG_STREAM_("Request:\n" << req); - // I have commented out this line because I need to cancel the goal. If Uncommented, cancel works after sendGoal() finishes its execution std::lock_guard l(mutex_ongoing_navigation_requests_); + auto navigation_request_it = std::find_if( + ongoing_navigation_requests_.begin() + , ongoing_navigation_requests_.end() + , [&](const auto& ongoing_req) + { + return req.robot_name == ongoing_req.second.request.robot_name; + }); + + /* + * DECLINE: If priority is low + */ + if (req.priority <= navigation_request_it->second.request.priority) + { + TEMOTO_WARN_STREAM_("Cancel goal request declined: Priority lower than required" << std::endl); + return true; + } + + /* + * ACCEPT: If the priority is higher + */ RobotPtr loaded_robot = findLoadedRobot(req.robot_name); if (loaded_robot->isLocal()) { loaded_robot->cancelNavigationGoal(); + if (navigation_request_it != ongoing_navigation_requests_.end()) + { + ongoing_navigation_requests_.erase(navigation_request_it); + } + res.result = true; } else diff --git a/srv/RobotCancelNavigationGoal.srv b/srv/RobotCancelNavigationGoal.srv index 8cb5a0c..96d1bb1 100644 --- a/srv/RobotCancelNavigationGoal.srv +++ b/srv/RobotCancelNavigationGoal.srv @@ -1,4 +1,5 @@ string robot_name +uint8 priority --- diff --git a/srv/RobotNavigationGoal.srv b/srv/RobotNavigationGoal.srv index 5f77352..1bb77bf 100644 --- a/srv/RobotNavigationGoal.srv +++ b/srv/RobotNavigationGoal.srv @@ -1,5 +1,6 @@ string robot_name geometry_msgs/PoseStamped target_pose +uint8 priority --- From b869e6abf44d5913bd5e08203608dbee7aa239e3 Mon Sep 17 00:00:00 2001 From: FabianEP11 Date: Mon, 22 Jan 2024 12:09:08 -0600 Subject: [PATCH 09/12] Fixed issue passing data to the lambda function --- .../navigation_plugin_helper.h | 1 + src/navigation_plugin_helper.cpp | 31 ++++++++++--------- src/robot.cpp | 10 +----- src/robot_manager.cpp | 23 +++++++------- srv/RobotNavigationGoal.srv | 2 +- 5 files changed, 30 insertions(+), 37 deletions(-) diff --git a/include/temoto_robot_manager/navigation_plugin_helper.h b/include/temoto_robot_manager/navigation_plugin_helper.h index 8c084ac..c274ac5 100644 --- a/include/temoto_robot_manager/navigation_plugin_helper.h +++ b/include/temoto_robot_manager/navigation_plugin_helper.h @@ -55,6 +55,7 @@ class NavigationPluginHelper std::thread exec_thread_; std::string plugin_path_; std::string robot_ns_; + bool is_thread_running_ = false; State state_; mutable std::mutex mutex_state_; diff --git a/src/navigation_plugin_helper.cpp b/src/navigation_plugin_helper.cpp index 054e57c..bce0bb4 100644 --- a/src/navigation_plugin_helper.cpp +++ b/src/navigation_plugin_helper.cpp @@ -113,24 +113,25 @@ void NavigationPluginHelper::sendGoal(const RmNavigationRequestWrap& request) if (exec_thread_.joinable()) { exec_thread_.join(); + is_thread_running_ = false; } - - current_request_ = request; - exec_thread_ = std::thread( - [&] + if(!is_thread_running_) { - if (plugin->sendGoal(request)) - { - setState(State::FINISHED); - } - else + current_request_ = request; + exec_thread_ = std::thread([this, request] { - setState(State::ERROR); - //throw TEMOTO_ERRSTACK("Unable to invoke the plugin"); - } - - sendUpdate(); - }); + if (this->plugin->sendGoal(request)) + { + setState(State::FINISHED); + } + else + { + setState(State::ERROR); + } + sendUpdate(); + }); + is_thread_running_ = true; + } setState(State::PROCESSING); sendUpdate(); diff --git a/src/robot.cpp b/src/robot.cpp index 71e2518..652395e 100644 --- a/src/robot.cpp +++ b/src/robot.cpp @@ -1033,15 +1033,7 @@ void Robot::goalNavigation(const geometry_msgs::PoseStamped& target_pose) TEMOTO_DEBUG_("Navigation feature feedback thread finished"); }); - } - - // DO I NEED TO STOP THE FEEDBACK THREAD? - // navigation_feature_feedback_thread_running_ = false; - // while (!navigation_feature_feedback_thread_.joinable()) - // { - // std::this_thread::sleep_for(std::chrono::milliseconds(5)); - // } - // navigation_feature_feedback_thread_.join(); + } } void Robot::cancelNavigationGoal() diff --git a/src/robot_manager.cpp b/src/robot_manager.cpp index 08ef11e..a961754 100644 --- a/src/robot_manager.cpp +++ b/src/robot_manager.cpp @@ -167,7 +167,7 @@ try RobotPtr loaded_robot = findLoadedRobot(req.robot_name); /* - * Pre-empt the lower priority request + *Pre-empt the lower priority request */ if (custom_request_it != ongoing_custom_requests_.end()) { @@ -823,20 +823,19 @@ try TEMOTO_DEBUG_STREAM_("Navigating '" << req.robot_name << " to pose: " << req.target_pose << " ..."); loaded_robot->goalNavigation(req.target_pose); // The robot would move with respect to the coordinate frame defined in the header - // ongoing_navigation_requests_.insert({req.robot_name - // , [&] - // { - // RobotNavigationGoal goal; - // goal.request = req; - // goal.response = res; - // return goal; - // }()}); RobotNavigationGoal goal; goal.request = req; goal.response = res; - ongoing_navigation_requests_.insert({req.robot_name, goal}); - - res.success = true; + + auto it = ongoing_navigation_requests_.find(req.robot_name); + if (it != ongoing_navigation_requests_.end()) + { + it->second = goal; + } + else + { + ongoing_navigation_requests_.insert(std::make_pair(req.robot_name, goal)); + } } else { diff --git a/srv/RobotNavigationGoal.srv b/srv/RobotNavigationGoal.srv index 1bb77bf..3ba4320 100644 --- a/srv/RobotNavigationGoal.srv +++ b/srv/RobotNavigationGoal.srv @@ -4,4 +4,4 @@ uint8 priority --- -bool success +bool success \ No newline at end of file From 9f557a428c76649496e9696a65c5f261c7514db5 Mon Sep 17 00:00:00 2001 From: FabianEP11 Date: Wed, 24 Jan 2024 13:23:42 -0600 Subject: [PATCH 10/12] Use RobotCancelNavigationGoal instead just robot name on RMI --- .../robot_manager_interface.h | 27 +++++-------------- src/robot_manager.cpp | 2 -- srv/RobotCancelNavigationGoal.srv | 1 - 3 files changed, 6 insertions(+), 24 deletions(-) diff --git a/include/temoto_robot_manager/robot_manager_interface.h b/include/temoto_robot_manager/robot_manager_interface.h index 838a7af..fee8c83 100644 --- a/include/temoto_robot_manager/robot_manager_interface.h +++ b/include/temoto_robot_manager/robot_manager_interface.h @@ -159,7 +159,7 @@ class RobotManagerInterface } return preempt_srv_msg.response.accepted; - } + } YAML::Node getRobotConfig(const std::string& robot_name) try @@ -434,37 +434,22 @@ class RobotManagerInterface if (ongoing_query_it == ongoing_navigation_queries_.end()) { - TEMOTO_INFO_STREAM_("There's no ongoing query"); + TEMOTO_INFO_STREAM_("There's no ongoing query for the " << robot_name << " robot"); return {}; //throw TEMOTO_ERRSTACK("Could not find the request in the list of ongoing requests"); } - if (ongoing_query_it->second.feedback.status == NavigationFeedback::FINISHED) - { - auto feedback = ongoing_query_it->second.feedback; - return feedback; - } - else - { - return ongoing_query_it->second.feedback; - } + return ongoing_query_it->second.feedback; } - bool cancelNavigationGoal(const std::string& robot_name) + bool cancelNavigationGoal(RobotCancelNavigationGoal& cancel_goal) { - temoto_robot_manager::RobotCancelNavigationGoal msg; - msg.request.robot_name = robot_name; - - if (!client_cancel_navigation_goal_.call(msg)) + if (!client_cancel_navigation_goal_.call(cancel_goal)) { throw TEMOTO_ERRSTACK("Unable to reach the CancelNavigationGoal server"); } - if (!msg.response.result) - { - throw TEMOTO_ERRSTACK("Unsuccessful attempt to invoke 'CancelNavigationGoal'"); - } - return msg.response.result; + return cancel_goal.response.result; } void controlGripperPosition(const std::string& robot_name, const float& position) diff --git a/src/robot_manager.cpp b/src/robot_manager.cpp index a961754..6c4ec0d 100644 --- a/src/robot_manager.cpp +++ b/src/robot_manager.cpp @@ -901,8 +901,6 @@ try { ongoing_navigation_requests_.erase(navigation_request_it); } - - res.result = true; } else { diff --git a/srv/RobotCancelNavigationGoal.srv b/srv/RobotCancelNavigationGoal.srv index 96d1bb1..a44c5d2 100644 --- a/srv/RobotCancelNavigationGoal.srv +++ b/srv/RobotCancelNavigationGoal.srv @@ -4,4 +4,3 @@ uint8 priority --- bool result -string message From 20e3a25b045df28d4cb372b88b94da7976d8eb49 Mon Sep 17 00:00:00 2001 From: FabianEP11 Date: Tue, 13 Feb 2024 13:57:14 -0600 Subject: [PATCH 11/12] Included request_id / priority on navigation and cancel goal requests --- CMakeLists.txt | 4 +- .../navigation_plugin_helper.h | 2 + include/temoto_robot_manager/robot.h | 2 +- include/temoto_robot_manager/robot_manager.h | 6 +- .../robot_manager_interface.h | 48 ++++++++----- .../robot_manager_services.h | 4 +- msg/NavigationFeedback.msg | 2 +- src/navigation_plugin_helper.cpp | 1 + src/robot.cpp | 12 +--- src/robot_manager.cpp | 72 ++++++++++++++----- ...ationGoal.srv => CancelNavigationGoal.srv} | 2 + ...tNavigationGoal.srv => NavigationGoal.srv} | 3 + 12 files changed, 104 insertions(+), 54 deletions(-) rename srv/{RobotCancelNavigationGoal.srv => CancelNavigationGoal.srv} (61%) rename srv/{RobotNavigationGoal.srv => NavigationGoal.srv} (62%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 7b30d64..d4f6b97 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -49,9 +49,9 @@ add_service_files(FILES RobotGetTarget.srv RobotGetNamedTargets.srv RobotGetConfig.srv - RobotNavigationGoal.srv + NavigationGoal.srv RobotGripperControlPosition.srv - RobotCancelNavigationGoal.srv + CancelNavigationGoal.srv ) generate_messages(DEPENDENCIES diff --git a/include/temoto_robot_manager/navigation_plugin_helper.h b/include/temoto_robot_manager/navigation_plugin_helper.h index c274ac5..6211669 100644 --- a/include/temoto_robot_manager/navigation_plugin_helper.h +++ b/include/temoto_robot_manager/navigation_plugin_helper.h @@ -12,11 +12,13 @@ namespace temoto_robot_manager struct RmNavigationFeedbackWrap : RmNavigationFeedback { std::string robot_name; + std::string request_id; }; struct RmNavigationRequestWrap : RmNavigationGoal { std::string robot_name; + std::string request_id; }; class NavigationPluginHelper; diff --git a/include/temoto_robot_manager/robot.h b/include/temoto_robot_manager/robot.h index 1c585fb..4530c03 100644 --- a/include/temoto_robot_manager/robot.h +++ b/include/temoto_robot_manager/robot.h @@ -62,7 +62,7 @@ class Robot std::vector getCurrentJointValues(const std::string& planning_group_name); std::vector getNamedTargetPoses(const std::string& planning_group_name); - void goalNavigation(const geometry_msgs::PoseStamped& target_pose); + void goalNavigation(const RmNavigationRequestWrap& request); void cancelNavigationGoal(); void controlGripper(const std::string& robot_name, const float position); diff --git a/include/temoto_robot_manager/robot_manager.h b/include/temoto_robot_manager/robot_manager.h index 6fc0e7f..328b6a6 100644 --- a/include/temoto_robot_manager/robot_manager.h +++ b/include/temoto_robot_manager/robot_manager.h @@ -90,7 +90,7 @@ class RobotManager : public temoto_core::BaseSubsystem bool getManipulationNamedTargetsCb(RobotGetNamedTargets::Request& req, RobotGetNamedTargets::Response& res); - bool goalNavigationCb(RobotNavigationGoal::Request& req, RobotNavigationGoal::Response& res); + bool goalNavigationCb(NavigationGoal::Request& req, NavigationGoal::Response& res); bool gripperControlPositionCb(RobotGripperControlPosition::Request& req, RobotGripperControlPosition::Response& res); @@ -112,7 +112,7 @@ class RobotManager : public temoto_core::BaseSubsystem void navigationFeatureUpdateCb(const RmNavigationFeedbackWrap& feedback); - bool cancelNavigationGoalCb(RobotCancelNavigationGoal::Request& req, RobotCancelNavigationGoal::Response& res); + bool cancelNavigationGoalCb(CancelNavigationGoal::Request& req, CancelNavigationGoal::Response& res); RobotConfigs parseRobotConfigs(const YAML::Node& config); @@ -160,7 +160,7 @@ class RobotManager : public temoto_core::BaseSubsystem ros::ServiceClient client_gripper_control_position_; ros::ServiceClient client_cancel_navigation_goal_; - std::map ongoing_navigation_requests_; + std::map ongoing_navigation_requests_; std::mutex mutex_ongoing_navigation_requests_; ros::Publisher pub_navigation_feature_feedback_; diff --git a/include/temoto_robot_manager/robot_manager_interface.h b/include/temoto_robot_manager/robot_manager_interface.h index fee8c83..ea81723 100644 --- a/include/temoto_robot_manager/robot_manager_interface.h +++ b/include/temoto_robot_manager/robot_manager_interface.h @@ -61,14 +61,14 @@ class RobotManagerInterface client_get_manipulation_named_targets_ = nh_.serviceClient(srv_name::SERVER_GET_MANIPULATION_NAMED_TARGETS); client_navigation_goal_ = - nh_.serviceClient(srv_name::SERVER_NAVIGATION_GOAL); + nh_.serviceClient(srv_name::SERVER_NAVIGATION_GOAL); client_gripper_control_position_ = nh_.serviceClient(srv_name::SERVER_GRIPPER_CONTROL_POSITION); client_get_robot_config_ = nh_.serviceClient(srv_name::SERVER_GET_CONFIG); client_cancel_navigation_goal_ = - nh_.serviceClient(srv_name::SERVER_CANCEL_NAVIGATION_GOAL); + nh_.serviceClient(srv_name::SERVER_CANCEL_NAVIGATION_GOAL); client_custom_request_ = nh_.serviceClient(channels::custom::REQUEST); @@ -367,7 +367,7 @@ class RobotManagerInterface void navigationGoal(const std::string& robot_name , const geometry_msgs::PoseStamped& pose) { - temoto_robot_manager::RobotNavigationGoal msg; + temoto_robot_manager::NavigationGoal msg; msg.request.target_pose = pose; if (pose.header.frame_id.empty()) @@ -388,7 +388,7 @@ class RobotManagerInterface } } - bool navigationGoal(RobotNavigationGoal& goal) + bool navigationGoal(NavigationGoal& goal) { if (goal.request.target_pose.header.frame_id.empty()) { @@ -405,45 +405,57 @@ class RobotManagerInterface TEMOTO_INFO_("Goal response not success"); } - ongoing_navigation_queries_.insert({goal.request.robot_name, NavigationQuery(goal)}); + goal.request.request_id = goal.response.request_id; + ongoing_navigation_queries_.insert({goal.request.request_id , NavigationQuery(goal)}); // wait to finish execution - while (getNavigationFeedback(goal.request.robot_name)->status != NavigationFeedback::FINISHED - && getNavigationFeedback(goal.request.robot_name)->status != NavigationFeedback::CANCELLED) + while (getNavigationFeedback(goal.request.request_id)->status != NavigationFeedback::FINISHED + && getNavigationFeedback(goal.request.request_id)->status != NavigationFeedback::CANCELLED && ros::ok()) { ros::Duration(1).sleep(); } - auto ongoing_query_it = ongoing_navigation_queries_.find(goal.request.robot_name); + auto ongoing_query_it = ongoing_navigation_queries_.find(goal.request.request_id); if (ongoing_query_it != ongoing_navigation_queries_.end()) { ongoing_navigation_queries_.erase(ongoing_query_it); } - if (getNavigationFeedback(goal.request.robot_name)->status == NavigationFeedback::CANCELLED) + if (getNavigationFeedback(goal.request.request_id)->status == NavigationFeedback::CANCELLED) { return false; } return goal.response.success; } - std::optional getNavigationFeedback(const std::string& robot_name) + std::optional getNavigationFeedback(const std::string& request_id) { std::lock_guard lock(navigation_queries_mutex_); - auto ongoing_query_it = ongoing_navigation_queries_.find(robot_name); - + auto ongoing_query_it = ongoing_navigation_queries_.find(request_id); if (ongoing_query_it == ongoing_navigation_queries_.end()) { - TEMOTO_INFO_STREAM_("There's no ongoing query for the " << robot_name << " robot"); + TEMOTO_INFO_STREAM_("There's no ongoing query with " << request_id << " request id"); return {}; - //throw TEMOTO_ERRSTACK("Could not find the request in the list of ongoing requests"); } return ongoing_query_it->second.feedback; } - bool cancelNavigationGoal(RobotCancelNavigationGoal& cancel_goal) + bool cancelNavigationGoal(const std::string& request_id) { + std::lock_guard lock(custom_queries_mutex_); + auto ongoing_query_it = ongoing_navigation_queries_.find(request_id); + + if (ongoing_query_it == ongoing_navigation_queries_.end()) + { + throw TEMOTO_ERRSTACK("Could not find the request in the list of ongoing requests"); + } + + CancelNavigationGoal cancel_goal; + cancel_goal.request.robot_name = ongoing_query_it->second.request.request.robot_name; + cancel_goal.request.priority = ongoing_query_it->second.request.request.priority; + cancel_goal.request.request_id = ongoing_query_it->first; + if (!client_cancel_navigation_goal_.call(cancel_goal)) { throw TEMOTO_ERRSTACK("Unable to reach the CancelNavigationGoal server"); @@ -538,8 +550,8 @@ class RobotManagerInterface struct NavigationQuery { - NavigationQuery(const RobotNavigationGoal& nr) : request{nr}{} - RobotNavigationGoal request; + NavigationQuery(const NavigationGoal& nr) : request{nr}{} + NavigationGoal request; NavigationFeedback feedback; }; @@ -557,7 +569,7 @@ class RobotManagerInterface void navigationFeedbackCb(const NavigationFeedback& msg) { std::lock_guard lock(custom_queries_mutex_); - auto ongoing_nav_query_it = ongoing_navigation_queries_.find(msg.robot_name); + auto ongoing_nav_query_it = ongoing_navigation_queries_.find(msg.request_id); if (ongoing_nav_query_it != ongoing_navigation_queries_.end()) { ongoing_nav_query_it->second.feedback = msg; diff --git a/include/temoto_robot_manager/robot_manager_services.h b/include/temoto_robot_manager/robot_manager_services.h index b039de9..6f8d2f3 100644 --- a/include/temoto_robot_manager/robot_manager_services.h +++ b/include/temoto_robot_manager/robot_manager_services.h @@ -25,14 +25,14 @@ #include "temoto_robot_manager/RobotGetVizInfo.h" #include "temoto_robot_manager/RobotGetTarget.h" #include "temoto_robot_manager/RobotGetNamedTargets.h" -#include "temoto_robot_manager/RobotNavigationGoal.h" +#include "temoto_robot_manager/NavigationGoal.h" #include "temoto_robot_manager/RobotGripperControlPosition.h" #include "temoto_robot_manager/RobotGetConfig.h" #include "temoto_robot_manager/CustomRequest.h" #include "temoto_robot_manager/CustomRequestPreempt.h" #include "temoto_robot_manager/CustomFeedback.h" #include "temoto_robot_manager/NavigationFeedback.h" -#include "temoto_robot_manager/RobotCancelNavigationGoal.h" +#include "temoto_robot_manager/CancelNavigationGoal.h" #include diff --git a/msg/NavigationFeedback.msg b/msg/NavigationFeedback.msg index f3a5bc2..6b6ae56 100644 --- a/msg/NavigationFeedback.msg +++ b/msg/NavigationFeedback.msg @@ -1,6 +1,6 @@ Header header string robot_name -# string request_id +string request_id uint8 status uint8 IDLE=2 diff --git a/src/navigation_plugin_helper.cpp b/src/navigation_plugin_helper.cpp index bce0bb4..5c81a76 100644 --- a/src/navigation_plugin_helper.cpp +++ b/src/navigation_plugin_helper.cpp @@ -206,6 +206,7 @@ void NavigationPluginHelper::sendUpdate() const RmNavigationFeedbackWrap fbw; fbw.robot_name = current_request_->robot_name; + fbw.request_id = current_request_->request_id; fbw.status = uint8_t(state_); fbw.progress = fb->progress; fbw.base_position = fb->base_position; diff --git a/src/robot.cpp b/src/robot.cpp index 652395e..e87f7e9 100644 --- a/src/robot.cpp +++ b/src/robot.cpp @@ -993,23 +993,13 @@ std::vector Robot::getNamedTargetPoses(const std::string& planning_ return group_it->second->getNamedTargets(); } -void Robot::goalNavigation(const geometry_msgs::PoseStamped& target_pose) +void Robot::goalNavigation(const RmNavigationRequestWrap& request) { if (!isRobotOperational()) { throw TEMOTO_ERRSTACK("Could not navigate the robot because robot is not operational"); } FeatureNavigation& ftr = config_->getFeatureNavigation(); - RmNavigationRequestWrap request; - request.robot_name = config_->getName(); - request.goal_pose.header.frame_id = target_pose.header.frame_id; - request.goal_pose.pose.position.x = target_pose.pose.position.x; - request.goal_pose.pose.position.y = target_pose.pose.position.y; - request.goal_pose.pose.position.z = target_pose.pose.position.z; - request.goal_pose.pose.orientation.x = target_pose.pose.orientation.x; - request.goal_pose.pose.orientation.y = target_pose.pose.orientation.y; - request.goal_pose.pose.orientation.z = target_pose.pose.orientation.z; - request.goal_pose.pose.orientation.w = target_pose.pose.orientation.w; navigation_feature_plugin_->sendGoal(request); /* diff --git a/src/robot_manager.cpp b/src/robot_manager.cpp index 6c4ec0d..7c2a4c2 100644 --- a/src/robot_manager.cpp +++ b/src/robot_manager.cpp @@ -305,7 +305,7 @@ void RobotManager::navigationFeatureUpdateCb(const RmNavigationFeedbackWrap& fee msg.header.stamp = ros::Time::now(); msg.robot_name = feedback.robot_name; - // msg.request_id = feedback.request_id; + msg.request_id = feedback.request_id; msg.status = feedback.status; msg.progress = feedback.progress; @@ -781,7 +781,7 @@ catch(resource_registrar::TemotoErrorStack& e) return true; } -bool RobotManager::goalNavigationCb(RobotNavigationGoal::Request& req, RobotNavigationGoal::Response& res) +bool RobotManager::goalNavigationCb(NavigationGoal::Request& req, NavigationGoal::Response& res) try { TEMOTO_INFO_("Received a goal Navigation request"); @@ -794,7 +794,6 @@ try , [&](const auto& ongoing_req) { return req == ongoing_req.second.request; - // return req.robot_name == ongoing_req.second.request.robot_name; }); /* @@ -818,23 +817,44 @@ try ongoing_navigation_requests_.erase(navigation_request_it); } + req.request_id = generateId(); + res.request_id = req.request_id; + + RmNavigationRequestWrap req_rm; + req_rm.robot_name = req.robot_name; + req_rm.request_id = res.request_id; + req_rm.goal_pose.header.frame_id = req.target_pose.header.frame_id; + req_rm.goal_pose.pose.position.x = req.target_pose.pose.position.x; + req_rm.goal_pose.pose.position.y = req.target_pose.pose.position.y; + req_rm.goal_pose.pose.position.z = req.target_pose.pose.position.z; + req_rm.goal_pose.pose.orientation.x = req.target_pose.pose.orientation.x; + req_rm.goal_pose.pose.orientation.y = req.target_pose.pose.orientation.y; + req_rm.goal_pose.pose.orientation.z = req.target_pose.pose.orientation.z; + req_rm.goal_pose.pose.orientation.w = req.target_pose.pose.orientation.w; + if (loaded_robot->isLocal()) { TEMOTO_DEBUG_STREAM_("Navigating '" << req.robot_name << " to pose: " << req.target_pose << " ..."); - loaded_robot->goalNavigation(req.target_pose); // The robot would move with respect to the coordinate frame defined in the header - - RobotNavigationGoal goal; + loaded_robot->goalNavigation(req_rm); + + NavigationGoal goal; goal.request = req; goal.response = res; - auto it = ongoing_navigation_requests_.find(req.robot_name); + auto it = std::find_if( + ongoing_navigation_requests_.begin() + , ongoing_navigation_requests_.end() + , [&](const auto& ongoing_req) + { + return req.robot_name == ongoing_req.second.request.robot_name; + }); if (it != ongoing_navigation_requests_.end()) { it->second = goal; } else { - ongoing_navigation_requests_.insert(std::make_pair(req.robot_name, goal)); + ongoing_navigation_requests_.insert(std::make_pair(req.request_id, goal)); } } else @@ -843,8 +863,8 @@ try + srv_name::SERVER_NAVIGATION_GOAL; TEMOTO_DEBUG_STREAM_("Forwarding the request to remote robot manager at '" << topic << "'."); - ros::ServiceClient client_navigation_goal_ = nh_.serviceClient(topic); - RobotNavigationGoal fwd_goal_srvc; + ros::ServiceClient client_navigation_goal_ = nh_.serviceClient(topic); + NavigationGoal fwd_goal_srvc; fwd_goal_srvc.request = req; fwd_goal_srvc.response = res; if (client_navigation_goal_.call(fwd_goal_srvc)) @@ -865,7 +885,7 @@ catch(resource_registrar::TemotoErrorStack& e) return true; } -bool RobotManager::cancelNavigationGoalCb(RobotCancelNavigationGoal::Request& req, RobotCancelNavigationGoal::Response& res) +bool RobotManager::cancelNavigationGoalCb(CancelNavigationGoal::Request& req, CancelNavigationGoal::Response& res) try { TEMOTO_INFO_("Cancel Navigation goal request"); @@ -881,17 +901,35 @@ try return req.robot_name == ongoing_req.second.request.robot_name; }); + if (navigation_request_it == ongoing_navigation_requests_.end()) + { + TEMOTO_WARN_STREAM_("There is no ongoing navigation. Request id " << req.request_id << " is invalid" << std::endl); + return true; + } + /* * DECLINE: If priority is low */ - if (req.priority <= navigation_request_it->second.request.priority) + if (req.request_id.empty() && (req.priority <= navigation_request_it->second.request.priority)) + { + res.message = "Cancel goal request declined: Priority lower than required"; + TEMOTO_WARN_STREAM_(res.message << std::endl); + return true; + } + + /* + * DECLINE: If ID was provided but mismatches + */ + if (!req.request_id.empty() && (req.request_id != navigation_request_it->second.response.request_id)) { - TEMOTO_WARN_STREAM_("Cancel goal request declined: Priority lower than required" << std::endl); + res.message = "Cancel Goal request declined: Request ID mismatch"; + TEMOTO_WARN_STREAM_(res.message << std::endl); + res.result = false; return true; } /* - * ACCEPT: If the priority is higher + * ACCEPT: If ID matches or the priority is higher */ RobotPtr loaded_robot = findLoadedRobot(req.robot_name); if (loaded_robot->isLocal()) @@ -908,8 +946,8 @@ try + srv_name::SERVER_CANCEL_NAVIGATION_GOAL; TEMOTO_DEBUG_STREAM_("Forwarding the request to remote robot manager at '" << topic << "'."); - ros::ServiceClient client_cancel_navigation_goal_ = nh_.serviceClient(topic); - RobotCancelNavigationGoal fwd_cancel_goal_srvc; + ros::ServiceClient client_cancel_navigation_goal_ = nh_.serviceClient(topic); + CancelNavigationGoal fwd_cancel_goal_srvc; fwd_cancel_goal_srvc.request = req; fwd_cancel_goal_srvc.response = res; if (client_cancel_navigation_goal_.call(fwd_cancel_goal_srvc)) @@ -926,6 +964,8 @@ try } catch(resource_registrar::TemotoErrorStack& e) { + res.message = std::string("Cancel Navigation Goal request declined: \n") + e.what(); + TEMOTO_WARN_STREAM_(res.message << std::endl); res.result = false; return true; } diff --git a/srv/RobotCancelNavigationGoal.srv b/srv/CancelNavigationGoal.srv similarity index 61% rename from srv/RobotCancelNavigationGoal.srv rename to srv/CancelNavigationGoal.srv index a44c5d2..d058980 100644 --- a/srv/RobotCancelNavigationGoal.srv +++ b/srv/CancelNavigationGoal.srv @@ -1,6 +1,8 @@ string robot_name uint8 priority +string request_id --- bool result +string message \ No newline at end of file diff --git a/srv/RobotNavigationGoal.srv b/srv/NavigationGoal.srv similarity index 62% rename from srv/RobotNavigationGoal.srv rename to srv/NavigationGoal.srv index 3ba4320..2b93672 100644 --- a/srv/RobotNavigationGoal.srv +++ b/srv/NavigationGoal.srv @@ -1,7 +1,10 @@ string robot_name geometry_msgs/PoseStamped target_pose +string client_id +string request_id uint8 priority --- +string request_id bool success \ No newline at end of file From 9b9c4b18b7b50a8b0a355cd678bbb5a685d28d08 Mon Sep 17 00:00:00 2001 From: FabianEP11 Date: Wed, 14 Feb 2024 11:45:40 -0600 Subject: [PATCH 12/12] Fixed navigation mutex & removed move_base dependencies --- CMakeLists.txt | 1 - include/temoto_robot_manager/robot.h | 2 -- .../robot_manager_interface.h | 24 +++++++++++-------- package.xml | 1 - 4 files changed, 14 insertions(+), 14 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d4f6b97..949e67c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -13,7 +13,6 @@ find_package(catkin REQUIRED COMPONENTS message_generation cmake_modules moveit_ros_planning_interface - move_base_msgs geometry_msgs std_msgs tf2 diff --git a/include/temoto_robot_manager/robot.h b/include/temoto_robot_manager/robot.h index 4530c03..da3e797 100644 --- a/include/temoto_robot_manager/robot.h +++ b/include/temoto_robot_manager/robot.h @@ -27,7 +27,6 @@ #include "temoto_robot_manager/navigation_plugin_helper.h" #include #include -#include #include #include #include @@ -138,7 +137,6 @@ class Robot std::map> planning_groups_; // Navigation related - typedef actionlib::SimpleActionClient MoveBaseClient; ros::Subscriber localized_pose_sub_; geometry_msgs::PoseWithCovarianceStamped current_pose_navigation_; diff --git a/include/temoto_robot_manager/robot_manager_interface.h b/include/temoto_robot_manager/robot_manager_interface.h index ea81723..30d3d54 100644 --- a/include/temoto_robot_manager/robot_manager_interface.h +++ b/include/temoto_robot_manager/robot_manager_interface.h @@ -404,9 +404,12 @@ class RobotManagerInterface { TEMOTO_INFO_("Goal response not success"); } - - goal.request.request_id = goal.response.request_id; - ongoing_navigation_queries_.insert({goal.request.request_id , NavigationQuery(goal)}); + else + { + goal.request.request_id = goal.response.request_id; + std::lock_guard lock(navigation_queries_mutex_); + ongoing_navigation_queries_.insert({goal.request.request_id , NavigationQuery(goal)}); + } // wait to finish execution while (getNavigationFeedback(goal.request.request_id)->status != NavigationFeedback::FINISHED @@ -415,16 +418,18 @@ class RobotManagerInterface ros::Duration(1).sleep(); } + if (getNavigationFeedback(goal.request.request_id)->status == NavigationFeedback::CANCELLED) + { + goal.response.success = false; + } + auto ongoing_query_it = ongoing_navigation_queries_.find(goal.request.request_id); if (ongoing_query_it != ongoing_navigation_queries_.end()) { + std::lock_guard lock(navigation_queries_mutex_); ongoing_navigation_queries_.erase(ongoing_query_it); } - if (getNavigationFeedback(goal.request.request_id)->status == NavigationFeedback::CANCELLED) - { - return false; - } return goal.response.success; } @@ -437,13 +442,12 @@ class RobotManagerInterface TEMOTO_INFO_STREAM_("There's no ongoing query with " << request_id << " request id"); return {}; } - return ongoing_query_it->second.feedback; } bool cancelNavigationGoal(const std::string& request_id) { - std::lock_guard lock(custom_queries_mutex_); + std::lock_guard lock(navigation_queries_mutex_); auto ongoing_query_it = ongoing_navigation_queries_.find(request_id); if (ongoing_query_it == ongoing_navigation_queries_.end()) @@ -568,7 +572,7 @@ class RobotManagerInterface void navigationFeedbackCb(const NavigationFeedback& msg) { - std::lock_guard lock(custom_queries_mutex_); + std::lock_guard lock(navigation_queries_mutex_); auto ongoing_nav_query_it = ongoing_navigation_queries_.find(msg.request_id); if (ongoing_nav_query_it != ongoing_navigation_queries_.end()) { diff --git a/package.xml b/package.xml index aa2fd78..b1c9941 100644 --- a/package.xml +++ b/package.xml @@ -15,7 +15,6 @@ tf2_geometry_msgs tf2 moveit_ros_planning_interface - move_base_msgs yaml-cpp temoto_core temoto_resource_registrar