robossembler/runtime
7
0
Fork 0
Code Issues 5 Pull requests Projects Releases Packages Wiki Activity Actions

fix: concurency in skill nodes

Browse source
This commit is contained in:
Ilya Uraev 2025-04-07 13:01:25 +03:00
parent adf84ebec7
commit 0798720a4c
8 changed files with 159 additions and 19 deletions
Download patch file Download diff file Expand all files Collapse all files

11
rbs_skill_servers/include/rbs_skill_servers/base_skill.hpp
View file

@ -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

45
rbs_skill_servers/src/mtp_moveit.cpp
View file

@ -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");

89
rbs_skill_servers/src/mtp_moveit_cpp.cpp Normal file
View file

@ -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);