add mtp with orientation constraint

rbs_bt_executor/CMakeLists.txt

@@ -95,11 +95,12 @@ list(APPEND plugin_libs rbs_skill_move_topose_array_bt_action_client)
 add_library(rbs_get_workspace SHARED src/GetWorkspace.cpp)
 list(APPEND plugin_libs rbs_get_workspace)
 
-add_library(rbss_act SHARED src/rbssAction.cpp)
-list(APPEND plugin_libs rbss_act)
+add_library(rbs_move_to_pose_orientation_constraint_bt_action_client 
+  SHARED src/MoveToPoseOrientationConstraint.cpp)
+list(APPEND plugin_libs rbs_move_to_pose_orientation_constraint_bt_action_client)
 
-add_library(rbss_cond SHARED src/rbssCondition.cpp)
-list(APPEND plugin_libs rbss_cond)
+add_library(rbs_act SHARED src/rbsBTAction.cpp)
+list(APPEND plugin_libs rbs_act)
 
 add_executable(rbs_bt_executor src/TreeRunner.cpp)
 ament_target_dependencies(rbs_bt_executor ${dependencies})

rbs_bt_executor/src/MoveToPoseOrientationConstraint.cpp

@@ -0,0 +1,62 @@
+#include "behaviortree_ros2/bt_action_node.hpp"
+#include "rbs_skill_interfaces/action/moveit_send_pose_with_constraints.hpp"
+#include <behaviortree_cpp/basic_types.h>
+#include <behaviortree_cpp/tree_node.h>
+#include <behaviortree_ros2/plugins.hpp>
+#include <behaviortree_ros2/ros_node_params.hpp>
+
+using namespace BT;
+using MoveitSendPoseWithConstraintAction =
+    rbs_skill_interfaces::action::MoveitSendPoseWithConstraints;
+
+class MoveToPoseOrientationConstraint
+    : public RosActionNode<MoveitSendPoseWithConstraintAction> {
+public:
+  MoveToPoseOrientationConstraint(const std::string &name,
+                                  const NodeConfig &conf,
+                                  const RosNodeParams &params)
+      : RosActionNode<MoveitSendPoseWithConstraintAction>(name, conf, params) {
+
+    RCLCPP_INFO(this->logger(), "Starting MoveToPose");
+  }
+
+  static BT::PortsList providedPorts() {
+    return providedBasicPorts(
+        {BT::InputPort<std::string>("robot_name"),
+         BT::InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("pose"),
+         BT::InputPort<double>("x_tol"), BT::InputPort<double>("y_tol"),
+         BT::InputPort<double>("z_tol"), BT::InputPort<double>("weight"),
+         BT::InputPort<double>("duration")});
+  }
+
+  bool setGoal(RosActionNode::Goal &goal) override {
+    RCLCPP_INFO(this->logger(), "[%s] Starting send goal [%s]", name().c_str(),
+                this->action_name_.c_str());
+
+    getInput("x_tol", goal.orientation_constraint.absolute_x_axis_tolerance);
+    getInput("y_tol", goal.orientation_constraint.absolute_y_axis_tolerance);
+    getInput("z_tol", goal.orientation_constraint.absolute_z_axis_tolerance);
+    getInput("weight", goal.orientation_constraint.weight);
+    getInput("robot_name", goal.robot_name);
+    getInput("pose", m_target_pose);
+    getInput("duration", goal.duration);
+    goal.target_pose = *m_target_pose;
+    return true;
+  }
+
+  NodeStatus onResultReceived(const WrappedResult &wr) override {
+
+    RCLCPP_INFO(this->logger(), "[%s] Starting get response %s with status %b",
+                name().c_str(), this->action_name_.c_str(), wr.result->success);
+    if (!wr.result->success) {
+      return NodeStatus::FAILURE;
+    }
+    return NodeStatus::SUCCESS;
+  }
+
+private:
+  std::shared_ptr<geometry_msgs::msg::Pose> m_target_pose;
+};
+
+CreateRosNodePlugin(MoveToPoseOrientationConstraint,
+                    "MtpOrientationConstraint");

