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

bt-node clang-format

Browse source
This commit is contained in:
Ilya Uraev 2023-11-22 14:43:30 +03:00
parent 4decc40c88
commit 034e172f62
7 changed files with 253 additions and 266 deletions
Download patch file Download diff file Expand all files Collapse all files

86
rbs_bt_executor/src/MoveToPose.cpp
View file

@ -1,7 +1,7 @@
#include "rbs_skill_interfaces/action/moveit_send_pose.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "behavior_tree/BtAction.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "moveit_msgs/action/move_group.h"
// #include "Eigen/Geometry"
@ -9,60 +9,56 @@
using namespace BT;
using MoveToPoseAction = rbs_skill_interfaces::action::MoveitSendPose;
static const rclcpp::Logger LOGGER = rclcpp::get_logger("MoveToPoseActionClient");
static const rclcpp::Logger LOGGER =
rclcpp::get_logger("MoveToPoseActionClient");
class MoveToPose : public BtAction<MoveToPoseAction>
{
public:
MoveToPose(const std::string & name, const BT::NodeConfiguration & config)
: BtAction<MoveToPoseAction>(name, config)
{
RCLCPP_INFO(_node->get_logger(), "Start the node");
}
class MoveToPose : public BtAction<MoveToPoseAction> {
public:
MoveToPose(const std::string &name, const BT::NodeConfiguration &config)
: BtAction<MoveToPoseAction>(name, config) {}
MoveToPoseAction::Goal populate_goal() override
{
getInput<std::string>("robot_name", robot_name_);
getInput<geometry_msgs::msg::Pose>("pose", pose_des);
RCLCPP_INFO(_node->get_logger(), "\n Send Pose: \n\t pose.x: %f \n\t pose.y: %f \n\t pose.z: %f \n\n\t opientation.x: %f \n\t orientation.y: %f \n\t orientation.z: %f \n\t orientation.w: %f",
pose_des.position.x, pose_des.position.y, pose_des.position.z,
pose_des.orientation.x, pose_des.orientation.y, pose_des.orientation.z, pose_des.orientation.w);
MoveToPoseAction::Goal populate_goal() override {
getInput<std::string>("robot_name", robot_name_);
getInput<geometry_msgs::msg::Pose>("pose", pose_des);
RCLCPP_INFO(_node->get_logger(),
"\n Send Pose: \n\t pose.x: %f \n\t pose.y: %f \n\t pose.z: %f "
"\n\n\t opientation.x: %f \n\t orientation.y: %f \n\t "
"orientation.z: %f \n\t orientation.w: %f",
pose_des.position.x, pose_des.position.y, pose_des.position.z,
pose_des.orientation.x, pose_des.orientation.y,
pose_des.orientation.z, pose_des.orientation.w);
auto goal = MoveToPoseAction::Goal();
RCLCPP_INFO(_node->get_logger(), "Send goal to robot [%s]", robot_name_.c_str());
goal.robot_name = robot_name_;
goal.target_pose = pose_des;
goal.end_effector_acceleration = 0.5;
goal.end_effector_velocity = 0.5;
RCLCPP_INFO(_node->get_logger(), "Goal populated");
auto goal = MoveToPoseAction::Goal();
RCLCPP_INFO(_node->get_logger(), "Send goal to robot [%s]",
robot_name_.c_str());
return goal;
}
goal.robot_name = robot_name_;
goal.target_pose = pose_des;
goal.end_effector_acceleration = 0.5;
goal.end_effector_velocity = 0.5;
static PortsList providedPorts()
{
return providedBasicPorts({
InputPort<std::string>("robot_name"),
InputPort<geometry_msgs::msg::Pose>("pose")
});
}
RCLCPP_INFO(_node->get_logger(), "Goal populated");
private:
std::string robot_name_;
//std::vector<double> pose_;
geometry_msgs::msg::Pose pose_des;
//std::map<std::string, geometry_msgs::msg::Pose> Poses;
return goal;
}
// geometry_msgs::msg::Pose array_to_pose(std::vector<double> pose_array){
static PortsList providedPorts() {
return providedBasicPorts({InputPort<std::string>("robot_name"),
InputPort<geometry_msgs::msg::Pose>("pose")});
}
// }
private:
std::string robot_name_;
// std::vector<double> pose_;
geometry_msgs::msg::Pose pose_des;
// std::map<std::string, geometry_msgs::msg::Pose> Poses;
// geometry_msgs::msg::Pose array_to_pose(std::vector<double> pose_array){
// }
}; // class MoveToPose
BT_REGISTER_NODES(factory)
{
factory.registerNodeType<MoveToPose>("MoveToPose");
BT_REGISTER_NODES(factory) {
factory.registerNodeType<MoveToPose>("MoveToPose");
}