Updated humble devel

.docker/Dockerfile

@@ -1,11 +1,8 @@
-# Dockerfile for ROSCon 2024 Deliberation Workshop
-
 FROM ros:humble AS robocup2024
 SHELL ["/bin/bash", "-o", "pipefail", "-c"]
 
 # Install apt dependencies.
 ARG DEBIAN_FRONTEND=noninteractive
-
 RUN apt-get update \
     && apt-get -y --quiet --no-install-recommends install \
     gcc \
@@ -18,29 +15,27 @@ RUN apt-get update \
     ros-humble-nav2-bringup \
     ros-humble-rmw-cyclonedds-cpp \
     nlohmann-json3-dev
-
+
+RUN pip install setuptools==58.2.0
 
 # Create a ROS 2 workspace.
 RUN mkdir -p /robocup/src/
 WORKDIR /robocup
 
-ADD https://raw.githubusercontent.com/mgonzs13/audio_common/refs/heads/main/requirements.txt /robocup/requirements2.txt
-ADD https://raw.githubusercontent.com/mgonzs13/whisper_ros/refs/heads/main/requirements.txt /robocup/requirements3.txt
-ADD https://raw.githubusercontent.com/mgonzs13/llama_ros/refs/heads/main/requirements.txt /robocup/requirements4.txt
-ADD https://raw.githubusercontent.com/jmguerreroh/yolov8_ros/refs/heads/main/requirements.txt /robocup/requirements5.txt
+ADD https://raw.githubusercontent.com/mgonzs13/llama_ros/refs/heads/main/requirements.txt /robocup/requirements1.txt
+ADD https://raw.githubusercontent.com/jmguerreroh/yolov8_ros/refs/heads/main/requirements.txt /robocup/requirements2.txt
+ADD https://raw.githubusercontent.com/mgonzs13/tts_ros/refs/heads/main/requirements.txt /robocup/requirements3.txt
 
 # Install external dependencies.
-RUN pip install coqui-tts
+RUN pip install -r requirements1.txt
 RUN pip install -r requirements2.txt
 RUN pip install -r requirements3.txt
-RUN pip install -r requirements4.txt
-RUN pip install -r requirements5.txt
-
 
 WORKDIR /robocup/src
 RUN git clone https://github.com/CoreSenseEU/CoreSense4Home.git -b humble-devel
 RUN vcs import --recursive < ./CoreSense4Home/robocup_bringup/thirdparty.repos
 
+
 WORKDIR /robocup
 RUN rosdep install --from-paths src --ignore-src -r -y
 

bt_nodes/arm/CMakeLists.txt

@@ -15,6 +15,7 @@ find_package(manipulation_interfaces REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
+find_package(play_motion2_msgs REQUIRED)
 # uncomment the following section in order to fill in
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
@@ -30,6 +31,7 @@ set(dependencies
   tf2_ros
   geometry_msgs
   sensor_msgs
+  play_motion2_msgs
 )
 
 include_directories(include)
@@ -40,6 +42,9 @@ 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(play_motion_predefined_bt_node SHARED src/manipulation/play_motion_predefined.cpp)
+list(APPEND plugin_libs play_motion_predefined_bt_node)
+
 add_library(point_at_bt_node SHARED src/manipulation/point_at.cpp)
 list(APPEND plugin_libs point_at_bt_node)
 

bt_nodes/arm/include/arm/manipulation/play_motion_predefined.hpp

@@ -0,0 +1,59 @@
+// 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.
+
+#ifndef ARM_MANIPULATION_PLAY_MOTION_PREDEFINED_HPP_
+#define ARM_MANIPULATION_PLAY_MOTION_PREDEFINED_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 "play_motion2_msgs/action/play_motion2.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp"
+
+namespace manipulation
+{
+
+class PlayMotionPredefined : public manipulation::BtActionNode<
+    play_motion2_msgs::action::PlayMotion2,
+    rclcpp_cascade_lifecycle::CascadeLifecycleNode>
+{
+public:
+  explicit PlayMotionPredefined(
+    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>("motion_name"),
+        BT::InputPort<bool>("skip_planning")
+      });
+  }
+
+private:
+  std::string motion_name_{};
+  bool skip_planning_{false};
+};
+
+}  // namespace manipulation
+
+#endif  // ARM_MANIPULATION__PLAY_MOTION_PREDEFINED_HPP_

bt_nodes/arm/package.xml

@@ -13,6 +13,7 @@
   <depend>rclcpp_action</depend>
   <depend>rclcpp_cascade_lifecycle</depend>
   <depend>manipulation_interfaces</depend>
+  <depend>play_motion2_msgs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

bt_nodes/arm/src/manipulation/play_motion_predefined.cpp

