Adapts callbacks to simplified mode services

* Adapts mode callbacks for nodes and systems (get mode, change mode,
get available modes)
* part name (system or node) no longer cames from request object, but is
coded in the callback

#24

Signed-off-by: Arne Nordmann <arne.nordmann@de.bosch.com>

system_modes/include/system_modes/mode_manager.hpp

@@ -65,26 +65,30 @@ class ModeManager : public rclcpp::Node
 
   // Mode service callbacks
   virtual void on_change_mode(
-    const std::shared_ptr<rmw_request_id_t>,
+    const std::string &,
     const std::shared_ptr<system_modes::srv::ChangeMode::Request>,
     std::shared_ptr<system_modes::srv::ChangeMode::Response>);
   virtual void on_get_mode(
-    const std::shared_ptr<rmw_request_id_t>,
-    const std::shared_ptr<system_modes::srv::GetMode::Request>,
+    const std::string &,
     std::shared_ptr<system_modes::srv::GetMode::Response>);
   virtual void on_get_available_modes(
-    const std::shared_ptr<rmw_request_id_t>,
-    const std::shared_ptr<system_modes::srv::GetAvailableModes::Request>,
+    const std::string &,
     std::shared_ptr<system_modes::srv::GetAvailableModes::Response>);
 
   virtual bool change_state(
-    const std::string & node,
+    const std::string &,
     unsigned int,
     bool transitive = true);
-  virtual bool change_mode(const std::string & node, const std::string & mode);
+  virtual bool change_mode(
+    const std::string &,
+    const std::string &);
 
-  virtual void change_part_state(const std::string & node, unsigned int);
-  virtual void change_part_mode(const std::string & node, const std::string & mode);
+  virtual void change_part_state(
+    const std::string &,
+    unsigned int);
+  virtual void change_part_mode(
+    const std::string &,
+    const std::string &);
 
 private:
   std::shared_ptr<ModeInference> mode_inference_;

system_modes/src/mode_monitor_node.cpp

@@ -103,9 +103,6 @@ bool parseOptions(int argc, char * argv[])
     return true;
   }
 
-  // if (modelfile.empty()) {
-  //   throw invalid_argument("Need path to model file.");
-  // }
   return false;
 }
 
@@ -121,7 +118,7 @@ void mode_change_callback(
   const string & node_name)
 {
   monitor->inference()->update_state(node_name, State::PRIMARY_STATE_ACTIVE);
-  monitor->inference()->update_mode(node_name, msg->goal_mode.label.c_str());
+  monitor->inference()->update_mode(node_name, msg->goal_mode.label);
 }
 
 void transition_request_callback(
@@ -145,7 +142,7 @@ void mode_request_callback(
 {
   monitor->inference()->update_target(
     node_name,
-    StateAndMode(State::PRIMARY_STATE_ACTIVE, msg->goal_mode.label.c_str()));
+    StateAndMode(State::PRIMARY_STATE_ACTIVE, msg->goal_mode.label));
 }
 
 void

system_modes/src/system_modes/mode_manager.cpp

@@ -47,6 +47,7 @@ using lifecycle_msgs::srv::ChangeState;
 using lifecycle_msgs::srv::GetAvailableStates;
 
 using system_modes::msg::ModeEvent;
+using system_modes::srv::GetMode;
 using system_modes::srv::ChangeMode;
 using system_modes::srv::GetAvailableModes;
 
@@ -137,18 +138,32 @@ ModeManager::add_system(const std::string & system)
 
   // Mode services
   service_name = system + "/change_mode";
+  std::function<void(const std::shared_ptr<rmw_request_id_t>,
+    const std::shared_ptr<ChangeMode::Request>,
+    std::shared_ptr<ChangeMode::Response>)> mode_change_callback =
+    std::bind(&ModeManager::on_change_mode, this, system, _2, _3);
   this->mode_change_srv_[system] = this->create_service<ChangeMode>(
-    service_name, std::bind(&ModeManager::on_change_mode, this, _1, _2, _3));
+    service_name,
+    mode_change_callback);
 
   service_name = system + "/get_mode";
+  std::function<void(const std::shared_ptr<rmw_request_id_t>,
+    const std::shared_ptr<GetMode::Request>,
+    std::shared_ptr<GetMode::Response>)> mode_get_callback =
+    std::bind(&ModeManager::on_get_mode, this, system, _3);
   this->get_mode_srv_[system] =
     this->create_service<system_modes::srv::GetMode>(
     service_name,
-    std::bind(&ModeManager::on_get_mode, this, _1, _2, _3));
+    mode_get_callback);
 
   service_name = system + "/get_available_modes";
