From b2e84b0f39fa63b2d02c51d623b7f7a3d5f49202 Mon Sep 17 00:00:00 2001 From: Juan Diego Date: Fri, 19 Sep 2025 11:11:42 +0200 Subject: [PATCH 1/5] carry working without arm movements --- bt_nodes/hri/include/hri/dialog/BTActionNode.hpp | 4 +++- bt_nodes/hri/src/hri/check_policy.cpp | 2 +- .../motion/include/motion/navigation/MoveTo.hpp | 2 +- robocup_bringup/bt_xml/carry_my_luggage.xml | 16 ++++++++-------- robocup_bringup/bt_xml/gpsr.xml | 2 +- robocup_bringup/config/gpsr/gpsr.yaml | 2 +- robocup_bringup/config/real_time_params.yaml | 3 +-- .../carry_my_luggage_dependencies.launch.py | 5 ++--- .../launch/receptionist_dependencies.launch.py | 2 +- robocup_bringup/src/behaviors_main.cpp | 2 +- robocup_bringup/thirdparty.repos | 6 +++--- 11 files changed, 23 insertions(+), 23 deletions(-) diff --git a/bt_nodes/hri/include/hri/dialog/BTActionNode.hpp b/bt_nodes/hri/include/hri/dialog/BTActionNode.hpp index fcf65b9..32c0043 100644 --- a/bt_nodes/hri/include/hri/dialog/BTActionNode.hpp +++ b/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 +template class BtActionNode : public BT::ActionNodeBase { public: diff --git a/bt_nodes/hri/src/hri/check_policy.cpp b/bt_nodes/hri/src/hri/check_policy.cpp index 8a74e27..1c5a979 100644 --- a/bt_nodes/hri/src/hri/check_policy.cpp +++ b/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 = diff --git a/bt_nodes/motion/include/motion/navigation/MoveTo.hpp b/bt_nodes/motion/include/motion/navigation/MoveTo.hpp index 16f1415..366400f 100644 --- a/bt_nodes/motion/include/motion/navigation/MoveTo.hpp +++ b/bt_nodes/motion/include/motion/navigation/MoveTo.hpp @@ -56,7 +56,7 @@ class MoveTo : public motion::BtActionNode< { return BT::PortsList( {BT::InputPort("distance_tolerance"), BT::InputPort("tf_frame"), - BT::InputPort("will_finish"), BT::InputPort("is_truncated")}); + BT::InputPort("will_finish"), BT::InputPort("is_truncated")}); } private: diff --git a/robocup_bringup/bt_xml/carry_my_luggage.xml b/robocup_bringup/bt_xml/carry_my_luggage.xml index 91f70ca..8d4d38d 100644 --- a/robocup_bringup/bt_xml/carry_my_luggage.xml +++ b/robocup_bringup/bt_xml/carry_my_luggage.xml @@ -34,9 +34,9 @@ - + @@ -44,12 +44,12 @@ - + @@ -101,14 +101,14 @@ - + - + diff --git a/robocup_bringup/bt_xml/gpsr.xml b/robocup_bringup/bt_xml/gpsr.xml index 48ce54e..26bf927 100644 --- a/robocup_bringup/bt_xml/gpsr.xml +++ b/robocup_bringup/bt_xml/gpsr.xml @@ -2,7 +2,7 @@ - + diff --git a/robocup_bringup/config/gpsr/gpsr.yaml b/robocup_bringup/config/gpsr/gpsr.yaml index 74f0065..25a1f05 100644 --- a/robocup_bringup/config/gpsr/gpsr.yaml +++ b/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" diff --git a/robocup_bringup/config/real_time_params.yaml b/robocup_bringup/config/real_time_params.yaml index cfdaed8..f19a14c 100644 --- a/robocup_bringup/config/real_time_params.yaml +++ b/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: diff --git a/robocup_bringup/launch/carry_my_luggage_dependencies.launch.py b/robocup_bringup/launch/carry_my_luggage_dependencies.launch.py index b21d88c..e793395 100644 --- a/robocup_bringup/launch/carry_my_luggage_dependencies.launch.py +++ b/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() diff --git a/robocup_bringup/launch/receptionist_dependencies.launch.py b/robocup_bringup/launch/receptionist_dependencies.launch.py index d3c5abe..1742131 100644 --- a/robocup_bringup/launch/receptionist_dependencies.launch.py +++ b/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() ) diff --git a/robocup_bringup/src/behaviors_main.cpp b/robocup_bringup/src/behaviors_main.cpp index d0bb976..6a6d259 100644 --- a/robocup_bringup/src/behaviors_main.cpp +++ b/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(node)); BT::Tree tree = factory.createTreeFromFile(xml_file, blackboard); auto publisher_zmq = std::make_shared(tree, 10, 1666, 1667); diff --git a/robocup_bringup/thirdparty.repos b/robocup_bringup/thirdparty.repos index 6ba6565..45b995a 100644 --- a/robocup_bringup/thirdparty.repos +++ b/robocup_bringup/thirdparty.repos @@ -30,11 +30,11 @@ repositories: ThirdParty/yolov8_ros: type: git url: https://github.com/jmguerreroh/yolov8_ros.git - version: main + version: robocup24 ThirdParty/rclcpp_cascade_lifecycle: type: git url: https://github.com/fmrico/cascade_lifecycle.git - version: rolling + version: ad2e905fc4bebcb436af177f35685bf60e821cdc ThirdParty/tts_ros: type: git url: https://github.com/mgonzs13/tts_ros @@ -65,7 +65,7 @@ repositories: version: humble systems/navigation_system: type: git - url: https://github.com/Juancams/navigation_system.git + url: https://github.com/juandpenan/navigation_system.git version: main ThirdParty/gpsr_planning: type: git From fa11554c9eb29a599ca4c6e16dd0012bd5b2406d Mon Sep 17 00:00:00 2001 From: Juan Diego Date: Fri, 19 Sep 2025 11:16:06 +0200 Subject: [PATCH 2/5] updated manipulation repo --- robocup_bringup/thirdparty.repos | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robocup_bringup/thirdparty.repos b/robocup_bringup/thirdparty.repos index 45b995a..7eb66e4 100644 --- a/robocup_bringup/thirdparty.repos +++ b/robocup_bringup/thirdparty.repos @@ -53,7 +53,7 @@ repositories: version: master systems/manipulation_server: type: git - url: https://github.com/rodperex/manipulation_server.git + url: https://github.com/juandpenan/manipulation_server.git version: main systems/perception_system: type: git From 2e198782f9b0ccd097dfca0241d0db7be12a9c26 Mon Sep 17 00:00:00 2001 From: Juan Diego Date: Mon, 29 Sep 2025 11:43:07 +0200 Subject: [PATCH 3/5] changed from manipulation system to play motion2 --- bt_nodes/arm/CMakeLists.txt | 5 ++ .../manipulation/play_motion_predefined.hpp | 59 ++++++++++++++ bt_nodes/arm/package.xml | 1 + .../manipulation/play_motion_predefined.cpp | 79 +++++++++++++++++++ robocup_bringup/bt_xml/carry_my_luggage.xml | 26 +++--- .../carry_my_luggage/carry_my_luggage.yaml | 2 +- .../carry_my_luggage_dependencies.launch.py | 4 +- 7 files changed, 160 insertions(+), 16 deletions(-) create mode 100644 bt_nodes/arm/include/arm/manipulation/play_motion_predefined.hpp create mode 100644 bt_nodes/arm/src/manipulation/play_motion_predefined.cpp diff --git a/bt_nodes/arm/CMakeLists.txt b/bt_nodes/arm/CMakeLists.txt index 717afe2..6632283 100644 --- a/bt_nodes/arm/CMakeLists.txt +++ b/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( 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) diff --git a/bt_nodes/arm/include/arm/manipulation/play_motion_predefined.hpp b/bt_nodes/arm/include/arm/manipulation/play_motion_predefined.hpp new file mode 100644 index 0000000..354c72f --- /dev/null +++ b/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 +#include + +#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("motion_name"), + BT::InputPort("skip_planning") + }); + } + +private: + std::string motion_name_{}; + bool skip_planning_{false}; +}; + +} // namespace manipulation + +#endif // ARM_MANIPULATION__PLAY_MOTION_PREDEFINED_HPP_ diff --git a/bt_nodes/arm/package.xml b/bt_nodes/arm/package.xml index c5e653b..04c7de0 100644 --- a/bt_nodes/arm/package.xml +++ b/bt_nodes/arm/package.xml @@ -13,6 +13,7 @@ rclcpp_action rclcpp_cascade_lifecycle manipulation_interfaces + play_motion2_msgs ament_lint_auto ament_lint_common diff --git a/bt_nodes/arm/src/manipulation/play_motion_predefined.cpp b/bt_nodes/arm/src/manipulation/play_motion_predefined.cpp new file mode 100644 index 0000000..f46e7d6 --- /dev/null +++ b/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 + +#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(name, "/play_motion2", config); + }; + + factory.registerBuilder("PlayMotionPredefined", builder); +} diff --git a/robocup_bringup/bt_xml/carry_my_luggage.xml b/robocup_bringup/bt_xml/carry_my_luggage.xml index 8d4d38d..729e561 100644 --- a/robocup_bringup/bt_xml/carry_my_luggage.xml +++ b/robocup_bringup/bt_xml/carry_my_luggage.xml @@ -32,11 +32,11 @@ - + - + + + @@ -44,12 +44,12 @@ - + + @@ -101,15 +101,15 @@ - + - - + + + + diff --git a/robocup_bringup/config/carry_my_luggage/carry_my_luggage.yaml b/robocup_bringup/config/carry_my_luggage/carry_my_luggage.yaml index 965ebd2..32bbe2b 100644 --- a/robocup_bringup/config/carry_my_luggage/carry_my_luggage.yaml +++ b/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 diff --git a/robocup_bringup/launch/carry_my_luggage_dependencies.launch.py b/robocup_bringup/launch/carry_my_luggage_dependencies.launch.py index e793395..30372f2 100644 --- a/robocup_bringup/launch/carry_my_luggage_dependencies.launch.py +++ b/robocup_bringup/launch/carry_my_luggage_dependencies.launch.py @@ -119,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 From 2880619a67d6e1bdb2530ec8320d88e3662cd0ca Mon Sep 17 00:00:00 2001 From: Juan Diego Date: Mon, 29 Sep 2025 11:52:30 +0200 Subject: [PATCH 4/5] modified docker for CI --- .docker/Dockerfile | 24 ++++++++++-------------- 1 file changed, 10 insertions(+), 14 deletions(-) diff --git a/.docker/Dockerfile b/.docker/Dockerfile index 1c26fa5..58fc281 100644 --- a/.docker/Dockerfile +++ b/.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,28 +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 +# RUN git clone https://github.com/CoreSenseEU/CoreSense4Home.git -b humble-devel +# RUN vcs import --recursive < ./CoreSense4Home/robocup_bringup/thirdparty.repos +COPY src/ /robocup/src/ + WORKDIR /robocup RUN rosdep install --from-paths src --ignore-src -r -y From ee3fb2687b651379b2dd043f44cf8b56bf1be33f Mon Sep 17 00:00:00 2001 From: Juan Diego Date: Mon, 29 Sep 2025 11:56:34 +0200 Subject: [PATCH 5/5] modifed dockerfile --- .docker/Dockerfile | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/.docker/Dockerfile b/.docker/Dockerfile index 58fc281..e31dd75 100644 --- a/.docker/Dockerfile +++ b/.docker/Dockerfile @@ -32,9 +32,8 @@ RUN pip install -r requirements2.txt RUN pip install -r requirements3.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 -COPY src/ /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