Skip to content

Commit db06733

Browse files
juandpenanjuandpenan
committed
changed to pure moveit api
1 parent aa0d5b7 commit db06733

File tree

2 files changed

+71
-50
lines changed

2 files changed

+71
-50
lines changed

‎bt_nodes/arm/include/arm/manipulation/point_at.hpp‎

Lines changed: 6 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -20,29 +20,26 @@
2020
#include <algorithm>
2121
#include <string>
2222

23-
#include "arm/manipulation/BTActionNode.hpp"
2423
#include "behaviortree_cpp_v3/behavior_tree.h"
2524
#include "behaviortree_cpp_v3/bt_factory.h"
2625
#include "geometry_msgs/msg/pose_stamped.hpp"
2726
#include "geometry_msgs/msg/transform_stamped.hpp"
28-
#include "manipulation_interfaces/action/move_end_effector.hpp"
27+
#include "moveit/move_group_interface/move_group_interface.h"
2928
#include "rclcpp/rclcpp.hpp"
3029
#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp"
3130

3231
namespace manipulation
3332
{
3433

35-
class PointAt : public manipulation::BtActionNode<
36-
manipulation_interfaces::action::MoveEndEffector,
37-
rclcpp_cascade_lifecycle::CascadeLifecycleNode>
34+
class PointAt : public BT::ActionNodeBase
3835
{
3936
public:
4037
explicit PointAt(
41-
const std::string & xml_tag_name, const std::string & action_name,
38+
const std::string & xml_tag_name,
4239
const BT::NodeConfiguration & conf);
4340

44-
void on_tick() override;
45-
BT::NodeStatus on_success() override;
41+
void halt() override;
42+
BT::NodeStatus tick() override;
4643

4744
static BT::PortsList providedPorts()
4845
{
@@ -53,6 +50,7 @@ class PointAt : public manipulation::BtActionNode<
5350
}
5451

5552
private:
53+
std::shared_ptr<rclcpp_cascade_lifecycle::CascadeLifecycleNode> node_;
5654
geometry_msgs::msg::PoseStamped::SharedPtr pose_to_point_;
5755
std::string tf_frame_, base_frame_;
5856
tf2_ros::Buffer::SharedPtr tf_buffer_;

‎bt_nodes/arm/src/manipulation/point_at.cpp‎

Lines changed: 65 additions & 42 deletions
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,7 @@
1515
#include "arm/manipulation/point_at.hpp"
1616

1717
#include <math.h>
18+
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
1819

1920
namespace manipulation
2021
{
@@ -23,15 +24,19 @@ using namespace std::chrono_literals;
2324
using namespace std::placeholders;
2425

2526
PointAt::PointAt(
26-
const std::string & xml_tag_name, const std::string & action_name,
27+
const std::string & xml_tag_name,
2728
const BT::NodeConfiguration & conf)
28-
: manipulation::BtActionNode<
29-
manipulation_interfaces::action::MoveEndEffector,
30-
rclcpp_cascade_lifecycle::CascadeLifecycleNode>(xml_tag_name, action_name, conf)
29+
: BT::ActionNodeBase(xml_tag_name, conf)
3130
{
31+
config().blackboard->get("node", node_);
3232
}
3333

34-
void PointAt::on_tick()
34+
void PointAt::halt()
35+
{
36+
RCLCPP_INFO(node_->get_logger(), "PointAt halted");
37+
}
38+
39+
BT::NodeStatus PointAt::tick()
3540
{
3641
RCLCPP_DEBUG(node_->get_logger(), "PointAt ticked");
3742

@@ -43,9 +48,10 @@ void PointAt::on_tick()
4348
getInput("low_z", low_z_);
4449
getInput("high_z", high_z_);
4550

51+
geometry_msgs::msg::Pose target_pose;
52+
4653
if (tf_frame_.empty() && pose_to_point_) {
47-
goal_.pose = *pose_to_point_;
48-
return;
54+
target_pose = pose_to_point_->pose;
4955
} else if (!tf_frame_.empty()) {
5056
try {
5157
transform_ = tf_buffer_->lookupTransform(
@@ -54,62 +60,79 @@ void PointAt::on_tick()
5460
RCLCPP_ERROR(
5561
node_->get_logger(), "Could not transform %s to %s: %s", tf_frame_.c_str(),
5662
base_frame_.c_str(), ex.what());
57-
setStatus(BT::NodeStatus::FAILURE);
58-
return;
63+
return BT::NodeStatus::FAILURE;
5964
}
60-
// goal_.pose.pose.position.x = 0.5;
61-
// auto y = 0.5*std::cos(std::atan2(transform_.transform.translation.y, transform_.transform.translation.x)) * std::sin(std::atan2(transform_.transform.translation.y, transform_.transform.translation.x));
62-
// goal_.pose.pose.position.y = (std::isnan(y) || std::isinf(y)) ? 0.0 : (y);
63-
// goal_.pose.pose.position.z = 0.5;
64-
// goal_.pose.pose.orientation = transform_.transform.rotation;
65-
auto angle = std::atan2(transform_.transform.translation.y, transform_.transform.translation.x);
66-
auto pitch = std::atan2(transform_.transform.translation.z, transform_.transform.translation.x);
67-
double desired_radius = 0.9; //0.5
65+
66+
// Calculate pointing pose
67+
auto angle = std::atan2(
68+
transform_.transform.translation.y,
69+
transform_.transform.translation.x);
70+
double desired_radius = 0.9;
6871
auto x_point = desired_radius * std::cos(angle);
6972
auto y_point = desired_radius * std::sin(angle);
70-
goal_.pose.pose.position.x = (std::isnan(x_point) || std::isinf(x_point)) ? 0.0 : (x_point);
71-
goal_.pose.pose.position.y = (std::isnan(y_point) || std::isinf(y_point)) ? 0.0 : (y_point);
72-
// if (transform_.transform.translation.z < low_z_) put 1.0 elif (transform_.transform.translation.z > high_z_) put 1.0 else put transform_.transform.translation.z
73-
goal_.pose.pose.position.z = (transform_.transform.translation.z < low_z_) ? low_z_ :
73+
74+
target_pose.position.x = (std::isnan(x_point) || std::isinf(x_point)) ? 0.0 : x_point;
75+
target_pose.position.y = (std::isnan(y_point) || std::isinf(y_point)) ? 0.0 : y_point;
76+
target_pose.position.z = (transform_.transform.translation.z < low_z_) ? low_z_ :
7477
(transform_.transform.translation.z > high_z_) ?
7578
high_z_ :
7679
transform_.transform.translation.z;
77-
goal_.pose.header.frame_id = base_frame_;
78-
auto orientation = tf2::Quaternion(0, 0, 0, 1);
80+
81+
tf2::Quaternion orientation(0, 0, 0, 1);
7982
orientation.setEuler(0, 0, angle);
80-
goal_.pose.pose.orientation.x = orientation.x();
81-
goal_.pose.pose.orientation.y = orientation.y();
82-
goal_.pose.pose.orientation.z = orientation.z();
83-
goal_.pose.pose.orientation.w = orientation.w();
83+
target_pose.orientation.x = orientation.x();
84+
target_pose.orientation.y = orientation.y();
85+
target_pose.orientation.z = orientation.z();
86+
target_pose.orientation.w = orientation.w();
8487

8588
RCLCPP_INFO(
8689
node_->get_logger(), "Pointing from %s to %s", base_frame_.c_str(),
8790
transform_.header.frame_id.c_str());
8891
RCLCPP_INFO(node_->get_logger(), "Pointing at %s", tf_frame_.c_str());
8992
RCLCPP_INFO(
90-
node_->get_logger(), "Pointing to %f %f %f", goal_.pose.pose.position.x,
91-
goal_.pose.pose.position.y, goal_.pose.pose.position.z);
92-
return;
93+
node_->get_logger(), "Pointing to %f %f %f", target_pose.position.x,
94+
target_pose.position.y, target_pose.position.z);
95+
} else {
96+
RCLCPP_ERROR(node_->get_logger(), "No pose_to_point or tf_frame provided");
97+
return BT::NodeStatus::FAILURE;
9398
}
94-
}
9599

96-
BT::NodeStatus PointAt::on_success()
97-
{
98-
if (result_.result->success) {
99-
return BT::NodeStatus::SUCCESS;
100-
} else {
101-
RCLCPP_ERROR(node_->get_logger(), "PointAt failed");
100+
// Use MoveIt to plan and execute
101+
try {
102+
using moveit::planning_interface::MoveGroupInterface;
103+
auto move_group_interface = MoveGroupInterface(node_, "arm_torso");
104+
105+
// Set the target pose
106+
move_group_interface.setPoseTarget(target_pose);
107+
108+
// Create a plan to that target pose
109+
moveit::planning_interface::MoveGroupInterface::Plan plan;
110+
bool success = static_cast<bool>(move_group_interface.plan(plan));
111+
112+
if (success) {
113+
RCLCPP_INFO(node_->get_logger(), "Plan found, executing...");
114+
auto execute_result = move_group_interface.execute(plan);
115+
if (execute_result == moveit::core::MoveItErrorCode::SUCCESS) {
116+
RCLCPP_INFO(node_->get_logger(), "PointAt execution successful");
117+
return BT::NodeStatus::SUCCESS;
118+
} else {
119+
RCLCPP_ERROR(node_->get_logger(), "Execution failed!");
120+
return BT::NodeStatus::FAILURE;
121+
}
122+
} else {
123+
RCLCPP_ERROR(node_->get_logger(), "Planning failed!");
124+
return BT::NodeStatus::FAILURE;
125+
}
126+
} catch (const std::exception & ex) {
127+
RCLCPP_ERROR(node_->get_logger(), "Exception during MoveIt planning: %s", ex.what());
102128
return BT::NodeStatus::FAILURE;
103129
}
104130
}
105131

106132
} // namespace manipulation
133+
107134
#include "behaviortree_cpp_v3/bt_factory.h"
108135
BT_REGISTER_NODES(factory)
109136
{
110-
BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) {
111-
return std::make_unique<manipulation::PointAt>(name, "move_end_effector", config);
112-
};
113-
114-
factory.registerBuilder<manipulation::PointAt>("PointAt", builder);
137+
factory.registerNodeType<manipulation::PointAt>("PointAt");
115138
}

0 commit comments

Comments
 (0)