Adjustment before robocup

bt_nodes/arm/CMakeLists.txt

@@ -14,6 +14,7 @@ find_package(rclcpp_action REQUIRED)
 find_package(manipulation_interfaces REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
 # uncomment the following section in order to fill in
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
@@ -28,6 +29,7 @@ set(dependencies
   manipulation_interfaces
   tf2_ros
   geometry_msgs
+  sensor_msgs
 )
 
 include_directories(include)
@@ -47,6 +49,16 @@ list(APPEND plugin_libs place_bt_node)
 add_library(move_joint_bt_node SHARED src/manipulation/move_joint.cpp)
 list(APPEND plugin_libs move_joint_bt_node)
 
+add_library(pick_object_from_pc_bt_node SHARED src/manipulation/pick_object_from_pc.cpp)
+list(APPEND plugin_libs pick_object_from_pc_bt_node)
+
+add_library(move_end_effector_bt_node SHARED src/manipulation/move_end_effector.cpp)
+list(APPEND plugin_libs move_end_effector_bt_node)
+
+add_library(generate_grasp_poses_bt_node SHARED src/manipulation/generate_grasp_poses.cpp)
+list(APPEND plugin_libs generate_grasp_poses_bt_node)
+
+
 foreach(bt_plugin ${plugin_libs})
   ament_target_dependencies(${bt_plugin} ${dependencies})
   target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT)

bt_nodes/arm/include/arm/manipulation/generate_grasp_poses.hpp

@@ -0,0 +1,56 @@
+// Copyright 2024 Intelligent Robotics Lab - Gentlebots
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions andGO2OBJECT
+// limitations under the License.
+
+#ifndef ARM_MANIPULATION__GENERATE_GRASP_POSES_HPP_
+#define ARM_MANIPULATION__GENERATE_GRASP_POSES_HPP_
+
+#include <algorithm>
+#include <string>
+
+#include "arm/manipulation/BTActionNode.hpp"
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp_v3/bt_factory.h"
+#include "manipulation_interfaces/action/generate_grasp_poses.hpp"
+#include "sensor_msgs/msg/point_cloud2.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp"
+
+namespace manipulation
+{
+
+class GenerateGraspPoses
+  : public manipulation::BtActionNode<
+    manipulation_interfaces::action::GenerateGraspPoses,
+    rclcpp_cascade_lifecycle::CascadeLifecycleNode>
+{
+public:
+  explicit GenerateGraspPoses(
+    const std::string & xml_tag_name, const std::string & action_name,
+    const BT::NodeConfiguration & conf);
+
+  void on_tick() override;
+  BT::NodeStatus on_success() override;
+
+  static BT::PortsList providedPorts()
+  {
+    return BT::PortsList(
+      {BT::InputPort<sensor_msgs::msg::PointCloud2>("object_to_pick"),
+        BT::OutputPort<std::vector<geometry_msgs::msg::PoseStamped>>("grasp_poses"),
+        BT::OutputPort<geometry_msgs::msg::PoseStamped>("best_pose")});
+  }
+};
+
+}  // namespace manipulation
+
+#endif  // arm_MANIPULATION__GENERATE_GRASP_POSES_HPP_

bt_nodes/arm/include/arm/manipulation/move_end_effector.hpp

@@ -0,0 +1,60 @@
+// Copyright 2024 Intelligent Robotics Lab - Gentlebots
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions andGO2OBJECT
+// limitations under the License.
+
+#ifndef ARM_MANIPULATION__MOVE_END_EFFECTOR_HPP_
+#define ARM_MANIPULATION__MOVE_END_EFFECTOR_HPP_
+
+#include <tf2_ros/buffer.h>
+
+#include <algorithm>
+#include <string>
+
+#include "arm/manipulation/BTActionNode.hpp"
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp_v3/bt_factory.h"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/transform_stamped.hpp"
+#include "manipulation_interfaces/action/move_end_effector.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp"
+
+namespace manipulation
+{
+
+class MoveEndEffector : public manipulation::BtActionNode<
+    manipulation_interfaces::action::MoveEndEffector,
+    rclcpp_cascade_lifecycle::CascadeLifecycleNode>
+{
+public:
+  explicit MoveEndEffector(
+    const std::string & xml_tag_name, const std::string & action_name,
+    const BT::NodeConfiguration & conf);
+
+  void on_tick() override;
+  BT::NodeStatus on_success() override;
+
+  static BT::PortsList providedPorts()
+  {
+    return BT::PortsList(
+      {BT::InputPort<geometry_msgs::msg::PoseStamped>("pose")});
+  }
+
+private:
+  geometry_msgs::msg::PoseStamped pose_to_move_;
+  std::string tf_frame_, base_frame_;
+};
+
+}  // namespace manipulation
+
+#endif  // ARM_MANIPULATION__MOVE_END_EFFECTOR_HPP_

bt_nodes/arm/include/arm/manipulation/pick_object_from_pc.hpp