+  std::function<void(const std::shared_ptr<rmw_request_id_t>,
+    const std::shared_ptr<GetAvailableModes::Request>,
+    std::shared_ptr<GetAvailableModes::Response>)> mode_avail_callback =
+    std::bind(&ModeManager::on_get_available_modes, this, system, _3);
   this->modes_srv_[system] = this->create_service<GetAvailableModes>(
-    service_name, std::bind(&ModeManager::on_get_available_modes, this, _1, _2, _3));
+    service_name,
+    mode_avail_callback);
 
   // Lifecycle change clients
   service_name = system + "/change_state";
@@ -174,18 +189,32 @@ ModeManager::add_node(const std::string & node)
 
   // Mode services
   service_name = node + "/change_mode";
+  std::function<void(const std::shared_ptr<rmw_request_id_t>,
+    const std::shared_ptr<ChangeMode::Request>,
+    std::shared_ptr<ChangeMode::Response>)> mode_change_callback =
+    std::bind(&ModeManager::on_change_mode, this, node, _2, _3);
   this->mode_change_srv_[node] = this->create_service<ChangeMode>(
-    service_name, std::bind(&ModeManager::on_change_mode, this, _1, _2, _3));
+    service_name,
+    mode_change_callback);
 
   service_name = node + "/get_mode";
+  std::function<void(const std::shared_ptr<rmw_request_id_t>,
+    const std::shared_ptr<GetMode::Request>,
+    std::shared_ptr<GetMode::Response>)> mode_get_callback =
+    std::bind(&ModeManager::on_get_mode, this, node, _3);
   this->get_mode_srv_[node] =
     this->create_service<system_modes::srv::GetMode>(
     service_name,
-    std::bind(&ModeManager::on_get_mode, this, _1, _2, _3));
+    mode_get_callback);
 
   service_name = node + "/get_available_modes";
+  std::function<void(const std::shared_ptr<rmw_request_id_t>,
+    const std::shared_ptr<GetAvailableModes::Request>,
+    std::shared_ptr<GetAvailableModes::Response>)> mode_avail_callback =
+    std::bind(&ModeManager::on_get_available_modes, this, node, _3);
   this->modes_srv_[node] = this->create_service<GetAvailableModes>(
-    service_name, std::bind(&ModeManager::on_get_available_modes, this, _1, _2, _3));
+    service_name,
+    mode_avail_callback);
 
   // Lifecycle change clients
   service_name = node + "/change_state";
@@ -278,31 +307,30 @@ ModeManager::on_get_available_states(
 
 void
 ModeManager::on_change_mode(
-  const std::shared_ptr<rmw_request_id_t>,
+  const std::string & node_name,
   const std::shared_ptr<ChangeMode::Request> request,
   std::shared_ptr<ChangeMode::Response> response)
 {
   RCLCPP_INFO(
     this->get_logger(),
     "-> callback change_mode of %s to %s",
-    request->node_name.c_str(),
+    node_name.c_str(),
     request->mode_name.c_str());
 
   // We can't wait for the state/mode transitions
-  response->success = this->change_mode(request->node_name, request->mode_name);
+  response->success = this->change_mode(node_name, request->mode_name);
 }
 
 void
 ModeManager::on_get_mode(
-  const std::shared_ptr<rmw_request_id_t>,
-  const std::shared_ptr<system_modes::srv::GetMode::Request> request,
+  const std::string & node_name,
   std::shared_ptr<system_modes::srv::GetMode::Response> response)
 {
   // TODO(anordman): to be on the safe side, don't use the node name from
   //       the request, but bind it to the callback instead
-  RCLCPP_INFO(this->get_logger(), "-> callback get_mode of %s", request->node_name.c_str());
+  RCLCPP_INFO(this->get_logger(), "-> callback get_mode of %s", node_name.c_str());
   try {
-    auto stateAndMode = this->mode_inference_->infer(request->node_name);
+    auto stateAndMode = this->mode_inference_->infer(node_name);
     response->current_mode = stateAndMode.mode;
   } catch (std::exception & ex) {
     response->current_mode = "unknown";
@@ -312,18 +340,17 @@ ModeManager::on_get_mode(
 
 void
 ModeManager::on_get_available_modes(
-  const std::shared_ptr<rmw_request_id_t>,
-  const std::shared_ptr<GetAvailableModes::Request> request,
+  const std::string & node_name,
   std::shared_ptr<GetAvailableModes::Response> response)
 {
   // TODO(anordman): to be on the safe side, don't use the node name from
   //       the request, but bind it to the callback instead
   RCLCPP_INFO(
     this->get_logger(),
     "-> callback get_available_modes of %s",
-    request->node_name.c_str());
+    node_name.c_str());
   try {
-    response->available_modes = mode_inference_->get_available_modes(request->node_name);
+    response->available_modes = mode_inference_->get_available_modes(node_name);
   } catch (std::exception & ex) {
     RCLCPP_INFO(this->get_logger(), " unknown");
   }