fix: concurency in skill nodes

rbs_skill_interfaces/action/MoveitSendPose.action

@@ -1,19 +1,12 @@
-##Description: Moves robot arm to a specified pose relative to the frame in header.frame_id of target_pose
-#goal definition
-
-#Used to indicate which hardcoded motion constraint to use 
-#e.g 0 no constraint, 1 keep the same end effector orientation
-int32 constraint_mode
-#similar to geometry_msgs/PoseStamped but using euler instead of quaternion
-#at target_pose->header.frame_id define the tf frame 
-# which the pose will be calculated relative from
 geometry_msgs/Pose target_pose
 string robot_name
 float32 end_effector_velocity
 float32 end_effector_acceleration
-float32 duration #if this action cannot be completed within this time period it should be considered failed.
+float32 duration
 string planner_id
 string pipeline_id
+float32 allowed_planning_time
+bool try_plan_untill_success
 ---
 #result definition
 bool success

rbs_skill_servers/include/rbs_skill_servers/base_skill.hpp

@@ -68,6 +68,8 @@ protected:
                 goal->robot_name.c_str());
     (void)uuid;
 
+    std::lock_guard<std::mutex> lock(m_goal_mutex);
+
     m_current_goal = goal;
     m_current_result = std::make_shared<typename ActionT::Result>();
 
@@ -93,6 +95,7 @@ protected:
 
   void execute(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>>
                    goal_handle) {
+    std::lock_guard<std::mutex> lock(m_goal_mutex);
 
     m_current_goal_handle = goal_handle;
 
@@ -122,6 +125,7 @@ protected:
   }
 
   void updateControllersMap() {
+
     using namespace std::chrono_literals;
 
     RCLCPP_INFO(this->get_logger(), "Called list_controllers service to check "
@@ -139,6 +143,8 @@ protected:
       return;
     }
 
+    std::lock_guard<std::mutex> lock(m_controllers_map_mutex);
+
     auto response = list_controllers_future.get();
     m_controllers_map.clear();
 
@@ -451,6 +457,11 @@ private:
   std::vector<std::string> m_required_controllers{};
   std::vector<std::string> m_missing_controllers{};
   std::unordered_map<std::string, std::string> m_controllers_map;
+
+  std::mutex m_goal_mutex;
+  std::mutex m_controllers_map_mutex;
+  std::mutex m_controllers_mutex;
+
 };
 
 } // namespace rbs_skill_actions

rbs_skill_servers/src/mtp_moveit.cpp

@@ -37,6 +37,43 @@ private:
     pose_stamped.header.frame_id = this->get_parameter("base_link").as_string();
     pose_stamped.header.stamp = this->get_clock()->now();
 
+    // TODO(solid-sinusoid): add chomp planner
+    // // move_group_->setPlannerId("BiTRRT");
+    // // move_group_->setPlannerId("chomp");
+    // move_group_->setPlanningPipelineId("chomp");
+    // RCLCPP_INFO(this->get_logger(), "Planning pipeline_id [%s]", move_group_->getPlanningPipelineId().c_str());
+    //
+    // move_group_->setPoseReferenceFrame(this->get_parameter("base_link").as_string());
+    //
+    // moveit::core::RobotState start_state(*move_group_->getCurrentState());
+    // auto current_pose = move_group_->getCurrentPose(this->get_parameter("ee_link").as_string());
+    // auto joint_model_group = move_group_->getRobotModel()->getJointModelGroup(m_current_goal->robot_name);
+    //
+    // auto transform_to_base = start_state.getFrameTransform(this->get_parameter("base_link").as_string());
+    //
+    // // Eigen::Isometry3d pose_eigen;
+    // auto pose = pose_stamped.pose;
+    // Eigen::Vector3d position(pose.position.x, pose.position.y, pose.position.z);
+    //
+    // Eigen::Quaterniond orientation(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z);
+    //
+    // Eigen::Isometry3d pose_eigen = Eigen::Isometry3d::Identity();
+    // pose_eigen.translate(position);
+    // pose_eigen.rotate(orientation);
+    //
+    // Eigen::Isometry3d pose_in_base = transform_to_base * pose_eigen;
+    //
+    // bool found_ik = start_state.setFromIK(joint_model_group, pose_in_base);
+    // std::vector<double> target_state{};
+    // if (found_ik) {
+    //   start_state.copyJointGroupPositions(joint_model_group, target_state);
+    //   move_group_->setJointValueTarget(target_state);
+    // } else{
+    //   RCLCPP_ERROR(this->get_logger(), "IK solution not found");
+    // }
+    //
+
+    move_group_->setPlanningTime(m_current_goal->allowed_planning_time);
     move_group_->setPoseTarget(pose_stamped);
     move_group_->setMaxVelocityScalingFactor(
         m_current_goal->end_effector_velocity);
