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

♻️ update to moveit2 ros2 actions bt

Browse source
This commit is contained in:
Ilya Uraev 2022-02-10 23:15:37 +04:00
parent 7b73c48f03
commit 54da56c7ae
18 changed files with 305 additions and 742 deletions
Download patch file Download diff file Expand all files Collapse all files

72
robossembler_servers/src/move_topose_action_server.cpp
View file

@ -40,18 +40,34 @@ public:
using MoveitSendPose = robossembler_interfaces::action::MoveitSendPose;
//explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr& node)
explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr &node) : Node("move_topose"), node_(node)
explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr &node) : Node("move_topose_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(),
// 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(&MoveToPoseActionServer::goal_callback, this, _1, _2),
// std::bind(&MoveToPoseActionServer::cancel_callback, this, _1),
// std::bind(&MoveToPoseActionServer::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_topose",
std::bind(&MoveToPoseActionServer::goal_callback, this, _1, _2),
std::bind(&MoveToPoseActionServer::cancel_callback, this, _1),
std::bind(&MoveToPoseActionServer::accepted_callback, this, _1));
std::bind(&MoveToPoseActionServer::goal_callback, this, std::placeholders::_1, std::placeholders::_2),
std::bind(&MoveToPoseActionServer::cancel_callback, this, std::placeholders::_1),
std::bind(&MoveToPoseActionServer::accepted_callback, this, std::placeholders::_1)
);
}
private:
@ -65,9 +81,9 @@ private:
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.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);
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;
if (false) {
return rclcpp_action::GoalResponse::REJECT;
@ -90,21 +106,15 @@ private:
void execute(const std::shared_ptr<ServerGoalHandle> goal_handle)
{
RCLCPP_INFO(this->get_logger(), "Executing goal");
RCLCPP_INFO(this->get_logger(), "Executing action 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.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);
@ -119,13 +129,13 @@ private:
if (goal_handle->is_canceling()) {
goal_handle->canceled(result);
RCLCPP_ERROR(this->get_logger(), "Goal Canceled");
RCLCPP_ERROR(this->get_logger(), "Action goal canceled");
return;
}
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "Successfully executed goal");
RCLCPP_INFO(this->get_logger(), "Successfully executed action goal");
}
}; // class MoveToPoseActionServer
@ -136,14 +146,16 @@ 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_topose", node_options);
auto action_server = std::make_shared<robossembler_actions::MoveToPoseActionServer>(node);
auto node = rclcpp::Node::make_shared("move_topose", "", node_options);
//rclcpp::spin(action_server);
robossembler_actions::MoveToPoseActionServer server(node);
std::thread run_server([&server]() {
rclcpp::sleep_for(std::chrono::seconds(3));
server.init();
});
rclcpp::executors::MultiThreadedExecutor executor;
executor.add_node(action_server);
executor.spin();
rclcpp::spin(node);
run_server.join();
return 0;
}