rbs_skill_interfaces/CMakeLists.txt

@@ -20,6 +20,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
     "action/MoveitSendPose.action"
     "action/MoveitSendJointStates.action"
     "action/GripperCommand.action"
+    "action/MoveitSendPoseWithConstraints.action"
     "action/RbsBt.action"
     "msg/ObjectInfo.msg"
     "msg/PropertyValuePair.msg"

rbs_skill_interfaces/action/MoveitSendPoseWithConstraints.action

@@ -0,0 +1,15 @@
+int32 constraint_mode
+geometry_msgs/Pose target_pose
+moveit_msgs/OrientationConstraint orientation_constraint
+string robot_name
+float32 end_effector_velocity
+float32 end_effector_acceleration
+float32 duration
+---
+bool success
+uint64 millis_passed
+string status
+---
+bool success
+uint64 millis_passed
+string status

rbs_skill_servers/CMakeLists.txt

@@ -100,6 +100,12 @@ add_ros2_component(move_to_pose_moveit_cartesian
     "rbs_skill_actions::MoveitMtpCart"
     mtp_moveit_cart)
 
+add_ros2_component(move_to_pose_orientation_constraint
+  src/mtp_moveit_constraints.cpp
+  "rbs_skill_actions::MoveitMtpOrientationConstraint"
+  mtp_moveit_orientation_constraint
+)
+
 # Install directories and targets
 install(DIRECTORY include/ DESTINATION include)
 install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME})
@@ -113,6 +119,7 @@ install(
           move_to_pose_cartesian
           move_to_pose_moveit_cartesian
           move_to_pose_moveit
+          move_to_pose_orientation_constraint
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION lib/${PROJECT_NAME})

rbs_skill_servers/src/mtp_moveit_constraints.cpp

@@ -0,0 +1,103 @@
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "moveit_msgs/msg/constraints.hpp"
+#include "moveit_msgs/msg/orientation_constraint.hpp"
+#include "rbs_skill_interfaces/action/moveit_send_pose.hpp"
+#include "rbs_skill_interfaces/action/moveit_send_pose_with_constraints.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 MoveitSendPoseOrientationConstraint = rbs_skill_interfaces::action::MoveitSendPoseWithConstraints;
+
+class MoveitMtpOrientationConstraint : public SkillBase<MoveitSendPoseOrientationConstraint> {
+public:
+  explicit MoveitMtpOrientationConstraint(const rclcpp::NodeOptions &options = rclcpp::NodeOptions())
+      : SkillBase("mtp_moveit_orientation_constraint", 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();
+
+    moveit_msgs::msg::OrientationConstraint orientation_constraint;
+    orientation_constraint.weight = m_current_goal->orientation_constraint.weight;
+    orientation_constraint.absolute_x_axis_tolerance = m_current_goal->orientation_constraint.absolute_x_axis_tolerance;
+    orientation_constraint.absolute_y_axis_tolerance = m_current_goal->orientation_constraint.absolute_y_axis_tolerance;
+    orientation_constraint.absolute_z_axis_tolerance = m_current_goal->orientation_constraint.absolute_z_axis_tolerance;
+    orientation_constraint.orientation = pose_stamped.pose.orientation;
+    orientation_constraint.header = pose_stamped.header;
+    orientation_constraint.link_name = this->get_parameter("ee_link").as_string();
+
+    moveit_msgs::msg::Constraints constraints;
+    constraints.orientation_constraints.emplace_back(orientation_constraint);
+
+    move_group_->setPoseTarget(pose_stamped);
+    move_group_->setPathConstraints(constraints);
+    move_group_->setMaxVelocityScalingFactor(
+        m_current_goal->end_effector_velocity);
+    move_group_->setMaxAccelerationScalingFactor(
+        m_current_goal->end_effector_acceleration);
+    move_group_->setPlanningTime(10.0);
+
+    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 (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::MoveitMtpOrientationConstraint);