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

add workspace inspector skill

Browse source
This commit is contained in:
Ilya Uraev 2023-12-30 23:42:10 +03:00
parent 4f0c43bee9
commit 9efab819af
9 changed files with 158 additions and 85 deletions
Download patch file Download diff file Expand all files Collapse all files

27
rbs_bt_executor/src/MoveToPose.cpp
View file

@ -1,19 +1,14 @@
// Copyright [2023] bill-finger
#include "rbs_skill_interfaces/action/moveit_send_pose.hpp"
#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"
#include "rclcpp/parameter.hpp"
using namespace BT;
using MoveToPoseAction = rbs_skill_interfaces::action::MoveitSendPose;
static const rclcpp::Logger LOGGER =
rclcpp::get_logger("MoveToPoseActionClient");
class MoveToPose : public BtAction<MoveToPoseAction> {
public:
public:
MoveToPose(const std::string &name, const BT::NodeConfiguration &config)
: BtAction<MoveToPoseAction>(name, config) {}
@ -42,23 +37,17 @@ public:
return goal;
}
static PortsList providedPorts() {
return providedBasicPorts({InputPort<std::string>("robot_name"),
InputPort<geometry_msgs::msg::Pose>("pose")});
static BT::PortsList providedPorts() {
return providedBasicPorts(
{BT::InputPort<std::string>("robot_name"),
BT::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
}; // class MoveToPose
BT_REGISTER_NODES(factory) {
factory.registerNodeType<MoveToPose>("MoveToPose");
}
}