Added rbs_gym package for RL & multi-robot launch setup
This commit is contained in:
parent
f92670cd0d
commit
b58307dea1
103 changed files with 15170 additions and 653 deletions
|
|
@ -1,5 +1,7 @@
|
|||
#include <functional>
|
||||
#include <memory>
|
||||
#include <moveit/robot_trajectory/robot_trajectory.h>
|
||||
#include <moveit/trajectory_processing/time_parameterization.h>
|
||||
#include <thread>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
|
@ -19,6 +21,7 @@
|
|||
#include "moveit/move_group_interface/move_group_interface.h"
|
||||
#include "moveit/planning_interface/planning_interface.h"
|
||||
#include "moveit/robot_model_loader/robot_model_loader.h"
|
||||
#include "moveit/trajectory_processing/time_optimal_trajectory_generation.h"
|
||||
|
||||
/*
|
||||
#include <tf2/LinearMath/Quaternion.h>
|
||||
|
|
@ -122,23 +125,59 @@ private:
|
|||
std::vector<geometry_msgs::msg::Pose> waypoints;
|
||||
auto current_pose = move_group_interface.getCurrentPose();
|
||||
// waypoints.push_back(current_pose.pose);
|
||||
// geometry_msgs::msg::Pose start_pose = current_pose.pose;
|
||||
geometry_msgs::msg::Pose target_pose = goal->target_pose;
|
||||
// target_pose.position = goal->target_pose.position;
|
||||
// int num_waypoints = 100;
|
||||
// for (int i = 1; i <= num_waypoints; ++i) {
|
||||
// geometry_msgs::msg::Pose intermediate_pose;
|
||||
// double fraction = static_cast<double>(i) / (num_waypoints + 1);
|
||||
//
|
||||
// intermediate_pose.position.x =
|
||||
// start_pose.position.x +
|
||||
// fraction * (target_pose.position.x - start_pose.position.x);
|
||||
// intermediate_pose.position.y =
|
||||
// start_pose.position.y +
|
||||
// fraction * (target_pose.position.y - start_pose.position.y);
|
||||
// intermediate_pose.position.z =
|
||||
// start_pose.position.z +
|
||||
// fraction * (target_pose.position.z - start_pose.position.z);
|
||||
//
|
||||
// intermediate_pose.orientation = start_pose.orientation;
|
||||
//
|
||||
// waypoints.push_back(intermediate_pose);
|
||||
// }
|
||||
waypoints.push_back(target_pose);
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "New cartesian target pose [%f, %f, %f]",
|
||||
target_pose.position.x, target_pose.position.y,
|
||||
target_pose.position.z);
|
||||
// waypoints.push_back(start_pose.pose);
|
||||
|
||||
moveit_msgs::msg::RobotTrajectory trajectory;
|
||||
const double jump_threshold = 0.0;
|
||||
const double eef_step = 0.001;
|
||||
double fraction = move_group_interface.computeCartesianPath(
|
||||
waypoints, eef_step, jump_threshold, trajectory);
|
||||
|
||||
robot_trajectory::RobotTrajectory rt(
|
||||
move_group_interface.getCurrentState()->getRobotModel(),
|
||||
goal->robot_name);
|
||||
|
||||
rt.setRobotTrajectoryMsg(*move_group_interface.getCurrentState(), trajectory);
|
||||
|
||||
trajectory_processing::TimeOptimalTrajectoryGeneration tp;
|
||||
|
||||
bool su = tp.computeTimeStamps(rt);
|
||||
|
||||
rt.getRobotTrajectoryMsg(trajectory);
|
||||
|
||||
moveit::planning_interface::MoveGroupInterface::Plan plan;
|
||||
plan.trajectory_ = trajectory;
|
||||
|
||||
if (fraction > 0) {
|
||||
RCLCPP_INFO(this->get_logger(), "Planning success");
|
||||
moveit::core::MoveItErrorCode execute_err_code =
|
||||
move_group_interface.execute(trajectory);
|
||||
move_group_interface.execute(plan);
|
||||
if (execute_err_code == moveit::core::MoveItErrorCode::SUCCESS) {
|
||||
goal_handle->succeed(result);
|
||||
RCLCPP_INFO(this->get_logger(), "Successfully executed action goal");
|
||||
|
|
|
|||
232
rbs_skill_servers/src/moveit_servo_skill_server.cpp
Normal file
232
rbs_skill_servers/src/moveit_servo_skill_server.cpp
Normal file
|
|
@ -0,0 +1,232 @@
|
|||
#include <functional>
|
||||
#include <geometry_msgs/msg/detail/pose_stamped__struct.hpp>
|
||||
#include <geometry_msgs/msg/detail/transform_stamped__struct.hpp>
|
||||
#include <memory>
|
||||
#include <rclcpp/publisher.hpp>
|
||||
#include <rclcpp/qos.hpp>
|
||||
#include <tf2/LinearMath/Transform.h>
|
||||
#include <tf2/convert.h>
|
||||
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
|
||||
#include <thread>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "rclcpp/timer.hpp"
|
||||
#include "rclcpp_components/register_node_macro.hpp"
|
||||
|
||||
// action libs
|
||||
#include "rbs_skill_interfaces/action/moveit_send_pose.hpp"
|
||||
#include "rbs_skill_interfaces/msg/action_feedback_status_constants.hpp"
|
||||
#include "rclcpp_action/rclcpp_action.hpp"
|
||||
|
||||
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||
#include "geometry_msgs/msg/quaternion.hpp"
|
||||
#include "geometry_msgs/msg/transform.hpp"
|
||||
|
||||
// moveit libs
|
||||
#include <moveit_servo/make_shared_from_pool.h>
|
||||
#include <moveit_servo/pose_tracking.h>
|
||||
#include <moveit_servo/servo.h>
|
||||
#include <moveit_servo/servo_parameters.h>
|
||||
#include <moveit_servo/status_codes.h>
|
||||
|
||||
namespace rbs_skill_actions {
|
||||
|
||||
class StatusMonitor {
|
||||
public:
|
||||
StatusMonitor(const rclcpp::Node::SharedPtr &node, const std::string &topic)
|
||||
: m_node(node) {
|
||||
sub_ = node->create_subscription<std_msgs::msg::Int8>(
|
||||
topic, rclcpp::SystemDefaultsQoS(),
|
||||
[this](const std_msgs::msg::Int8::ConstSharedPtr &msg) {
|
||||
return statusCB(msg);
|
||||
});
|
||||
}
|
||||
|
||||
private:
|
||||
rclcpp::Node::SharedPtr m_node;
|
||||
void statusCB(const std_msgs::msg::Int8::ConstSharedPtr &msg) {
|
||||
moveit_servo::StatusCode latest_status =
|
||||
static_cast<moveit_servo::StatusCode>(msg->data);
|
||||
if (latest_status != status_) {
|
||||
status_ = latest_status;
|
||||
const auto &status_str = moveit_servo::SERVO_STATUS_CODE_MAP.at(status_);
|
||||
RCLCPP_INFO_STREAM(m_node->get_logger(), "Servo status: " << status_str);
|
||||
}
|
||||
}
|
||||
|
||||
moveit_servo::StatusCode status_ = moveit_servo::StatusCode::INVALID;
|
||||
rclcpp::Subscription<std_msgs::msg::Int8>::SharedPtr sub_;
|
||||
};
|
||||
|
||||
class MoveServoActionServer : public rclcpp::Node {
|
||||
public:
|
||||
using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose;
|
||||
|
||||
// explicit MoveCartesianActionServer(const rclcpp::Node::SharedPtr& node)
|
||||
explicit MoveServoActionServer(const rclcpp::Node::SharedPtr &node)
|
||||
: Node("move_servo_action_server"), m_node(node) {
|
||||
auto servo_parameters =
|
||||
moveit_servo::ServoParameters::makeServoParameters(node);
|
||||
if (servo_parameters == nullptr) {
|
||||
RCLCPP_FATAL(node->get_logger(), "Could not get servo parameters!");
|
||||
// exit(EXIT_FAILURE);
|
||||
}
|
||||
|
||||
// Load the planning scene monitor
|
||||
m_planning_scene_monitor =
|
||||
std::make_shared<planning_scene_monitor::PlanningSceneMonitor>(
|
||||
node, "robot_description");
|
||||
if (!m_planning_scene_monitor->getPlanningScene()) {
|
||||
RCLCPP_ERROR_STREAM(node->get_logger(),
|
||||
"Error in setting up the PlanningSceneMonitor.");
|
||||
}
|
||||
m_planning_scene_monitor->providePlanningSceneService();
|
||||
m_planning_scene_monitor->startSceneMonitor();
|
||||
m_planning_scene_monitor->startWorldGeometryMonitor(
|
||||
planning_scene_monitor::PlanningSceneMonitor::
|
||||
DEFAULT_COLLISION_OBJECT_TOPIC,
|
||||
planning_scene_monitor::PlanningSceneMonitor::
|
||||
DEFAULT_PLANNING_SCENE_WORLD_TOPIC,
|
||||
false /* skip octomap monitor */);
|
||||
m_planning_scene_monitor->startStateMonitor("/joint_states");
|
||||
m_planning_scene_monitor->startPublishingPlanningScene(
|
||||
planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE);
|
||||
|
||||
// Wait for Planning Scene Monitor to setup
|
||||
if (!m_planning_scene_monitor->waitForCurrentRobotState(
|
||||
node->now(), 5.0 /* seconds */)) {
|
||||
RCLCPP_ERROR_STREAM(
|
||||
node->get_logger(),
|
||||
"Error waiting for current robot state in PlanningSceneMonitor.");
|
||||
}
|
||||
|
||||
// Create the pose tracker
|
||||
m_tracker = std::make_shared<moveit_servo::PoseTracking>(
|
||||
node, servo_parameters, m_planning_scene_monitor);
|
||||
|
||||
m_status_monitor = std::make_shared<StatusMonitor>(node, servo_parameters->status_topic);
|
||||
|
||||
}
|
||||
|
||||
void init() {
|
||||
|
||||
m_action_server = rclcpp_action::create_server<MoveitSendPose>(
|
||||
m_node->get_node_base_interface(), m_node->get_node_clock_interface(),
|
||||
m_node->get_node_logging_interface(),
|
||||
m_node->get_node_waitables_interface(), "move_servo",
|
||||
std::bind(&MoveServoActionServer::goal_callback, this,
|
||||
std::placeholders::_1, std::placeholders::_2),
|
||||
std::bind(&MoveServoActionServer::cancel_callback, this,
|
||||
std::placeholders::_1),
|
||||
std::bind(&MoveServoActionServer::accepted_callback, this,
|
||||
std::placeholders::_1));
|
||||
|
||||
m_pose_pub = m_node->create_publisher<geometry_msgs::msg::PoseStamped>("target_pose", rclcpp::SystemDefaultsQoS());
|
||||
}
|
||||
|
||||
private:
|
||||
rclcpp::Node::SharedPtr m_node;
|
||||
rclcpp_action::Server<MoveitSendPose>::SharedPtr m_action_server;
|
||||
rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr m_pose_pub;
|
||||
planning_scene_monitor::PlanningSceneMonitorPtr m_planning_scene_monitor;
|
||||
moveit_servo::PoseTrackingPtr m_tracker;
|
||||
std::shared_ptr<StatusMonitor> m_status_monitor;
|
||||
|
||||
using ServerGoalHandle = rclcpp_action::ServerGoalHandle<MoveitSendPose>;
|
||||
|
||||
rclcpp_action::GoalResponse
|
||||
goal_callback(const rclcpp_action::GoalUUID &uuid,
|
||||
std::shared_ptr<const MoveitSendPose::Goal> goal) {
|
||||
RCLCPP_INFO(
|
||||
this->get_logger(),
|
||||
"Received goal request for robot [%s] with Pose [%f, %f, %f, %f, %f, "
|
||||
"%f, %f]",
|
||||
goal->robot_name.c_str(), goal->target_pose.position.x,
|
||||
goal->target_pose.position.y, goal->target_pose.position.z,
|
||||
goal->target_pose.orientation.x, goal->target_pose.orientation.y,
|
||||
goal->target_pose.orientation.z, goal->target_pose.orientation.w);
|
||||
(void)uuid;
|
||||
|
||||
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
|
||||
}
|
||||
|
||||
rclcpp_action::CancelResponse
|
||||
cancel_callback(const std::shared_ptr<ServerGoalHandle> goal_handle) {
|
||||
RCLCPP_INFO(this->get_logger(), "Received cancel request");
|
||||
(void)goal_handle;
|
||||
return rclcpp_action::CancelResponse::ACCEPT;
|
||||
}
|
||||
|
||||
void accepted_callback(const std::shared_ptr<ServerGoalHandle> goal_handle) {
|
||||
using namespace std::placeholders;
|
||||
std::thread(std::bind(&MoveServoActionServer::execute, this, _1),
|
||||
goal_handle)
|
||||
.detach();
|
||||
// std::thread(
|
||||
// [this, goal_handle]() {
|
||||
// execute(goal_handle);
|
||||
// }).detach();
|
||||
}
|
||||
|
||||
void execute(const std::shared_ptr<ServerGoalHandle> goal_handle) {
|
||||
RCLCPP_INFO(this->get_logger(), "Executing action goal");
|
||||
const auto goal = goal_handle->get_goal();
|
||||
auto result = std::make_shared<MoveitSendPose::Result>();
|
||||
|
||||
geometry_msgs::msg::TransformStamped current_ee_pose;
|
||||
m_tracker->getCommandFrameTransform(current_ee_pose);
|
||||
// Convert it to a Pose
|
||||
geometry_msgs::msg::PoseStamped target_pose;
|
||||
target_pose.header.frame_id = current_ee_pose.header.frame_id;
|
||||
target_pose.pose = goal->target_pose;
|
||||
// target_pose.pose.position.y = current_ee_pose.transform.translation.y;
|
||||
// target_pose.pose.position.z = current_ee_pose.transform.translation.z;
|
||||
// target_pose.pose.orientation = current_ee_pose.transform.rotation;
|
||||
|
||||
// target_pose.pose.position.x += 0.1;
|
||||
|
||||
m_tracker->resetTargetPose();
|
||||
|
||||
target_pose.header.stamp = m_node->now();
|
||||
m_pose_pub->publish(target_pose);
|
||||
|
||||
Eigen::Vector3d lin_tol{ 0.001, 0.001, 0.001 };
|
||||
double rot_tol = 0.01;
|
||||
// Run the pose tracking
|
||||
moveit_servo::PoseTrackingStatusCode tracking_status =
|
||||
m_tracker->moveToPose(lin_tol, rot_tol, 0.1 /* target pose timeout */);
|
||||
|
||||
if (tracking_status == moveit_servo::PoseTrackingStatusCode::SUCCESS) {
|
||||
result->success = true;
|
||||
goal_handle->succeed(result);
|
||||
RCLCPP_INFO(this->get_logger(), "Pose tracking succeeded.");
|
||||
} else {
|
||||
result->success = false;
|
||||
goal_handle->abort(result);
|
||||
RCLCPP_INFO(this->get_logger(), "Pose tracking failed with status: %d", static_cast<int>(tracking_status));
|
||||
}
|
||||
RCLCPP_INFO_STREAM(m_node->get_logger(), "Pose tracker exited with status: "
|
||||
<< moveit_servo::POSE_TRACKING_STATUS_CODE_MAP.at(tracking_status));
|
||||
}
|
||||
}; // class MoveCartesianActionServer
|
||||
|
||||
} // namespace rbs_skill_actions
|
||||
|
||||
int main(int argc, char **argv) {
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::NodeOptions node_options;
|
||||
// node_options.automatically_declare_parameters_from_overrides(true);
|
||||
node_options.allow_undeclared_parameters();
|
||||
auto node = rclcpp::Node::make_shared("move_servo", "", node_options);
|
||||
|
||||
rbs_skill_actions::MoveServoActionServer server(node);
|
||||
std::thread run_server([&server]() {
|
||||
rclcpp::sleep_for(std::chrono::seconds(3));
|
||||
server.init();
|
||||
});
|
||||
|
||||
rclcpp::spin(node);
|
||||
run_server.join();
|
||||
|
||||
return 0;
|
||||
}
|
||||
Loading…
Add table
Add a link
Reference in a new issue