Add minimal pick-and-place case with world embedded
This commit is contained in:
parent
209e99a4b3
commit
a87fb8a7ec
64 changed files with 2419 additions and 275 deletions
71
rbs_skill_servers/src/add_planning_scene_objects_service.cpp
Normal file
71
rbs_skill_servers/src/add_planning_scene_objects_service.cpp
Normal file
|
|
@ -0,0 +1,71 @@
|
|||
#include <cinttypes>
|
||||
#include <memory>
|
||||
#include <algorithm>
|
||||
#include <moveit/planning_scene_interface/planning_scene_interface.h>
|
||||
#include "rbs_skill_interfaces/srv/add_planning_scene_object.hpp"
|
||||
#include "moveit/move_group_interface/move_group_interface.h"
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
using AddPlanningSceneObject = rbs_skill_interfaces::srv::AddPlanningSceneObject;
|
||||
rclcpp::Node::SharedPtr g_node = nullptr;
|
||||
|
||||
void handle_service(
|
||||
const std::shared_ptr<rmw_request_id_t> request_header,
|
||||
const std::shared_ptr<AddPlanningSceneObject::Request> request,
|
||||
const std::shared_ptr<AddPlanningSceneObject::Response> response)
|
||||
{
|
||||
(void)request_header;
|
||||
// Create collision object for the robot to avoid
|
||||
auto const collision_object = [frame_id =
|
||||
"world", object_name=request->object_id, object_pose=request->object_pose] {
|
||||
moveit_msgs::msg::CollisionObject collision_object;
|
||||
collision_object.header.frame_id = frame_id;
|
||||
collision_object.id = object_name;
|
||||
shape_msgs::msg::SolidPrimitive primitive;
|
||||
|
||||
// Define the size of the box in meters
|
||||
primitive.type = primitive.BOX;
|
||||
primitive.dimensions.resize(3);
|
||||
primitive.dimensions[primitive.BOX_X] = 0.05;
|
||||
primitive.dimensions[primitive.BOX_Y] = 0.05;
|
||||
primitive.dimensions[primitive.BOX_Z] = 0.05;
|
||||
|
||||
// Define the pose of the box (relative to the frame_id)
|
||||
geometry_msgs::msg::Pose box_pose;
|
||||
box_pose.position.x = object_pose[0];
|
||||
box_pose.position.y = object_pose[1];
|
||||
box_pose.position.z = object_pose[2];
|
||||
box_pose.orientation.x = object_pose[3];
|
||||
box_pose.orientation.y = object_pose[4];
|
||||
box_pose.orientation.z = object_pose[5];
|
||||
box_pose.orientation.w = object_pose[6];
|
||||
|
||||
|
||||
|
||||
collision_object.primitives.push_back(primitive);
|
||||
collision_object.primitive_poses.push_back(box_pose);
|
||||
collision_object.operation = collision_object.ADD;
|
||||
|
||||
return collision_object;
|
||||
}();
|
||||
|
||||
// Add the collision object to the scene
|
||||
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
|
||||
planning_scene_interface.applyCollisionObject(collision_object);
|
||||
|
||||
response->success = true;
|
||||
}
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::NodeOptions node_options;
|
||||
node_options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true);
|
||||
g_node = rclcpp::Node::make_shared("add_planing_scene_object", "", node_options);
|
||||
auto server = g_node->create_service<AddPlanningSceneObject>("add_planing_scene_object_service", handle_service);
|
||||
rclcpp::spin(g_node);
|
||||
rclcpp::shutdown();
|
||||
g_node = nullptr;
|
||||
return 0;
|
||||
}
|
||||
71
rbs_skill_servers/src/get_grasp_pick_pose.cpp
Normal file
71
rbs_skill_servers/src/get_grasp_pick_pose.cpp
Normal file
|
|
@ -0,0 +1,71 @@
|
|||
#include <cinttypes>
|
||||
#include <memory>
|
||||
#include <algorithm>
|
||||
#include "rbs_skill_interfaces/srv/get_pick_place_poses.hpp"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
using GetGraspPlacePose = rbs_skill_interfaces::srv::GetPickPlacePoses;
|
||||
rclcpp::Node::SharedPtr g_node = nullptr;
|
||||
std::string pick_next_object(std::vector<std::string> scene_objects, std::string current_object_name);
|
||||
|
||||
void handle_service(
|
||||
const std::shared_ptr<rmw_request_id_t> request_header,
|
||||
const std::shared_ptr<GetGraspPlacePose::Request> request,
|
||||
const std::shared_ptr<GetGraspPlacePose::Response> response)
|
||||
{
|
||||
(void)request_header;
|
||||
RCLCPP_INFO(
|
||||
g_node->get_logger(),"GetPoseCallback");
|
||||
std::vector<std::string> param_names = {
|
||||
request->object_name + ".GraspPose",
|
||||
request->object_name + ".PreGraspPose",
|
||||
request->object_name + ".PostGraspPose",
|
||||
request->object_name + ".PlacePose",
|
||||
request->object_name + ".PrePlacePose",
|
||||
request->object_name + ".PostPlacePose",
|
||||
request->object_name + ".PostGraspPose2",
|
||||
"pre_place_joint_states",
|
||||
"scene_objects"
|
||||
};
|
||||
std::vector<rclcpp::Parameter> params = g_node->get_parameters(param_names);
|
||||
for (auto ¶m : params)
|
||||
{
|
||||
RCLCPP_INFO(g_node->get_logger(), "param name: %s, value: %s",
|
||||
param.get_name().c_str(), param.value_to_string().c_str());
|
||||
}
|
||||
response->grasp_pose = params[0].as_double_array();
|
||||
response->pre_grasp_pose = params[1].as_double_array();
|
||||
response->post_grasp_pose = params[2].as_double_array();
|
||||
response->place_pose = params[3].as_double_array();
|
||||
response->pre_place_pose = params[4].as_double_array();
|
||||
response->post_place_pose = params[5].as_double_array();
|
||||
response->post_grasp_pose_d = params[6].as_double_array();
|
||||
response->pre_place_joint_state = params[7].as_double_array();
|
||||
response->next_object = pick_next_object(params[8].as_string_array(), request->object_name);
|
||||
}
|
||||
|
||||
std::string pick_next_object(std::vector<std::string> scene_objects, std::string current_object_name)
|
||||
{
|
||||
std::string next_object_name{};
|
||||
auto current_object_idx = std::find(scene_objects.begin(), scene_objects.end(), current_object_name);
|
||||
if (!(current_object_idx == scene_objects.end()))
|
||||
{
|
||||
next_object_name = *std::next(current_object_idx, 1);
|
||||
} else {
|
||||
next_object_name = "It is last object";
|
||||
}
|
||||
return next_object_name;
|
||||
}
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::NodeOptions node_options;
|
||||
node_options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true);
|
||||
g_node = rclcpp::Node::make_shared("get_grasp_pick_pose", "", node_options);
|
||||
auto server = g_node->create_service<GetGraspPlacePose>("get_pick_place_pose_service", handle_service);
|
||||
rclcpp::spin(g_node);
|
||||
rclcpp::shutdown();
|
||||
g_node = nullptr;
|
||||
return 0;
|
||||
}
|
||||
114
rbs_skill_servers/src/gripper_control_action_server.cpp
Normal file
114
rbs_skill_servers/src/gripper_control_action_server.cpp
Normal file
|
|
@ -0,0 +1,114 @@
|
|||
#include <functional>
|
||||
#include <memory>
|
||||
#include <thread>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "rclcpp/timer.hpp"
|
||||
#include "rclcpp_components/register_node_macro.hpp"
|
||||
|
||||
// action libs
|
||||
#include "rclcpp_action/rclcpp_action.hpp"
|
||||
#include "rbs_skill_interfaces/action/gripper_command.hpp"
|
||||
|
||||
#include "std_msgs/msg/float64_multi_array.hpp"
|
||||
#include "sensor_msgs/msg/joint_state.hpp"
|
||||
|
||||
|
||||
|
||||
namespace rbs_skill_actions {
|
||||
|
||||
class GripperControlActionServer: public rclcpp::Node {
|
||||
|
||||
public:
|
||||
using GripperCommand = rbs_skill_interfaces::action::GripperCommand;
|
||||
explicit GripperControlActionServer(rclcpp::NodeOptions options): Node("gripper_control_action_server", options.allow_undeclared_parameters(true))
|
||||
{
|
||||
this->actionServer_ = rclcpp_action::create_server<GripperCommand>(
|
||||
this,
|
||||
"gripper_control",
|
||||
std::bind(&GripperControlActionServer::goal_callback, this, std::placeholders::_1, std::placeholders::_2),
|
||||
std::bind(&GripperControlActionServer::cancel_callback, this, std::placeholders::_1),
|
||||
std::bind(&GripperControlActionServer::accepted_callback, this, std::placeholders::_1)
|
||||
);
|
||||
publisher = this->create_publisher<std_msgs::msg::Float64MultiArray>("/gripper_controller/commands", 10);
|
||||
join_state_subscriber = this->create_subscription<sensor_msgs::msg::JointState>(
|
||||
"/joint_states",10, std::bind(&GripperControlActionServer::joint_state_callback, this, std::placeholders::_1));
|
||||
}
|
||||
|
||||
|
||||
private:
|
||||
|
||||
rclcpp_action::Server<GripperCommand>::SharedPtr actionServer_;
|
||||
rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr publisher;
|
||||
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr join_state_subscriber;
|
||||
double gripper_joint_state{1.0};
|
||||
|
||||
using ServerGoalHandle = rclcpp_action::ServerGoalHandle<GripperCommand>;
|
||||
|
||||
void joint_state_callback(const sensor_msgs::msg::JointState & msg)
|
||||
{
|
||||
gripper_joint_state = msg.position.back();
|
||||
}
|
||||
|
||||
rclcpp_action::GoalResponse goal_callback(const rclcpp_action::GoalUUID& uuid, std::shared_ptr<const GripperCommand::Goal> goal) {
|
||||
|
||||
RCLCPP_INFO(this->get_logger(),"goal request recieved for gripper [%.2f m]", goal->position);
|
||||
(void)uuid;
|
||||
if(goal->position > 0.9 or goal->position < 0) {
|
||||
return rclcpp_action::GoalResponse::REJECT;
|
||||
}
|
||||
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 request to cancel goal");
|
||||
(void)goal_handle;
|
||||
return rclcpp_action::CancelResponse::ACCEPT;
|
||||
}
|
||||
|
||||
void accepted_callback(const std::shared_ptr<ServerGoalHandle> goal_handle) {
|
||||
std::thread{std::bind(&GripperControlActionServer::execute, this, std::placeholders::_1), goal_handle}.detach();
|
||||
}
|
||||
|
||||
void execute(const std::shared_ptr<ServerGoalHandle> goal_handle){
|
||||
|
||||
const auto goal = goal_handle->get_goal();
|
||||
auto result = std::make_shared<GripperCommand::Result>();
|
||||
auto feedback = std::make_shared<GripperCommand::Feedback>();
|
||||
|
||||
if (goal_handle->is_canceling()) {
|
||||
goal_handle->canceled(result);
|
||||
RCLCPP_ERROR(this->get_logger(), "Goal Canceled");
|
||||
return;
|
||||
}
|
||||
RCLCPP_INFO(this->get_logger(), "Current gripper state %f", gripper_joint_state);
|
||||
|
||||
std_msgs::msg::Float64MultiArray command;
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
command.data.push_back(goal->position);
|
||||
RCLCPP_INFO(this->get_logger(),"Publishing goal to gripper");
|
||||
publisher->publish(command);
|
||||
std::this_thread::sleep_for(3s);
|
||||
|
||||
|
||||
goal_handle->succeed(result);
|
||||
RCLCPP_INFO(this->get_logger(), "Goal successfully completed");
|
||||
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
|
||||
RCLCPP_COMPONENTS_REGISTER_NODE(rbs_skill_actions::GripperControlActionServer);
|
||||
|
||||
|
||||
// int main(int argc, char ** argv) {
|
||||
// rclcpp::init(argc, argv);
|
||||
// rbs_skill_actions::GripperControlActionServer server;
|
||||
// rclcpp::spin(server);
|
||||
// return 0;
|
||||
// }
|
||||
|
|
@ -86,9 +86,7 @@ private:
|
|||
goal->target_pose.orientation.x, goal->target_pose.orientation.y, goal->target_pose.orientation.z,
|
||||
goal->target_pose.orientation.w);
|
||||
(void)uuid;
|
||||
if (false) {
|
||||
return rclcpp_action::GoalResponse::REJECT;
|
||||
}
|
||||
|
||||
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
|
||||
}
|
||||
|
||||
|
|
@ -103,6 +101,10 @@ private:
|
|||
{
|
||||
using namespace std::placeholders;
|
||||
std::thread(std::bind(&MoveCartesianActionServer::execute, this, _1), goal_handle).detach();
|
||||
// std::thread(
|
||||
// [this, goal_handle]() {
|
||||
// execute(goal_handle);
|
||||
// }).detach();
|
||||
}
|
||||
|
||||
void execute(const std::shared_ptr<ServerGoalHandle> goal_handle)
|
||||
|
|
@ -112,22 +114,15 @@ private:
|
|||
auto result = std::make_shared<MoveitSendPose::Result>();
|
||||
|
||||
moveit::planning_interface::MoveGroupInterface move_group_interface(node_, goal->robot_name);
|
||||
|
||||
move_group_interface.startStateMonitor();
|
||||
|
||||
moveit::core::RobotState start_state(*move_group_interface.getCurrentState());
|
||||
auto start_pose = move_group_interface.getCurrentPose();
|
||||
|
||||
std::vector<geometry_msgs::msg::Pose> waypoints;
|
||||
RCLCPP_INFO(this->get_logger(), "Current with Pose [%f, %f, %f, %f, %f, %f, %f]",
|
||||
start_pose.pose.position.x, start_pose.pose.position.y, start_pose.pose.position.z,
|
||||
start_pose.pose.orientation.x, start_pose.pose.orientation.y, start_pose.pose.orientation.z,
|
||||
start_pose.pose.orientation.w);
|
||||
//waypoints.push_back(start_pose.pose);
|
||||
geometry_msgs::msg::Pose target_pose = start_pose.pose;
|
||||
//goal->target_pose
|
||||
target_pose.position.z = target_pose.position.z + goal->target_pose.position.z;
|
||||
target_pose.position.y = target_pose.position.y + goal->target_pose.position.y;
|
||||
target_pose.position.x = target_pose.position.x + goal->target_pose.position.x;
|
||||
auto current_pose = move_group_interface.getCurrentPose();
|
||||
//waypoints.push_back(current_pose.pose);
|
||||
geometry_msgs::msg::Pose target_pose = goal->target_pose;
|
||||
//target_pose.position = goal->target_pose.position;
|
||||
waypoints.push_back(target_pose);
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "New cartesian target pose [%f, %f, %f]",
|
||||
|
|
@ -135,16 +130,25 @@ private:
|
|||
//waypoints.push_back(start_pose.pose);
|
||||
moveit_msgs::msg::RobotTrajectory trajectory;
|
||||
const double jump_threshold = 0.0;
|
||||
const double eef_step = 0.01;
|
||||
const double eef_step = 0.001;
|
||||
double fraction = move_group_interface.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
|
||||
waypoints.clear();
|
||||
if(fraction>0)
|
||||
{
|
||||
RCLCPP_INFO(this->get_logger(), "Planning success");
|
||||
move_group_interface.execute(trajectory);
|
||||
moveit::core::MoveItErrorCode execute_err_code = move_group_interface.execute(trajectory);
|
||||
if (execute_err_code == moveit::core::MoveItErrorCode::SUCCESS)
|
||||
{
|
||||
goal_handle->succeed(result);
|
||||
RCLCPP_INFO(this->get_logger(), "Successfully executed action goal");
|
||||
}else if (execute_err_code == moveit::core::MoveItErrorCode::FAILURE)
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Failure in move:");
|
||||
}
|
||||
|
||||
//move_group_interface.move();
|
||||
}else{
|
||||
RCLCPP_WARN(this->get_logger(), "Failed to generate plan");
|
||||
goal_handle->abort(result);
|
||||
}
|
||||
|
||||
if (goal_handle->is_canceling()) {
|
||||
|
|
@ -152,10 +156,6 @@ private:
|
|||
RCLCPP_ERROR(this->get_logger(), "Action goal canceled");
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
goal_handle->succeed(result);
|
||||
RCLCPP_INFO(this->get_logger(), "Successfully executed action goal");
|
||||
}
|
||||
}; // class MoveCartesianActionServer
|
||||
|
||||
|
|
|
|||
|
|
@ -1,6 +1,8 @@
|
|||
#include <functional>
|
||||
#include <memory>
|
||||
#include <thread>
|
||||
#include <cinttypes>
|
||||
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "rclcpp/timer.hpp"
|
||||
|
|
@ -107,17 +109,16 @@ private:
|
|||
auto result = std::make_shared<MoveitSendJointStates::Result>();
|
||||
|
||||
moveit::planning_interface::MoveGroupInterface move_group_interface(node_, goal->robot_name);
|
||||
move_group_interface.startStateMonitor();
|
||||
|
||||
const moveit::core::JointModelGroup* joint_model_group =
|
||||
move_group_interface.getCurrentState()->getJointModelGroup(goal->robot_name);
|
||||
moveit::core::RobotStatePtr current_state = move_group_interface.getCurrentState(10);
|
||||
|
||||
std::vector<double> joint_group_positions;
|
||||
current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);
|
||||
|
||||
joint_group_positions[0] = goal->joint_value;
|
||||
joint_group_positions[1] = goal->joint_value;
|
||||
move_group_interface.setJointValueTarget(joint_group_positions);
|
||||
std::vector<double> joint_states = goal->joint_values;
|
||||
for (auto &joint : joint_states)
|
||||
{
|
||||
RCLCPP_INFO(this->get_logger(), "Joint value %f", joint);
|
||||
}
|
||||
|
||||
|
||||
move_group_interface.setJointValueTarget(goal->joint_values);
|
||||
move_group_interface.setMaxVelocityScalingFactor(goal->joints_velocity_scaling_factor);
|
||||
move_group_interface.setMaxAccelerationScalingFactor(goal->joints_acceleration_scaling_factor);
|
||||
|
||||
|
|
@ -128,7 +129,7 @@ private:
|
|||
{
|
||||
RCLCPP_INFO(this->get_logger(), "Planning success");
|
||||
move_group_interface.execute(plan);
|
||||
move_group_interface.move();
|
||||
//move_group_interface.move();
|
||||
}else{
|
||||
RCLCPP_WARN(this->get_logger(), "Failed to generate plan");
|
||||
}
|
||||
|
|
|
|||
|
|
@ -19,6 +19,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/planning_scene_interface/planning_scene_interface.h"
|
||||
|
||||
/*
|
||||
#include <tf2/LinearMath/Quaternion.h>
|
||||
|
|
@ -27,6 +28,7 @@
|
|||
*/
|
||||
// For Visualization
|
||||
//#include <eigen_conversions/eigen_msg.h>
|
||||
#include <moveit_msgs/msg/display_robot_state.hpp>
|
||||
#include "moveit_msgs/msg/display_robot_state.hpp"
|
||||
#include "moveit_msgs/msg/display_trajectory.hpp"
|
||||
#include "moveit_msgs/msg/robot_trajectory.hpp"
|
||||
|
|
@ -112,21 +114,34 @@ private:
|
|||
auto result = std::make_shared<MoveitSendPose::Result>();
|
||||
|
||||
moveit::planning_interface::MoveGroupInterface move_group_interface(node_, goal->robot_name);
|
||||
|
||||
move_group_interface.setStartState(*move_group_interface.getCurrentState());
|
||||
move_group_interface.startStateMonitor();
|
||||
|
||||
move_group_interface.setPoseTarget(goal->target_pose);
|
||||
move_group_interface.setMaxVelocityScalingFactor(goal->end_effector_velocity);
|
||||
move_group_interface.setMaxAccelerationScalingFactor(goal->end_effector_acceleration);
|
||||
|
||||
moveit::planning_interface::MoveGroupInterface::Plan plan;
|
||||
bool success = (move_group_interface.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS);
|
||||
if(success)
|
||||
moveit::core::MoveItErrorCode plan_err_code = move_group_interface.plan(plan);
|
||||
if (plan_err_code == moveit::core::MoveItErrorCode::INVALID_MOTION_PLAN){
|
||||
move_group_interface.plan(plan);
|
||||
}
|
||||
if(plan_err_code == moveit::core::MoveItErrorCode::SUCCESS)
|
||||
{
|
||||
RCLCPP_INFO(this->get_logger(), "Planning success");
|
||||
//move_group_interface.execute(plan);
|
||||
move_group_interface.move();
|
||||
moveit::core::MoveItErrorCode move_err_code = move_group_interface.execute(plan);
|
||||
if (move_err_code == moveit::core::MoveItErrorCode::SUCCESS)
|
||||
{
|
||||
goal_handle->succeed(result);
|
||||
RCLCPP_INFO(this->get_logger(), "Successfully executed action goal");
|
||||
} else if (move_err_code == moveit::core::MoveItErrorCode::FAILURE)
|
||||
{
|
||||
RCLCPP_ERROR(this->get_logger(), "Failure in move:");
|
||||
}
|
||||
|
||||
|
||||
}else{
|
||||
RCLCPP_WARN(this->get_logger(), "Failed to generate plan");
|
||||
RCLCPP_WARN_STREAM(this->get_logger(), "Failed to generate plan because " << error_code_to_string(plan_err_code));
|
||||
goal_handle->abort(result);
|
||||
}
|
||||
|
||||
if (goal_handle->is_canceling()) {
|
||||
|
|
@ -134,10 +149,6 @@ private:
|
|||
RCLCPP_ERROR(this->get_logger(), "Action goal canceled");
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
goal_handle->succeed(result);
|
||||
RCLCPP_INFO(this->get_logger(), "Successfully executed action goal");
|
||||
}
|
||||
}; // class MoveToPoseActionServer
|
||||
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue