Merge branch 'main' into coresense_main

Author: juandpenan

README.md

@@ -10,6 +10,27 @@ See [Software setup](https://github.com/CoreSenseEU/CoreSense4Home/wiki/C-Softwa
 ```bash
 ros2 launch robocup_bringup navigation.launch.py
 ```
+### Launch current carry my luggage implementation
+
+First kill move_group node inside tiago robot. Then in separate terminals launch:
+
+```bash
+ros2 launch robocup_bringup navigation_follow.launch.py rviz:=True
+```
+```bash
+ros2 launch attention_system attention.launch.py
+```
+```bash
+ros2 launch robocup_bringup carry_my_luggage_dependencies.launch.py
+``` 
+```bash
+ros2 launch whisper_bringup whisper.launch.py
+```
+Finally:
+
+```bash
+ros2 run bt_test carry_my_luggage_test
+```
 
 ### Follow navigation with small objects
 ```bash

bt_nodes/arm/CMakeLists.txt

@@ -12,6 +12,8 @@ find_package(rclcpp REQUIRED)
 find_package(rclcpp_cascade_lifecycle REQUIRED)
 find_package(rclcpp_action REQUIRED)
 find_package(manipulation_interfaces REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(geometry_msgs REQUIRED)
 # uncomment the following section in order to fill in
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
@@ -24,6 +26,8 @@ set(dependencies
   rclcpp_cascade_lifecycle
   rclcpp_action
   manipulation_interfaces
+  tf2_ros
+  geometry_msgs
 )
 
 include_directories(include)
@@ -34,6 +38,12 @@ list(APPEND plugin_libs pick_bt_node)
 add_library(move_to_predefined_bt_node SHARED src/manipulation/move_to_predefined.cpp)
 list(APPEND plugin_libs move_to_predefined_bt_node)
 
+add_library(point_at_bt_node SHARED src/manipulation/point_at.cpp)
+list(APPEND plugin_libs point_at_bt_node)
+
+add_library(place_bt_node SHARED src/manipulation/place_object.cpp)
+list(APPEND plugin_libs place_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/BTActionNode.hpp

@@ -25,7 +25,7 @@
 namespace manipulation
 {
 
-using namespace std::chrono_literals; // NOLINT
+using namespace std::chrono_literals;  // NOLINT
 
 template<class ActionT, class NodeT = rclcpp::Node>
 class BtActionNode : public BT::ActionNodeBase
@@ -42,8 +42,7 @@ class BtActionNode : public BT::ActionNodeBase
 
     // Initialize the input and output messages
     goal_ = typename ActionT::Goal();
-    result_ =
-      typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult();
+    result_ = typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult();
 
     std::string remapped_action_name;
     if (getInput("server_name", remapped_action_name)) {
@@ -52,9 +51,7 @@ class BtActionNode : public BT::ActionNodeBase
     createActionClient(action_name_);
 
     // Give the derive class a chance to do any initialization
-    RCLCPP_INFO(
-      node_->get_logger(), "\"%s\" BtActionNode initialized",
-      xml_tag_name.c_str());
+    RCLCPP_INFO(node_->get_logger(), "\"%s\" BtActionNode initialized", xml_tag_name.c_str());
   }
 
   BtActionNode() = delete;
@@ -69,9 +66,7 @@ class BtActionNode : public BT::ActionNodeBase
     action_client_ = rclcpp_action::create_client<ActionT>(node_, action_name);
 
     // Make sure the server is actually there before continuing
-    RCLCPP_INFO(
-      node_->get_logger(), "Waiting for \"%s\" action server",
-      action_name.c_str());
+    RCLCPP_INFO(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str());
     action_client_->wait_for_action_server();
   }
 
@@ -134,8 +129,8 @@ class BtActionNode : public BT::ActionNodeBase
       on_wait_for_result();
 
       auto goal_status = goal_handle_->get_status();
-      if (goal_updated_ &&
-        (goal_status == action_msgs::msg::GoalStatus::STATUS_EXECUTING ||
+      if (
+        goal_updated_ && (goal_status == action_msgs::msg::GoalStatus::STATUS_EXECUTING ||
         goal_status == action_msgs::msg::GoalStatus::STATUS_ACCEPTED))
       {
         goal_updated_ = false;
@@ -172,15 +167,13 @@ class BtActionNode : public BT::ActionNodeBase
   {
     if (should_cancel_goal()) {
       auto future_cancel = action_client_->async_cancel_goal(goal_handle_);
-      if (rclcpp::spin_until_future_complete(
-          node_->get_node_base_interface(),
-          future_cancel, server_timeout_) !=
+      if (
+        rclcpp::spin_until_future_complete(
+          node_->get_node_base_interface(), future_cancel, server_timeout_) !=
         rclcpp::FutureReturnCode::SUCCESS)
       {
         RCLCPP_ERROR(
-          node_->get_logger(),
-          "Failed to cancel action server for %s",
-          action_name_.c_str());
+          node_->get_logger(), "Failed to cancel action server for %s", action_name_.c_str());
       }
     }
 
@@ -206,11 +199,9 @@ class BtActionNode : public BT::ActionNodeBase
   void on_new_goal_received()
   {
     goal_result_available_ = false;
-    auto send_goal_options =
-      typename rclcpp_action::Client<ActionT>::SendGoalOptions();
+    auto send_goal_options = typename rclcpp_action::Client<ActionT>::SendGoalOptions();
     send_goal_options.result_callback =
-      [this](const typename rclcpp_action::ClientGoalHandle<
-          ActionT>::WrappedResult & result) {
+      [this](const typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult & result) {
         // TODO(#1652): a work around until rcl_action interface is updated
         // if goal ids are not matched, the older goal call this callback so
         // ignore the result if matched, it must be processed (including
@@ -221,12 +212,12 @@ class BtActionNode : public BT::ActionNodeBase
         }
       };
 
-    auto future_goal_handle =
-      action_client_->async_send_goal(goal_, send_goal_options);
+    auto future_goal_handle = action_client_->async_send_goal(goal_, send_goal_options);
 
-    if (rclcpp::spin_until_future_complete(
-        node_->get_node_base_interface(), future_goal_handle,
-        server_timeout_) != rclcpp::FutureReturnCode::SUCCESS)
+    if (
+      rclcpp::spin_until_future_complete(
+        node_->get_node_base_interface(), future_goal_handle, server_timeout_) !=
+      rclcpp::FutureReturnCode::SUCCESS)
     {
       throw std::runtime_error("send_goal failed");
     }
@@ -242,11 +233,11 @@ class BtActionNode : public BT::ActionNodeBase
     int recovery_count = 0;
     config().blackboard->get<int>(
       "number_recoveries",
-      recovery_count);                             // NOLINT
+      recovery_count);                              // NOLINT
     recovery_count += 1;
     config().blackboard->set<int>(
       "number_recoveries",
-      recovery_count);                             // NOLINT
+      recovery_count);                              // NOLINT
   }
 
   std::string action_name_;
@@ -267,6 +258,6 @@ class BtActionNode : public BT::ActionNodeBase
   std::chrono::milliseconds server_timeout_;
 };
 
-} // namespace manipulation
+}  // namespace manipulation
 
-#endif // ARM__MANIPULATION__BTACTIONNODE_HPP_
+#endif  // ARM__MANIPULATION__BTACTIONNODE_HPP_

bt_nodes/arm/include/arm/manipulation/move_to_predefined.hpp

@@ -18,42 +18,43 @@
 #include <algorithm>
 #include <string>
 
-#include "manipulation_interfaces/action/move_to_predefined.hpp"
-#include "moveit_msgs/msg/collision_object.hpp"
+#include "arm/manipulation/BTActionNode.hpp"
 #include "behaviortree_cpp_v3/behavior_tree.h"
 #include "behaviortree_cpp_v3/bt_factory.h"
-#include "arm/manipulation/BTActionNode.hpp"
-
+#include "manipulation_interfaces/action/move_to_predefined.hpp"
+#include "moveit_msgs/msg/collision_object.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp"
 
 namespace manipulation
 {
 
-class MoveToPredefined : public manipulation::BtActionNode<
-    manipulation_interfaces::action::MoveToPredefined,
-    rclcpp_cascade_lifecycle::CascadeLifecycleNode>
+class MoveToPredefined
+  : public manipulation::BtActionNode<manipulation_interfaces::action::MoveToPredefined>
 {
 public:
   explicit MoveToPredefined(
-    const std::string & xml_tag_name,
-    const std::string & action_name,
+    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<std::string>(
-          "pose")});
+    return BT::PortsList({BT::InputPort<std::string>("pose"),
+                          BT::InputPort<std::string>("group_name")});
   }
 
 private:
+  // rclcpp::Node::SharedPtr node_;
+  //  rclcpp::ActionClient<audio_common_msgs::action::TTS>::SharedPtr
+  //  tts_action_;
+  std::string group_name_{"arm_torso"};
+
   std::string pose_;
 };
 
-} // namespace manipulation
+}  // namespace manipulation
 
-#endif // ARM_MANIPULATION__MOVE_TO_PREDEFINED_HPP_
+#endif  // ARM_MANIPULATION__MOVE_TO_PREDEFINED_HPP_

bt_nodes/arm/include/arm/manipulation/pick_object.hpp

@@ -18,12 +18,11 @@
 #include <algorithm>
 #include <string>
 
-#include "manipulation_interfaces/action/pick.hpp"
-#include "moveit_msgs/msg/collision_object.hpp"
+#include "arm/manipulation/BTActionNode.hpp"
 #include "behaviortree_cpp_v3/behavior_tree.h"
 #include "behaviortree_cpp_v3/bt_factory.h"
-#include "arm/manipulation/BTActionNode.hpp"
-
+#include "manipulation_interfaces/action/pick.hpp"
+#include "moveit_msgs/msg/collision_object.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp"
 
@@ -35,8 +34,7 @@ class PickObject : public manipulation::BtActionNode<
 {
 public:
   explicit PickObject(
-    const std::string & xml_tag_name,
-    const std::string & action_name,
+    const std::string & xml_tag_name, const std::string & action_name,
     const BT::NodeConfiguration & conf);
 
   void on_tick() override;
@@ -45,11 +43,10 @@ class PickObject : public manipulation::BtActionNode<
   static BT::PortsList providedPorts()
   {
     return BT::PortsList(
-      {BT::InputPort<moveit_msgs::msg::CollisionObject::SharedPtr>(
-          "object_to_pick")});
+      {BT::InputPort<moveit_msgs::msg::CollisionObject::SharedPtr>("object_to_pick")});
   }
 };
 
-} // namespace manipulation
+}  // namespace manipulation
 
-#endif // arm_MANIPULATION__PICK_OBJECT_HPP_
+#endif  // arm_MANIPULATION__PICK_OBJECT_HPP_

bt_nodes/arm/include/arm/manipulation/place_object.hpp

@@ -0,0 +1,69 @@
+// 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__PLACE_OBJECT_HPP_
+#define ARM_MANIPULATION__PLACE_OBJECT_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/place.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/transform_stamped.hpp"
+#include <tf2_ros/buffer.h>
+#include "moveit_msgs/msg/collision_object.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+namespace manipulation
+{
+
+class PlaceObject : public manipulation::BtActionNode<manipulation_interfaces::action::Place>
+{
+public:
+  explicit PlaceObject(
+    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<moveit_msgs::msg::CollisionObject::SharedPtr>("object_to_place"),
+       BT::InputPort<geometry_msgs::msg::PoseStamped>("place_pose"),
+       BT::InputPort<std::string>("base_frame")});
+  }
+
+private:
+
+
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::string tf_to_place_, base_frame_;
+  moveit_msgs::msg::CollisionObject::SharedPtr object_;
+  geometry_msgs::msg::PoseStamped place_pose_;
+  geometry_msgs::msg::TransformStamped transform_to_place_;
+  // rclcpp::Node::SharedPtr node_;
+  //  rclcpp::ActionClient<audio_common_msgs::action::TTS>::SharedPtr
+  //  tts_action_;
+
+  // std::string text_;
+};
+
+}  // namespace manipulation
+
+#endif  // arm_MANIPULATION__PLACE_OBJECT_HPP_