@@ -0,0 +1,79 @@
+// 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/play_motion_predefined.hpp"
+
+#include <string>
+
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "play_motion2_msgs/action/play_motion2.hpp"
+
+namespace manipulation
+{
+
+using namespace std::chrono_literals;
+using namespace std::placeholders;
+
+PlayMotionPredefined::PlayMotionPredefined(
+  const std::string & xml_tag_name, const std::string & action_name,
+  const BT::NodeConfiguration & conf)
+: manipulation::BtActionNode<
+    play_motion2_msgs::action::PlayMotion2,
+    rclcpp_cascade_lifecycle::CascadeLifecycleNode>(xml_tag_name, action_name, conf)
+{
+}
+
+void PlayMotionPredefined::on_tick()
+{
+  RCLCPP_DEBUG(node_->get_logger(), "PlayMotionPredefined ticked");
+
+  // Read ports
+  getInput("motion_name", motion_name_);
+  getInput("skip_planning", skip_planning_);
+
+  // Fill goal
+  goal_.motion_name = motion_name_;
+  goal_.skip_planning = skip_planning_;
+
+  RCLCPP_INFO(
+    node_->get_logger(), "Requesting PlayMotion2 '%s' (skip_planning=%s)",
+    goal_.motion_name.c_str(), skip_planning_ ? "true" : "false");
+}
+
+BT::NodeStatus PlayMotionPredefined::on_success()
+{
+  if (result_.result && result_.result->success) {
+    return BT::NodeStatus::SUCCESS;
+  } else {
+    if (result_.result) {
+      RCLCPP_ERROR(node_->get_logger(), "PlayMotion2 failed: %s", result_.result->error.c_str());
+    } else {
+      RCLCPP_ERROR(node_->get_logger(), "PlayMotion2 failed: no result received");
+    }
+    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) {
+      // Default action server name as per play_motion2: "/play_motion2"
+      return std::make_unique<manipulation::PlayMotionPredefined>(name, "/play_motion2", config);
+    };
+
+  factory.registerBuilder<manipulation::PlayMotionPredefined>("PlayMotionPredefined", builder);
+}

bt_nodes/hri/include/hri/dialog/BTActionNode.hpp

@@ -21,13 +21,15 @@
 #include "behaviortree_cpp_v3/action_node.h"
 #include "rclcpp/rclcpp.hpp"
 #include "rclcpp_action/rclcpp_action.hpp"
+#include "rclcpp_cascade_lifecycle/rclcpp_cascade_lifecycle.hpp"
+
 
 namespace dialog
 {
 
 using namespace std::chrono_literals;  // NOLINT
 
-template<class ActionT, class NodeT = rclcpp::Node>
+template<class ActionT, class NodeT = rclcpp_cascade_lifecycle::CascadeLifecycleNode>
 class BtActionNode : public BT::ActionNodeBase
 {
 public:

bt_nodes/hri/src/hri/check_policy.cpp

@@ -57,7 +57,7 @@ void CheckPolicy::on_tick()
 
   std::string prompt_ = text_ + ". Please answer only with 'yes' or 'no'";
   goal_.prompt = prompt_;
-  goal_.image = *image_;
+  goal_.images.push_back(*image_);
   goal_.reset = true;
   goal_.sampling_config.temp = 0.0;
   goal_.sampling_config.grammar =

bt_nodes/motion/include/motion/navigation/MoveTo.hpp

@@ -56,7 +56,7 @@ class MoveTo : public motion::BtActionNode<
   {
     return BT::PortsList(
       {BT::InputPort<double>("distance_tolerance"), BT::InputPort<std::string>("tf_frame"),
-        BT::InputPort<bool>("will_finish"), BT::InputPort<bool>("is_truncated")});
+      BT::InputPort<bool>("will_finish"), BT::InputPort<bool>("is_truncated")});
   }
 
 private:

robocup_bringup/bt_xml/carry_my_luggage.xml

@@ -32,10 +32,10 @@
         <Action ID="Speak" param="{bag_direction}" say_text="I see you are pointing to the bag at my"/>
         <Action ID="Speak" param="" say_text="I am on my way"/>
         <Action ID="MoveTo" is_truncated="true"  distance_tolerance="1.2" tf_frame="{bag_tf}"/>
-        <Action ID="MoveToPredefined" pose="offer" group_name="arm_torso"/> 
+        <Action ID="PlayMotionPredefined" motion_name="offer" /> 
         <Action ID="Speak" param="" say_text="please put the bag in my gripper"/> 
         <Delay delay_msec="140">
-            <Action ID="MoveToPredefined" pose="open" group_name="gripper"/>
+            <Action ID="PlayMotionPredefined" motion_name="open"/>
         </Delay>
         <ReactiveSequence>
             <RetryUntilSuccessful num_attempts="-1">
@@ -45,10 +45,10 @@
             <Action ID="LookAt" tf_frame="person_0_filtered" />
         </ReactiveSequence>
         <Delay delay_msec="300">
-            <Action ID="MoveToPredefined" pose="close" group_name="gripper"/>
-        </Delay>
+            <Action ID="PlayMotionPredefined" motion_name="close"/>
+        </Delay> 
         <Delay delay_msec="500">
-            <Action ID="MoveToPredefined" pose="home" group_name="arm_torso"/>
+            <Action ID="PlayMotionPredefined" motion_name="home"/>
         </Delay>
         <Action ID="Speak" param="" say_text="Perfect, now i will follow you. Please stop at the end "/>
         <ReactiveSequence>
@@ -101,15 +101,15 @@
                 </Sequence>
             </RetryUntilSuccessful>
             <Action ID="Speak" param="" say_text="I will give you the bag now, please be carefull"/>
-            <Action ID="MoveToPredefined" pose="offer" group_name="arm_torso"/>
+            <Action ID="PlayMotionPredefined" motion_name="offer" />
             <ReactiveSequence>
                 <Action ID="LookAt" tf_frame="person_0"/>
                 <Action ID="Speak" param="" say_text="Here is the bag, please take it"/>
             </ReactiveSequence>
             <Delay delay_msec="140">
-                <Action ID="MoveToPredefined" pose="open" group_name="gripper"/>
+                <Action ID="PlayMotionPredefined" motion_name="open"/>
             </Delay>
-            <Action ID="MoveToPredefined" pose="home" group_name="arm_torso"/>
+            <Action ID="PlayMotionPredefined" motion_name="home"/>
             <Delay delay_msec="3000">
                 <Action ID="Speak" param="" say_text="I am going back, have a nice day"/>
             </Delay>

robocup_bringup/bt_xml/gpsr.xml

@@ -2,7 +2,7 @@
     <BehaviorTree ID="GPSR">
         <Sequence>
             <Action ID="SetupGPSR" plugins="{plugins}"/>
-            <SetBlackboard output_key="cam_frame" value="head_front_camera_link_color_optical_frame"/>
+            <SetBlackboard output_key="cam_frame" value="head_front_camera_color_optical_frame"/>
             <!-- <Action ID="SetStartPosition" frame_name="instruction point" /> -->
             <Action ID="SetWp"/>
 

robocup_bringup/config/carry_my_luggage/carry_my_luggage.yaml

@@ -14,7 +14,7 @@ behaviors_main:
     - is_pointing_bt_node
     - extract_entity_color_bt_node
     - move_to_bt_node
-    - move_to_predefined_bt_node
+    - play_motion_predefined_bt_node
     - look_at_bt_node
     - speak_bt_node
     - is_entity_moving_bt_node

robocup_bringup/config/gpsr/gpsr.yaml

@@ -1,7 +1,7 @@
 behaviors_main:
   ros__parameters:
     use_sim_time: False
-    cam_frame: "head_front_camera_link_color_optical_frame"
+    cam_frame: "head_front_camera_color_optical_frame"
     home_position: [54.701, 1.360, 0.001]
     home_pose: "home"
     offer_pose: "offer"

robocup_bringup/config/real_time_params.yaml

@@ -1,7 +1,6 @@
 /**:
   ros__parameters:
-    use_sim_time: false
-    allow_duplicate_names: true
+    allow_duplicate_names: True
 
 attention_server:
   ros__parameters:

robocup_bringup/launch/carry_my_luggage_dependencies.launch.py

@@ -101,13 +101,12 @@ def generate_launch_description():
             os.path.join(navigation_dir, 'launch', 'navigation_system.launch.py')
         ),
         launch_arguments={
-            'rviz': 'True',
+            'rviz': 'False',
             'mode': 'amcl',
             'params_file': package_dir + '/config/carry_my_luggage/tiago_nav_params.yaml',
             'slam_params_file': package_dir +
                     '/config/carry_my_luggage/tiago_nav_follow_params.yaml',
-            'map': os.path.join(
-                                package_dir,
+            'map': os.path.join(package_dir,
                                 'maps',
                                 'carry_map.yaml'),
         }.items()
@@ -120,7 +119,7 @@ def generate_launch_description():
     ld.add_action(audio_common_tts_node)
     ld.add_action(yolo3d)
     ld.add_action(real_time)
-    ld.add_action(move_group)
-    ld.add_action(manipulation_server)
+    # ld.add_action(move_group)
+    # ld.add_action(manipulation_server)
 
     return ld

robocup_bringup/launch/receptionist_dependencies.launch.py

@@ -63,7 +63,7 @@ def generate_launch_description():
             'input_depth_topic': '/head_front_camera/depth/image_raw',
             'input_depth_info_topic': '/head_front_camera/depth/camera_info',
             'depth_image_units_divisor': '1000',  # 1 for simulation, 1000 real
-            'target_frame': 'head_front_camera_link_color_optical_frame',
+            'target_frame': 'head_front_camera_color_optical_frame',
             'threshold': '0.5'
             }.items()
     )

robocup_bringup/src/behaviors_main.cpp

@@ -50,7 +50,7 @@ int main(int argc, char * argv[])
   RCLCPP_INFO(node->get_logger(), "Loading BT: [%s]", xml_file.c_str());
 
   auto blackboard = BT::Blackboard::create();
-  blackboard->set("node", node);
+  blackboard->set("node", std::static_pointer_cast<rclcpp_cascade_lifecycle::CascadeLifecycleNode>(node));
   BT::Tree tree = factory.createTreeFromFile(xml_file, blackboard);
 
   auto publisher_zmq = std::make_shared<BT::PublisherZMQ>(tree, 10, 1666, 1667);