diff --git a/.docker/Dockerfile b/.docker/Dockerfile index 1c26fa5..e31dd75 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,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 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/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..729e561 100644 --- a/robocup_bringup/bt_xml/carry_my_luggage.xml +++ b/robocup_bringup/bt_xml/carry_my_luggage.xml @@ -32,10 +32,10 @@ - + - + @@ -45,10 +45,10 @@ - - + + - + @@ -101,15 +101,15 @@ - + - + - + 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/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/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..30372f2 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() @@ -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 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..7eb66e4 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 @@ -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 @@ -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