@@ -0,0 +1,53 @@
+// Copyright 2024 Intelligent Robotics Lab - Gentlebots
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions andGO2OBJECT
+// limitations under the License.
+
+#ifndef ARM_MANIPULATION__PICK_OBJECT_FROM_PC_HPP_
+#define ARM_MANIPULATION__PICK_OBJECT_FROM_PC_HPP_
+
+#include <algorithm>
+#include <string>
+
+#include "arm/manipulation/BTActionNode.hpp"
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp_v3/bt_factory.h"
+#include "manipulation_interfaces/action/pick_from_pc.hpp"
+#include "sensor_msgs/msg/point_cloud2.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp"
+
+namespace manipulation
+{
+
+class PickObjectFromPC
+  : public manipulation::BtActionNode<
+    manipulation_interfaces::action::PickFromPc, rclcpp_cascade_lifecycle::CascadeLifecycleNode>
+{
+public:
+  explicit PickObjectFromPC(
+    const std::string & xml_tag_name, const std::string & action_name,
+    const BT::NodeConfiguration & conf);
+
+  void on_tick() override;
+  BT::NodeStatus on_success() override;
+
+  static BT::PortsList providedPorts()
+  {
+    return BT::PortsList(
+      {BT::InputPort<sensor_msgs::msg::PointCloud2>("object_to_pick")});
+  }
+};
+
+}  // namespace manipulation
+
+#endif  // arm_MANIPULATION__PICK_OBJECT_FROM_PC_HPP_

bt_nodes/arm/src/manipulation/generate_grasp_poses.cpp

@@ -0,0 +1,78 @@
+// Copyright 2024 Intelligent Robotics Lab - Gentlebots
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "arm/manipulation/generate_grasp_poses.hpp"
+
+#include <string>
+#include <utility>
+
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "manipulation_interfaces/action/pick.hpp"
+
+namespace manipulation
+{
+
+using namespace std::chrono_literals;
+using namespace std::placeholders;
+
+GenerateGraspPoses::GenerateGraspPoses(
+  const std::string & xml_tag_name, const std::string & action_name,
+  const BT::NodeConfiguration & conf)
+: manipulation::BtActionNode<
+    manipulation_interfaces::action::GenerateGraspPoses,
+    rclcpp_cascade_lifecycle::CascadeLifecycleNode>(
+    xml_tag_name, action_name, conf)
+{
+}
+
+void GenerateGraspPoses::on_tick()
+{
+  RCLCPP_DEBUG(node_->get_logger(), "GenerateGraspPoses ticked");
+  sensor_msgs::msg::PointCloud2 pc;
+  getInput("object_to_pick", pc);
+  goal_.object_goal = pc;
+}
+
+BT::NodeStatus GenerateGraspPoses::on_success()
+{
+  if (result_.result->success) {
+    setOutput("grasp_poses", result_.result->poses);
+    RCLCPP_INFO(node_->get_logger(), "GenerateGraspPoses succeeded");
+    // print the best pose header and position
+    RCLCPP_INFO(
+      node_->get_logger(), "Best pose header: %s",
+      result_.result->poses[0].header.frame_id.c_str());
+    RCLCPP_INFO(
+      node_->get_logger(), "Best pose position: %f %f %f", result_.result->poses[0].pose.position.x,
+      result_.result->poses[0].pose.position.y, result_.result->poses[0].pose.position.z);
+    setOutput("best_pose", result_.result->poses[0]);
+    return BT::NodeStatus::SUCCESS;
+  } else {
+    RCLCPP_ERROR(node_->get_logger(), "GenerateGraspPoses failed");
+    return BT::NodeStatus::FAILURE;
+  }
+}
+
+}  // namespace manipulation
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) {
+      return std::make_unique<manipulation::GenerateGraspPoses>(
+        name, "/generate_grasp_poses",
+        config);
+    };
+
+  factory.registerBuilder<manipulation::GenerateGraspPoses>("GenerateGraspPoses", builder);
+}

bt_nodes/arm/src/manipulation/move_end_effector.cpp

@@ -0,0 +1,68 @@
+// Copyright 2024 Intelligent Robotics Lab - Gentlebots
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "arm/manipulation/move_end_effector.hpp"
+
+#include <math.h>
+
+namespace manipulation
+{
+
+using namespace std::chrono_literals;
+using namespace std::placeholders;
+
+MoveEndEffector::MoveEndEffector(
+  const std::string & xml_tag_name, const std::string & action_name,
+  const BT::NodeConfiguration & conf)
+: manipulation::BtActionNode<
+    manipulation_interfaces::action::MoveEndEffector,
+    rclcpp_cascade_lifecycle::CascadeLifecycleNode>(xml_tag_name, action_name, conf)
+{
+}
+
+void MoveEndEffector::on_tick()
+{
+  RCLCPP_DEBUG(node_->get_logger(), "MoveEndEffector ticked");
+  getInput("pose", pose_to_move_);
+
+  goal_.pose = pose_to_move_;
+
+  RCLCPP_INFO(
+    node_->get_logger(), "Moving to %f %f %f", goal_.pose.pose.position.x,
+    goal_.pose.pose.position.y, goal_.pose.pose.position.z);
+
+  return;
+
+}
+
+BT::NodeStatus MoveEndEffector::on_success()
+{
+  if (result_.result->success) {
+    return BT::NodeStatus::SUCCESS;
+  } else {
+    RCLCPP_ERROR(node_->get_logger(), "MoveEndEffector failed");
+    return BT::NodeStatus::FAILURE;
+  }
+}
+
+}  // namespace manipulation
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+  BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) {
+      return std::make_unique<manipulation::MoveEndEffector>(name, "move_end_effector", config);
+    };
+
+  factory.registerBuilder<manipulation::MoveEndEffector>("MoveEndEffector", builder);
+}