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

Add minimal pick-and-place case with world embedded

Browse source
This commit is contained in:
Igor Brylyov 2023-03-31 20:24:56 +00:00
parent 209e99a4b3
commit a87fb8a7ec
64 changed files with 2419 additions and 275 deletions
Download patch file Download diff file Expand all files Collapse all files

33
rbs_skill_servers/src/move_topose_action_server.cpp
View file

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