@@ -44,8 +81,12 @@ private:
         m_current_goal->end_effector_acceleration);
     moveit::planning_interface::MoveGroupInterface::Plan plan;
     moveit::core::MoveItErrorCode plan_err_code = move_group_->plan(plan);
-    if (plan_err_code == moveit::core::MoveItErrorCode::INVALID_MOTION_PLAN) {
-      move_group_->plan(plan);
+    if (m_current_goal->try_plan_untill_success) {
+      RCLCPP_INFO(this->get_logger(), "Plan will be trying untill seccess");
+      while (plan_err_code != moveit::core::MoveItErrorCode::SUCCESS) {
+        RCLCPP_WARN(this->get_logger(), "Failed to plan, trying untill seccess");
+        plan_err_code = move_group_->plan(plan);
+      }
     }
     if (plan_err_code == moveit::core::MoveItErrorCode::SUCCESS) {
       RCLCPP_INFO(this->get_logger(), "Planning success");

rbs_skill_servers/src/mtp_moveit_cpp.cpp

@@ -0,0 +1,89 @@
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "rbs_skill_interfaces/action/moveit_send_pose.hpp"
+#include "rbs_skill_servers/base_skill.hpp"
+#include <memory>
+#include <moveit/move_group_interface/move_group_interface.hpp>
+#include <moveit/utils/moveit_error_code.hpp>
+#include <rclcpp/logging.hpp>
+
+namespace rbs_skill_actions {
+
+using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose;
+
+class MoveitMtp : public SkillBase<MoveitSendPose> {
+public:
+  explicit MoveitMtp(const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
+      : SkillBase("mtp_moveit", options) {
+    this->declare_parameter("base_link", "");
+    this->declare_parameter("ee_link", "");
+  }
+
+private:
+  std::shared_ptr<moveit::planning_interface::MoveGroupInterface> move_group_;
+
+  std::string requiredActionController() override {
+    return "joint_trajectory_controller";
+  }
+
+  void executeAction() override {
+    RCLCPP_INFO(this->get_logger(), "Executing action goal");
+    move_group_ =
+        std::make_shared<moveit::planning_interface::MoveGroupInterface>(
+            this->shared_from_this(), m_current_goal->robot_name);
+
+    move_group_->startStateMonitor();
+
+    geometry_msgs::msg::PoseStamped pose_stamped;
+    pose_stamped.pose = m_current_goal->target_pose;
+    pose_stamped.header.frame_id = this->get_parameter("base_link").as_string();
+    pose_stamped.header.stamp = this->get_clock()->now();
+
+    move_group_->setPlanningTime(m_current_goal->alowed_planning_time);
+    move_group_->setPoseTarget(pose_stamped);
+    move_group_->setMaxVelocityScalingFactor(
+        m_current_goal->end_effector_velocity);
+    move_group_->setMaxAccelerationScalingFactor(
+        m_current_goal->end_effector_acceleration);
+    moveit::planning_interface::MoveGroupInterface::Plan plan;
+    moveit::core::MoveItErrorCode plan_err_code = move_group_->plan(plan);
+    if (m_current_goal->try_plan_untill_success) {
+      RCLCPP_INFO(this->get_logger(), "Plan will be trying untill seccess");
+      while (plan_err_code != moveit::core::MoveItErrorCode::SUCCESS) {
+        RCLCPP_WARN(this->get_logger(), "Failed to plan, trying untill seccess");
+        plan_err_code = move_group_->plan(plan);
+      }
+    }
+    if (plan_err_code == moveit::core::MoveItErrorCode::SUCCESS) {
+      RCLCPP_INFO(this->get_logger(), "Planning success");
+      moveit::core::MoveItErrorCode move_err_code = move_group_->execute(plan);
+      if (move_err_code == moveit::core::MoveItErrorCode::SUCCESS) {
+        m_current_result->success = true;
+        m_current_goal_handle->succeed(m_current_result);
+        RCLCPP_INFO(this->get_logger(), "Successfully executed action goal");
+      } else {
+        RCLCPP_ERROR_STREAM(
+            this->get_logger(),
+            "Failed to move because:"
+                << moveit::core::errorCodeToString(move_err_code));
+      }
+    } else {
+      RCLCPP_WARN_STREAM(this->get_logger(),
+                         "Failed to generate plan because: "
+                             << moveit::core::errorCodeToString(plan_err_code));
+      m_current_result->success = false;
+      m_current_goal_handle->abort(m_current_result);
+    }
+
+    if (m_current_goal_handle->is_canceling()) {
+      m_current_result->success = false;
+      m_current_goal_handle->canceled(m_current_result);
+      RCLCPP_ERROR(this->get_logger(), "Action goal canceled");
+      return;
+    }
+  }
+}; // class MoveToPoseActionServer
+
+} // namespace rbs_skill_actions
+
+#include "rclcpp_components/register_node_macro.hpp"
+RCLCPP_COMPONENTS_REGISTER_NODE(rbs_skill_actions::MoveitMtp);

rbs_utils/rbs_utils/scripts/assembly_config_service.py

@@ -36,7 +36,7 @@ class AssemblyConfigService(Node):
             self.tf2_listner = Tf2Listener(self)
             for relative_part in self.assembly_config.relative_part:
                 tf2_broadcaster.broadcast_tf(
-                    relative_part.relative_at,
+                    relative_part.relative_to,
                     relative_part.name,
                     translation=(
                         relative_part.pose.position.x,
@@ -53,7 +53,7 @@ class AssemblyConfigService(Node):
 
             for grasp_poses in self.assembly_config.grasp_poses:
                 tf2_broadcaster.broadcast_tf(
-                    grasp_poses.relative_at,
+                    grasp_poses.relative_to,
                     grasp_poses.name,
                     translation=(
                         grasp_poses.pose.position.x,
@@ -70,7 +70,7 @@ class AssemblyConfigService(Node):
 
             for place_poses in self.assembly_config.place_poses:
                 tf2_broadcaster.broadcast_tf(
-                    place_poses.relative_at,
+                    place_poses.relative_to,
                     place_poses.name,
                     translation=(
                         place_poses.pose.position.x,
@@ -333,7 +333,7 @@ def parse_assembly_config(yaml_file):
         pose = Pose(position=Point(**position), orientation=Quaternion(**orientation))
         relative_part.append(
             RelativeNamedPose(
-                name=part["name"], relative_at=part["relative_at"], pose=pose
+                name=part["name"], relative_to=part["relative_to"], pose=pose
             )
         )
 
@@ -348,7 +348,7 @@ def parse_assembly_config(yaml_file):
         )
         grasp_pose.append(
             RelativeNamedPose(
-                name=pose["name"], relative_at=pose["relative_at"], pose=pose_obj
+                name=pose["name"], relative_to=pose["relative_to"], pose=pose_obj
             )
         )
 
@@ -374,7 +374,7 @@ def parse_assembly_config(yaml_file):
         )
         place_poses.append(
             RelativeNamedPose(
-                name=pose["name"], relative_at=pose["relative_at"], pose=pose_obj
+                name=pose["name"], relative_to=pose["relative_to"], pose=pose_obj
             )
         )
 

rbs_utils/rbs_utils_interfaces/CMakeLists.txt

@@ -17,6 +17,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
     "msg/RelativeNamedPose.msg"
     "srv/GetGraspPose.srv"
     "srv/GetPlacePose.srv"
+    "srv/GetRelativeNamedPose.srv"
     "srv/GetWorkspace.srv"
     "srv/GetNamedPose.srv"
     "srv/AddTasks.srv"

rbs_utils/rbs_utils_interfaces/msg/RelativeNamedPose.msg

@@ -1,3 +1,3 @@
 string name
-string relative_at
+string relative_to "world"
 geometry_msgs/Pose pose

rbs_utils/rbs_utils_interfaces/srv/GetRelativeNamedPose.srv

@@ -0,0 +1,5 @@
+string pose_name
+string relative_to "world"
+---
+rbs_utils_interfaces/RelativeNamedPose named_pose
+bool ok