✨ suave + rosa + plansys working

suave_rosa_plansys/include/suave_rosa_plansys/action_start_robot.hpp

@@ -33,17 +33,13 @@ class StartRobot : public plansys2::ActionExecutorClient
 
   virtual ~StartRobot();
 
-  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
-  on_activate(const rclcpp_lifecycle::State & previous_state);
-
-  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
-  on_deactivate(const rclcpp_lifecycle::State & previous_state);
-
 private:
   bool armed_ = false;
   bool guided_ = false;
   std::string mode_;
 
+  rclcpp::CallbackGroup::SharedPtr callback_group_srv_client_;
+
   rclcpp::Client<mavros_msgs::srv::CommandBool>::SharedPtr arm_motors_cli_;
 
   rclcpp::Client<mavros_msgs::srv::SetMode>::SharedPtr set_guided_cli_;

suave_rosa_plansys/src/action_inspect_pipeline.cpp

@@ -11,17 +11,41 @@
 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 // See the License for the specific language governing permissions and
 // limitations under the License.
+#include "std_msgs/msg/bool.hpp"
 #include "rosa_task_plan_plansys/rosa_action.hpp"
 
 using namespace std::chrono_literals;
+using namespace std::placeholders;
+
+class InspectPipelineAction : public rosa_task_plan_plansys::RosaAction{
+public:
+  InspectPipelineAction(const std::string & node_name,
+    const std::chrono::nanoseconds & rate) : RosaAction(node_name, rate){
+      pipeline_inspected_sub_  = this->create_subscription<std_msgs::msg::Bool>(
+        "/pipeline/inspected",
+        10,
+        std::bind(&InspectPipelineAction::pipeline_inspected_cb, this, _1));
+  };
+
+private:
+  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr pipeline_inspected_sub_;
+
+  bool _pipeline_inspected=false;
+  void pipeline_inspected_cb(const std_msgs::msg::Bool &msg){
+    _pipeline_inspected = msg.data;
+  };
+
+  void do_work(){
+    if(_pipeline_inspected==true) finish(true, 1.0, "Pipeline inspected!");
+  };
+};
 
 int main(int argc, char ** argv)
 {
   rclcpp::init(argc, argv);
   auto node = std::make_shared<rosa_task_plan_plansys::RosaAction>(
     "inspect_pipeline", 500ms);
 
-  // node->set_parameter(rclcpp::Parameter("action_name", "inspect_pipeline"));
   node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
 
   rclcpp::executors::MultiThreadedExecutor executor;

suave_rosa_plansys/src/action_search_pipeline.cpp

@@ -11,14 +11,39 @@
 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 // See the License for the specific language governing permissions and
 // limitations under the License.
+#include "std_msgs/msg/bool.hpp"
 #include "rosa_task_plan_plansys/rosa_action.hpp"
 
 using namespace std::chrono_literals;
+using namespace std::placeholders;
+
+class SearchPipelineAction : public rosa_task_plan_plansys::RosaAction{
+public:
+  SearchPipelineAction(const std::string & node_name,
+    const std::chrono::nanoseconds & rate) : RosaAction(node_name, rate){
+      pipeline_detection_sub_  = this->create_subscription<std_msgs::msg::Bool>(
+        "/pipeline/detected",
+        10,
+        std::bind(&SearchPipelineAction::pipeline_detected_cb, this, _1));
+  };
+
+private:
+  rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr pipeline_detection_sub_;
+
+  bool _pipeline_detected=false;
+  void pipeline_detected_cb(const std_msgs::msg::Bool &msg){
+    _pipeline_detected = msg.data;
+  };
+
+  void do_work(){
+    if(_pipeline_detected==true) finish(true, 1.0, "Pipeline found!");
+  };
+};
 
 int main(int argc, char ** argv)
 {
   rclcpp::init(argc, argv);
-  auto node = std::make_shared<rosa_task_plan_plansys::RosaAction>(
+  auto node = std::make_shared<SearchPipelineAction>(
     "search_pipeline", 500ms);
 
   node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);

suave_rosa_plansys/src/action_start_robot.cpp

@@ -22,11 +22,17 @@ namespace suave_rosa_plansys
     const std::chrono::nanoseconds & rate)
   :   plansys2::ActionExecutorClient(node_name, rate)
   {
+    callback_group_srv_client_ = create_callback_group(
+      rclcpp::CallbackGroupType::MutuallyExclusive);
     arm_motors_cli_ = this->create_client<mavros_msgs::srv::CommandBool>(
-      "mavros/cmd/arming");
+      "mavros/cmd/arming",
+      rmw_qos_profile_services_default,
+      callback_group_srv_client_);
 
     set_guided_cli_ = this->create_client<mavros_msgs::srv::SetMode>(
-      "mavros/set_mode");
+      "mavros/set_mode",
+      rmw_qos_profile_services_default,
+      callback_group_srv_client_);
     mavros_state_sub_  = this->create_subscription<mavros_msgs::msg::State>(
       "mavros/state",
       10,
@@ -37,19 +43,6 @@ namespace suave_rosa_plansys
   {
   }
 
-  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
-  StartRobot::on_activate(const rclcpp_lifecycle::State & previous_state)
-  {
-    RCLCPP_INFO(this->get_logger(), "Starting robot!");
-    return ActionExecutorClient::on_activate(previous_state);
-  }
-
-  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
-  StartRobot::on_deactivate(const rclcpp_lifecycle::State & previous_state)
-  {
-    return ActionExecutorClient::on_deactivate(previous_state);
-  }
-
   void StartRobot::arm_thrusters(){
     if(arm_motors_cli_->service_is_ready()){
       auto request = std::make_shared<mavros_msgs::srv::CommandBool::Request>();
@@ -96,6 +89,7 @@ namespace suave_rosa_plansys
         if(result_->mode_sent && mode_ == "GUIDED"){
           guided_ = true;
           RCLCPP_INFO(this->get_logger(), "Mode set to GUIDED!");
+          finish(true, 1.0, "Robot in guided mode and armed!");
           return;
         }
       }
@@ -122,7 +116,6 @@ int main(int argc, char ** argv)
   auto node = std::make_shared<suave_rosa_plansys::StartRobot>(
     "start_robot", 500ms);
 
-  // node->set_parameter(rclcpp::Parameter("action_name", "inspect_pipeline"));
   node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
 
   rclcpp::executors::MultiThreadedExecutor executor;