Skip to content

Commit aa15c75

Browse files
juandpenanjuandpenan
committed
revert changes
1 parent db06733 commit aa15c75

2 files changed

Lines changed: 50 additions & 71 deletions

File tree

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

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

23+
#include "arm/manipulation/BTActionNode.hpp"
2324
#include "behaviortree_cpp_v3/behavior_tree.h"
2425
#include "behaviortree_cpp_v3/bt_factory.h"
2526
#include "geometry_msgs/msg/pose_stamped.hpp"
2627
#include "geometry_msgs/msg/transform_stamped.hpp"
27-
#include "moveit/move_group_interface/move_group_interface.h"
28+
#include "manipulation_interfaces/action/move_end_effector.hpp"
2829
#include "rclcpp/rclcpp.hpp"
2930
#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp"
3031

3132
namespace manipulation
3233
{
3334

34-
class PointAt : public BT::ActionNodeBase
35+
class PointAt : public manipulation::BtActionNode<
36+
manipulation_interfaces::action::MoveEndEffector,
37+
rclcpp_cascade_lifecycle::CascadeLifecycleNode>
3538
{
3639
public:
3740
explicit PointAt(
38-
const std::string & xml_tag_name,
41+
const std::string & xml_tag_name, const std::string & action_name,
3942
const BT::NodeConfiguration & conf);
4043

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

4447
static BT::PortsList providedPorts()
4548
{
@@ -50,7 +53,6 @@ class PointAt : public BT::ActionNodeBase
5053
}
5154

5255
private:
53-
std::shared_ptr<rclcpp_cascade_lifecycle::CascadeLifecycleNode> node_;
5456
geometry_msgs::msg::PoseStamped::SharedPtr pose_to_point_;
5557
std::string tf_frame_, base_frame_;
5658
tf2_ros::Buffer::SharedPtr tf_buffer_;

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

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

1717
#include <math.h>
18-
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
1918

2019
namespace manipulation
2120
{
@@ -24,19 +23,15 @@ using namespace std::chrono_literals;
2423
using namespace std::placeholders;
2524

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

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

@@ -48,10 +43,9 @@ BT::NodeStatus PointAt::tick()
4843
getInput("low_z", low_z_);
4944
getInput("high_z", high_z_);
5045

51-
geometry_msgs::msg::Pose target_pose;
52-
5346
if (tf_frame_.empty() && pose_to_point_) {
54-
target_pose = pose_to_point_->pose;
47+
goal_.pose = *pose_to_point_;
48+
return;
5549
} else if (!tf_frame_.empty()) {
5650
try {
5751
transform_ = tf_buffer_->lookupTransform(
@@ -60,79 +54,62 @@ BT::NodeStatus PointAt::tick()
6054
RCLCPP_ERROR(
6155
node_->get_logger(), "Could not transform %s to %s: %s", tf_frame_.c_str(),
6256
base_frame_.c_str(), ex.what());
63-
return BT::NodeStatus::FAILURE;
57+
setStatus(BT::NodeStatus::FAILURE);
58+
return;
6459
}
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;
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
7168
auto x_point = desired_radius * std::cos(angle);
7269
auto y_point = desired_radius * std::sin(angle);
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_ :
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_ :
7774
(transform_.transform.translation.z > high_z_) ?
7875
high_z_ :
7976
transform_.transform.translation.z;
80-
81-
tf2::Quaternion orientation(0, 0, 0, 1);
77+
goal_.pose.header.frame_id = base_frame_;
78+
auto orientation = tf2::Quaternion(0, 0, 0, 1);
8279
orientation.setEuler(0, 0, angle);
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();
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();
8784

8885
RCLCPP_INFO(
8986
node_->get_logger(), "Pointing from %s to %s", base_frame_.c_str(),
9087
transform_.header.frame_id.c_str());
9188
RCLCPP_INFO(node_->get_logger(), "Pointing at %s", tf_frame_.c_str());
9289
RCLCPP_INFO(
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;
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;
9893
}
94+
}
9995

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());
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");
128102
return BT::NodeStatus::FAILURE;
129103
}
130104
}
131105

132106
} // namespace manipulation
133-
134107
#include "behaviortree_cpp_v3/bt_factory.h"
135108
BT_REGISTER_NODES(factory)
136109
{
137-
factory.registerNodeType<manipulation::PointAt>("PointAt");
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);
138115
}

0 commit comments

Comments
 (0)