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

Action Server Fix (Incomplete)

Browse source
This commit is contained in:
Ilya Uraev 2022-02-07 18:34:33 +04:00
parent 7ca3fbeb1d
commit 7b73c48f03
6 changed files with 43 additions and 28 deletions
Download patch file Download diff file Expand all files Collapse all files

40
robossembler_servers/src/move_topose_action_server.cpp
View file

@ -18,6 +18,7 @@
// moveit libs
#include "moveit/move_group_interface/move_group_interface.h"
#include "moveit/planning_interface/planning_interface.h"
/*
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
@ -39,12 +40,14 @@ public:
using MoveitSendPose = robossembler_interfaces::action::MoveitSendPose;
//explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr& node)
explicit MoveToPoseActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
: Node("move_topose_action_server", options)//, node_(node)
explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr &node) : Node("move_topose"), node_(node)
{
using namespace std::placeholders;
this->action_server_ = rclcpp_action::create_server<MoveitSendPose>(
this,
this->get_node_base_interface(),
this->get_node_clock_interface(),
this->get_node_logging_interface(),
this->get_node_waitables_interface(),
"move_topose",
std::bind(&MoveToPoseActionServer::goal_callback, this, _1, _2),
std::bind(&MoveToPoseActionServer::cancel_callback, this, _1),
@ -56,12 +59,13 @@ private:
rclcpp_action::Server<MoveitSendPose>::SharedPtr action_server_;
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, goal->target_pose.pose.position.x, goal->target_pose.pose.position.y, goal->target_pose.pose.position.z,
goal->robot_name.c_str(), goal->target_pose.pose.position.x, goal->target_pose.pose.position.y, goal->target_pose.pose.position.z,
goal->target_pose.pose.orientation.x, goal->target_pose.pose.orientation.y, goal->target_pose.pose.orientation.z,
goal->target_pose.pose.orientation.w);
(void)uuid;
@ -89,15 +93,18 @@ private:
RCLCPP_INFO(this->get_logger(), "Executing goal");
const auto goal = goal_handle->get_goal();
auto result = std::make_shared<MoveitSendPose::Result>();
RCLCPP_INFO(this->get_logger(), "Goal and Result Initialised");
moveit::planning_interface::MoveGroupInterface::Options move_group_options(goal->robot_name, "robot_description");
RCLCPP_INFO(this->get_logger(), "move_group_options");
moveit::planning_interface::MoveGroupInterface move_group_interface(node_, goal->robot_name);
moveit::core::RobotState start_state(*move_group_interface.getCurrentState());
RCLCPP_INFO(this->get_logger(), "Moveit2 Initialised");
move_group_interface.setStartState(start_state);
move_group_interface.setPoseTarget(goal->target_pose.pose);
//move_group_interface.setStartStateToCurrentState();
move_group_interface.setPoseTarget(goal->target_pose);
move_group_interface.setMaxVelocityScalingFactor(goal->end_effector_velocity);
move_group_interface.setMaxAccelerationScalingFactor(goal->end_effector_acceleration);
RCLCPP_INFO(this->get_logger(), "move_group ready to plan");
moveit::planning_interface::MoveGroupInterface::Plan plan;
bool success = (move_group_interface.plan(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
@ -124,16 +131,19 @@ private:
}// namespace robossembler_actions
RCLCPP_COMPONENTS_REGISTER_NODE(robossembler_actions::MoveToPoseActionServer)
/*
int main(int argc, char * argv[])
int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
rclcpp::NodeOptions node_options;
node_options.automatically_declare_parameters_from_overrides(true);
rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("move_topose", "", node_options);
rclcpp::spin(node);
//rclcpp::shutdown();
auto node = rclcpp::Node::make_shared("move_topose", node_options);
auto action_server = std::make_shared<robossembler_actions::MoveToPoseActionServer>(node);
//rclcpp::spin(action_server);
rclcpp::executors::MultiThreadedExecutor executor;
executor.add_node(action_server);
executor.spin();
return 0;
}*/
}