clang-format
This commit is contained in:
parent
18d2772cef
commit
6eee02bacc
17 changed files with 1299 additions and 1346 deletions
|
|
@ -7,9 +7,9 @@
|
|||
#include "rclcpp_components/register_node_macro.hpp"
|
||||
|
||||
// action libs
|
||||
#include "rclcpp_action/rclcpp_action.hpp"
|
||||
#include "rbs_skill_interfaces/msg/action_feedback_status_constants.hpp"
|
||||
#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"
|
||||
|
|
@ -26,156 +26,156 @@
|
|||
#include <tf2_ros/transform_listener.h>
|
||||
*/
|
||||
// For Visualization
|
||||
//#include <eigen_conversions/eigen_msg.h>
|
||||
// #include <eigen_conversions/eigen_msg.h>
|
||||
#include "moveit_msgs/action/move_group.hpp"
|
||||
#include "moveit_msgs/msg/display_robot_state.hpp"
|
||||
#include "moveit_msgs/msg/display_trajectory.hpp"
|
||||
#include "moveit_msgs/msg/robot_trajectory.hpp"
|
||||
#include "moveit_msgs/action/move_group.hpp"
|
||||
|
||||
namespace rbs_skill_actions
|
||||
{
|
||||
namespace rbs_skill_actions {
|
||||
|
||||
class MoveCartesianActionServer : public rclcpp::Node
|
||||
{
|
||||
class MoveCartesianActionServer : public rclcpp::Node {
|
||||
public:
|
||||
using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose;
|
||||
using MoveitSendPose = rbs_skill_interfaces::action::MoveitSendPose;
|
||||
|
||||
//explicit MoveCartesianActionServer(const rclcpp::Node::SharedPtr& node)
|
||||
explicit MoveCartesianActionServer(const rclcpp::Node::SharedPtr &node) : Node("move_cartesian_action_server"), node_(node)
|
||||
{
|
||||
// using namespace std::placeholders;
|
||||
// this->action_server_ = rclcpp_action::create_server<MoveitSendPose>(
|
||||
// this->get_node_base_interface(),
|
||||
// this->get_node_clock_interface(),
|
||||
// this->get_node_logging_interface(),
|
||||
// this->get_node_waitables_interface(),
|
||||
// "move_topose",
|
||||
// std::bind(&MoveCartesianActionServer::goal_callback, this, _1, _2),
|
||||
// std::bind(&MoveCartesianActionServer::cancel_callback, this, _1),
|
||||
// std::bind(&MoveCartesianActionServer::accepted_callback, this, _1));
|
||||
}
|
||||
// explicit MoveCartesianActionServer(const rclcpp::Node::SharedPtr& node)
|
||||
explicit MoveCartesianActionServer(const rclcpp::Node::SharedPtr &node)
|
||||
: Node("move_cartesian_action_server"), node_(node) {
|
||||
// using namespace std::placeholders;
|
||||
// this->action_server_ = rclcpp_action::create_server<MoveitSendPose>(
|
||||
// this->get_node_base_interface(),
|
||||
// this->get_node_clock_interface(),
|
||||
// this->get_node_logging_interface(),
|
||||
// this->get_node_waitables_interface(),
|
||||
// "move_topose",
|
||||
// std::bind(&MoveCartesianActionServer::goal_callback, this, _1, _2),
|
||||
// std::bind(&MoveCartesianActionServer::cancel_callback, this, _1),
|
||||
// std::bind(&MoveCartesianActionServer::accepted_callback, this, _1));
|
||||
}
|
||||
|
||||
void init()
|
||||
{
|
||||
|
||||
action_server_ = rclcpp_action::create_server<MoveitSendPose>(
|
||||
node_->get_node_base_interface(),
|
||||
node_->get_node_clock_interface(),
|
||||
node_->get_node_logging_interface(),
|
||||
node_->get_node_waitables_interface(),
|
||||
"move_cartesian",
|
||||
std::bind(&MoveCartesianActionServer::goal_callback, this, std::placeholders::_1, std::placeholders::_2),
|
||||
std::bind(&MoveCartesianActionServer::cancel_callback, this, std::placeholders::_1),
|
||||
std::bind(&MoveCartesianActionServer::accepted_callback, this, std::placeholders::_1)
|
||||
);
|
||||
void init() {
|
||||
|
||||
}
|
||||
action_server_ = rclcpp_action::create_server<MoveitSendPose>(
|
||||
node_->get_node_base_interface(), node_->get_node_clock_interface(),
|
||||
node_->get_node_logging_interface(),
|
||||
node_->get_node_waitables_interface(), "move_cartesian",
|
||||
std::bind(&MoveCartesianActionServer::goal_callback, this,
|
||||
std::placeholders::_1, std::placeholders::_2),
|
||||
std::bind(&MoveCartesianActionServer::cancel_callback, this,
|
||||
std::placeholders::_1),
|
||||
std::bind(&MoveCartesianActionServer::accepted_callback, this,
|
||||
std::placeholders::_1));
|
||||
}
|
||||
|
||||
private:
|
||||
rclcpp::Node::SharedPtr node_;
|
||||
rclcpp_action::Server<MoveitSendPose>::SharedPtr action_server_;
|
||||
rclcpp::Node::SharedPtr node_;
|
||||
rclcpp_action::Server<MoveitSendPose>::SharedPtr action_server_;
|
||||
|
||||
using ServerGoalHandle = rclcpp_action::ServerGoalHandle<MoveitSendPose>;
|
||||
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;
|
||||
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;
|
||||
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(&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) {
|
||||
RCLCPP_INFO(this->get_logger(), "Executing action goal");
|
||||
const auto goal = goal_handle->get_goal();
|
||||
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());
|
||||
|
||||
std::vector<geometry_msgs::msg::Pose> waypoints;
|
||||
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]",
|
||||
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);
|
||||
if (fraction > 0) {
|
||||
RCLCPP_INFO(this->get_logger(), "Planning success");
|
||||
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);
|
||||
}
|
||||
|
||||
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(&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)
|
||||
{
|
||||
RCLCPP_INFO(this->get_logger(), "Executing action goal");
|
||||
const auto goal = goal_handle->get_goal();
|
||||
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());
|
||||
|
||||
std::vector<geometry_msgs::msg::Pose> waypoints;
|
||||
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]",
|
||||
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);
|
||||
if(fraction>0)
|
||||
{
|
||||
RCLCPP_INFO(this->get_logger(), "Planning success");
|
||||
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()) {
|
||||
goal_handle->canceled(result);
|
||||
RCLCPP_ERROR(this->get_logger(), "Action goal canceled");
|
||||
return;
|
||||
}
|
||||
|
||||
if (goal_handle->is_canceling()) {
|
||||
goal_handle->canceled(result);
|
||||
RCLCPP_ERROR(this->get_logger(), "Action goal canceled");
|
||||
return;
|
||||
}
|
||||
}
|
||||
}; // class MoveCartesianActionServer
|
||||
|
||||
}// namespace rbs_skill_actions
|
||||
} // 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);
|
||||
auto node = rclcpp::Node::make_shared("move_cartesian", "", node_options);
|
||||
int main(int argc, char **argv) {
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::NodeOptions node_options;
|
||||
node_options.automatically_declare_parameters_from_overrides(true);
|
||||
auto node = rclcpp::Node::make_shared("move_cartesian", "", node_options);
|
||||
|
||||
rbs_skill_actions::MoveCartesianActionServer server(node);
|
||||
std::thread run_server([&server]() {
|
||||
rclcpp::sleep_for(std::chrono::seconds(3));
|
||||
server.init();
|
||||
});
|
||||
rbs_skill_actions::MoveCartesianActionServer server(node);
|
||||
std::thread run_server([&server]() {
|
||||
rclcpp::sleep_for(std::chrono::seconds(3));
|
||||
server.init();
|
||||
});
|
||||
|
||||
rclcpp::spin(node);
|
||||
run_server.join();
|
||||
rclcpp::spin(node);
|
||||
run_server.join();
|
||||
|
||||
return 0;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue