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

refactor(MoveToPose): add input ports try_plan_untill_success and allowed_planning time

Browse source
This commit is contained in:
Ilya Uraev 2025-04-08 14:43:33 +03:00
parent 0bfa17c30b
commit 698f8734ac
1 changed files with 30 additions and 7 deletions
Download patch file Download diff file Expand all files Collapse all files

37
rbs_bt_executor/src/MoveToPose.cpp
View file

@ -1,5 +1,6 @@
#include "behaviortree_ros2/bt_action_node.hpp"
#include "rbs_skill_interfaces/action/moveit_send_pose.hpp"
#include <behaviortree_cpp/basic_types.h>
#include <behaviortree_cpp/tree_node.h>
#include <behaviortree_ros2/plugins.hpp>
#include <behaviortree_ros2/ros_node_params.hpp>
@ -18,28 +19,50 @@ public:
}
static BT::PortsList providedPorts() {
return providedBasicPorts(
{BT::InputPort<std::string>("robot_name"),
BT::InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("pose"),
BT::InputPort<double>("duration")});
return providedBasicPorts({
BT::InputPort<std::string>("robot_name"),
BT::InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("pose"),
BT::InputPort<double>("duration"),
BT::InputPort<bool>("try_plan_untill_success", false, "Replan if failure to plan"),
BT::InputPort<double>("allowed_planning_time", 5.0, "Maximal time to plan"),
});
}
bool setGoal(RosActionNode::Goal &goal) override {
RCLCPP_INFO(this->logger(), "[%s] Starting send goal [%s]", name().c_str(),
this->action_name_.c_str());
if (!getInput("robot_name", goal.robot_name)) {
RCLCPP_FATAL(this->logger(), "[%s] Could not get robot_name from input port", name().c_str());
RCLCPP_FATAL(this->logger(),
"[%s] Could not get robot_name from input port",
name().c_str());
return false;
}
if (!getInput("pose", m_target_pose)) {
RCLCPP_FATAL(this->logger(), "[%s] Could not get target_pose from input port", name().c_str());
RCLCPP_FATAL(this->logger(),
"[%s] Could not get target_pose from input port",
name().c_str());
return false;
}
if (!getInput("duration", goal.duration)) {
RCLCPP_FATAL(this->logger(), "[%s] Could not get duration from input port", name().c_str());
RCLCPP_FATAL(this->logger(),
"[%s] Could not get duration from input port",
name().c_str());
return false;
}
RCLCPP_INFO_STREAM(
this->logger(),
"[" << name().c_str() << "]" << "Got target_pose: \nPosition [ "
<< m_target_pose->position.x << ", " << m_target_pose->position.y
<< ", " << m_target_pose->position.z << " ];\n"
<< "Orientation: [ " << m_target_pose->orientation.x << ", "
<< m_target_pose->orientation.y << ", "
<< m_target_pose->orientation.z << ", "
<< m_target_pose->orientation.w << "] ");
goal.target_pose = *m_target_pose;
getInput("try_plan_untill_success", goal.try_plan_untill_success);
getInput("allowed_planning_time", goal.allowed_planning_time);
return true;
}