From ace61e98e1ef8a9250392d627a569d07d70d9706 Mon Sep 17 00:00:00 2001 From: juan Date: Wed, 17 Jul 2024 11:11:48 +0200 Subject: [PATCH 01/14] carry my luggage filter person fine tuned --- robocup_bringup/bt_xml/carry_my_luggage.xml | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/robocup_bringup/bt_xml/carry_my_luggage.xml b/robocup_bringup/bt_xml/carry_my_luggage.xml index 97cf0f3..1a27091 100644 --- a/robocup_bringup/bt_xml/carry_my_luggage.xml +++ b/robocup_bringup/bt_xml/carry_my_luggage.xml @@ -32,23 +32,22 @@ - - + + - - + - + @@ -60,7 +59,13 @@ - + @@ -69,7 +74,6 @@ robot_distance_to_person="1.0" frame="person_0_filtered" check_time="9.0"/> - Date: Wed, 17 Jul 2024 11:12:50 +0200 Subject: [PATCH 02/14] changed door forward distance to avoiod colliding with the person --- robocup_bringup/bt_xml/robot_inspection.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robocup_bringup/bt_xml/robot_inspection.xml b/robocup_bringup/bt_xml/robot_inspection.xml index 2f0a1d2..bdeb65f 100644 --- a/robocup_bringup/bt_xml/robot_inspection.xml +++ b/robocup_bringup/bt_xml/robot_inspection.xml @@ -9,7 +9,7 @@ - + From f6b04eb4db6a88a6c7a87eeb1e7adccfd9b9853d Mon Sep 17 00:00:00 2001 From: juan Date: Wed, 17 Jul 2024 11:18:02 +0200 Subject: [PATCH 03/14] storing groceries torso lift position for detecting objects updated --- robocup_bringup/bt_xml/storing_groceries.xml | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/robocup_bringup/bt_xml/storing_groceries.xml b/robocup_bringup/bt_xml/storing_groceries.xml index f791de4..e359865 100644 --- a/robocup_bringup/bt_xml/storing_groceries.xml +++ b/robocup_bringup/bt_xml/storing_groceries.xml @@ -8,14 +8,17 @@ - + + + + @@ -40,6 +43,7 @@ result="{possible_pick}"/> + From 4d320c45678832452082fcb6de55825a70b38a27 Mon Sep 17 00:00:00 2001 From: juan Date: Wed, 17 Jul 2024 11:22:45 +0200 Subject: [PATCH 04/14] llama model for gpsr commented --- robocup_bringup/launch/dialog.launch.py | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/robocup_bringup/launch/dialog.launch.py b/robocup_bringup/launch/dialog.launch.py index fcae8ff..dae5514 100644 --- a/robocup_bringup/launch/dialog.launch.py +++ b/robocup_bringup/launch/dialog.launch.py @@ -49,8 +49,13 @@ def generate_launch_description(): n_threads=4, n_predict=-1, - model_repo="cstr/Spaetzle-v60-7b-Q4_0-GGUF", - model_filename="Spaetzle-v60-7b_Q4_0.gguf", + # uncomment this for GPSR: + # model_repo="cstr/Spaetzle-v60-7b-Q4_0-GGUF", + # model_filename="Spaetzle-v60-7b_Q4_0.gguf", + + # comment this for GPSR: + model_repo='TheBloke/Marcoroni-7B-v3-GGUF', + model_filename='marcoroni-7b-v3.Q3_K_L.gguf', prefix='\n\n### Instruction:\n', suffix='\n\n### Response:\n', From 4927a1137b44b082560d9d7a32305ca82c0ceff3 Mon Sep 17 00:00:00 2001 From: juan Date: Wed, 17 Jul 2024 11:23:33 +0200 Subject: [PATCH 05/14] map updated for receptinoist --- robocup_bringup/launch/receptionist_dependencies.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robocup_bringup/launch/receptionist_dependencies.launch.py b/robocup_bringup/launch/receptionist_dependencies.launch.py index 198861c..13d2c5b 100644 --- a/robocup_bringup/launch/receptionist_dependencies.launch.py +++ b/robocup_bringup/launch/receptionist_dependencies.launch.py @@ -80,7 +80,7 @@ def generate_launch_description(): ), launch_arguments={ 'rviz': 'True', - 'map': package_dir + '/maps/ir_lab.yaml', + 'map': package_dir + '/maps/robocup_arena_1.yaml', 'params_file': package_dir + '/config/receptionist/tiago_nav_params.yaml', 'slam_params_file': package_dir + From 3c1b5bd49fb074b95969e84a0eedf12d6bb9a479 Mon Sep 17 00:00:00 2001 From: juan Date: Wed, 17 Jul 2024 11:24:27 +0200 Subject: [PATCH 06/14] workaround for yolo model pending to be updated with custom --- robocup_bringup/launch/storing_dependencies.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robocup_bringup/launch/storing_dependencies.launch.py b/robocup_bringup/launch/storing_dependencies.launch.py index e569aa5..e5a072f 100644 --- a/robocup_bringup/launch/storing_dependencies.launch.py +++ b/robocup_bringup/launch/storing_dependencies.launch.py @@ -107,7 +107,7 @@ def generate_launch_description(): ), launch_arguments={ # 'namespace': 'perception_system', - 'model': yolo_model, # change to pretrained + 'model': "yolov8x-seg.pt", # change to pretrained 'input_image_topic': '/head_front_camera/rgb/image_raw', 'input_depth_topic': '/head_front_camera/depth/image_raw', 'input_depth_info_topic': '/head_front_camera/depth/camera_info', From 4cc89f639db5b62c6f707e6b8b1b61be35da639a Mon Sep 17 00:00:00 2001 From: juan Date: Wed, 17 Jul 2024 11:37:06 +0200 Subject: [PATCH 07/14] added publish bounding box in is_detected --- .../perception/include/perception/IsDetected.hpp | 6 +++--- bt_nodes/perception/src/perception/IsDetected.cpp | 14 ++++++-------- robocup_bringup/bt_xml/storing_groceries.xml | 4 +++- 3 files changed, 12 insertions(+), 12 deletions(-) diff --git a/bt_nodes/perception/include/perception/IsDetected.hpp b/bt_nodes/perception/include/perception/IsDetected.hpp index 62e1ea7..3ef2c93 100644 --- a/bt_nodes/perception/include/perception/IsDetected.hpp +++ b/bt_nodes/perception/include/perception/IsDetected.hpp @@ -67,7 +67,7 @@ class IsDetected : public BT::ConditionNode } private: - void image_callback(const sensor_msg::msg::Image::SharedPtr msg); + void image_callback(const sensor_msgs::msg::Image::SharedPtr msg); std::shared_ptr node_; std::shared_ptr tf_buffer_; @@ -89,8 +89,8 @@ class IsDetected : public BT::ConditionNode std::map pose_names_; bool pub_bb_img_{false}; - rclcpp::Publisher::SharedPtr bb_img_pub_; - rclcpp::Subscription::SharedPtr img_sub_; + rclcpp::Publisher::SharedPtr bb_img_pub_; + rclcpp::Subscription::SharedPtr img_sub_; cv::Mat last_image_; }; diff --git a/bt_nodes/perception/src/perception/IsDetected.cpp b/bt_nodes/perception/src/perception/IsDetected.cpp index a44bc77..998cdfa 100644 --- a/bt_nodes/perception/src/perception/IsDetected.cpp +++ b/bt_nodes/perception/src/perception/IsDetected.cpp @@ -69,13 +69,13 @@ IsDetected::IsDetected(const std::string & xml_tag_name, const BT::NodeConfigura getInput("pub_bb_img", pub_bb_img_); if (pub_bb_img_) { - pub_bb_img_ = node_->create_publisher( + bb_img_pub_ = node_->create_publisher( "/bb_img_best_detection", 10); img_sub_ = node_->create_subscription( "/camera/color/image_raw", 10, std::bind(&IsDetected::image_callback, this, _1)); } else { - pub_bb_img_ = nullptr; + bb_img_pub_ = nullptr; img_sub_ = nullptr; } @@ -233,9 +233,6 @@ BT::NodeStatus IsDetected::tick() RCLCPP_INFO(node_->get_logger(), "[IsDetected] %d detections after filter", frames_.size()); } - - - // pub->publish(detections[0].image); setOutput("best_detection", detections[0].class_name); @@ -250,7 +247,7 @@ BT::NodeStatus IsDetected::tick() cv::Scalar(0, 0, 255), 2); auto msg = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", last_image_).toImageMsg(); - pub_bb_img_->publish(*msg); + bb_img_pub_->publish(*msg); } RCLCPP_DEBUG(node_->get_logger(), "[IsDetected] Detections sorted"); @@ -269,13 +266,14 @@ void IsDetected::image_callback(const sensor_msgs::msg::Image::SharedPtr msg) { cv_bridge::CvImagePtr image_rgb_ptr; try { - image_rgb_ptr = cv_bridge::toCvCopy(msg->source_img, sensor_msgs::image_encodings::BGR8); + image_rgb_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); } catch (cv_bridge::Exception & e) { - RCLCPP_ERROR(get_logger(), "cv_bridge exception: %s", e.what()); + RCLCPP_ERROR(node_->get_logger(), "cv_bridge exception: %s", e.what()); return; } last_image_ = image_rgb_ptr->image; +} } // namespace perception BT_REGISTER_NODES(factory) { diff --git a/robocup_bringup/bt_xml/storing_groceries.xml b/robocup_bringup/bt_xml/storing_groceries.xml index e359865..63f18ef 100644 --- a/robocup_bringup/bt_xml/storing_groceries.xml +++ b/robocup_bringup/bt_xml/storing_groceries.xml @@ -36,7 +36,8 @@ max_depth="5" max_entities="1" order="depth" - best_detection="{best_detection}"/> + best_detection="{best_detection}" + pub_bb_img="true"/> + From 3228e694b49d61713c89d2a31edab800d5aaf0b2 Mon Sep 17 00:00:00 2001 From: igonzf Date: Wed, 17 Jul 2024 11:39:47 +0200 Subject: [PATCH 08/14] going to the same location in carry my luggage --- robocup_bringup/bt_xml/carry_my_luggage.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robocup_bringup/bt_xml/carry_my_luggage.xml b/robocup_bringup/bt_xml/carry_my_luggage.xml index 97cf0f3..38fa6bc 100644 --- a/robocup_bringup/bt_xml/carry_my_luggage.xml +++ b/robocup_bringup/bt_xml/carry_my_luggage.xml @@ -10,7 +10,7 @@ frame_name="initial" initial_pose="{going_back}" x_offset="0.0" - y_offset="2.0" + y_offset="0.0" /> From e6cba7e0c918777a989620a0f6b1b03972bb3e73 Mon Sep 17 00:00:00 2001 From: igonzf Date: Wed, 17 Jul 2024 11:40:29 +0200 Subject: [PATCH 09/14] gpsr updated --- robocup_bringup/bt_xml/gpsr.xml | 2 +- robocup_bringup/config/gpsr/gpsr.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/robocup_bringup/bt_xml/gpsr.xml b/robocup_bringup/bt_xml/gpsr.xml index 26bf927..48ce54e 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 2947d1d..be8d5b1 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_color_optical_frame" + cam_frame: "head_front_camera_link_color_optical_frame" home_position: [54.701, 1.360, 0.001] home_pose: "home" offer_pose: "offer" From 44f511da7419ae9633a4c1dfc97483bbea014a43 Mon Sep 17 00:00:00 2001 From: igonzf Date: Wed, 17 Jul 2024 11:41:29 +0200 Subject: [PATCH 10/14] avoid collition with referee in ispection --- robocup_bringup/bt_xml/robot_inspection.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robocup_bringup/bt_xml/robot_inspection.xml b/robocup_bringup/bt_xml/robot_inspection.xml index 2f0a1d2..92b73ef 100644 --- a/robocup_bringup/bt_xml/robot_inspection.xml +++ b/robocup_bringup/bt_xml/robot_inspection.xml @@ -9,7 +9,7 @@ - + From e1ead1688bcb7659fab4365211ac21eb35f0eea9 Mon Sep 17 00:00:00 2001 From: igonzf Date: Wed, 17 Jul 2024 11:42:14 +0200 Subject: [PATCH 11/14] inspection nav params changed to avoid colliding with a person --- robocup_bringup/config/inspection/tiago_nav_params.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/robocup_bringup/config/inspection/tiago_nav_params.yaml b/robocup_bringup/config/inspection/tiago_nav_params.yaml index 15e3512..5c1aff4 100644 --- a/robocup_bringup/config/inspection/tiago_nav_params.yaml +++ b/robocup_bringup/config/inspection/tiago_nav_params.yaml @@ -223,7 +223,7 @@ local_costmap: inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 3.0 - inflation_radius: 0.55 + inflation_radius: 0.40 voxel_layer: plugin: "nav2_costmap_2d::VoxelLayer" enabled: True @@ -278,7 +278,7 @@ global_costmap: robot_radius: 0.29 resolution: 0.05 track_unknown_space: true - plugins: ["static_layer", "inflation_layer"] + plugins: ["static_layer","obstacle_layer", "inflation_layer"] obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" enabled: True From e986dab6c731f368b5d4cac10e77ef66936820b8 Mon Sep 17 00:00:00 2001 From: igonzf Date: Wed, 17 Jul 2024 11:43:11 +0200 Subject: [PATCH 12/14] waypoints for receptionist added --- .../config/receptionist/receptionist_params.yaml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/robocup_bringup/config/receptionist/receptionist_params.yaml b/robocup_bringup/config/receptionist/receptionist_params.yaml index 7d31935..a2ccc60 100644 --- a/robocup_bringup/config/receptionist/receptionist_params.yaml +++ b/robocup_bringup/config/receptionist/receptionist_params.yaml @@ -7,11 +7,11 @@ offer_pose: "offer" person_id: 001122334455 waypoints_names: ["entrance", "party", "guest_confirmation"] - # allow_duplicate_names: false + # ARENA C: waypoints: # [x, y, yaw] only 3 numbers!! - entrance: [-2.065, -2.304, 1.174] - party: [-2.725, -3.593, -2.210] - guest_confirmation: [-2.725, -3.593, 0.482] + entrance: [10.552, -8.863, -2.737] + party: [15.103, -8.220, -0.446] + guest_confirmation: [15.103, -8.220, -1.949] behaviors_main: ros__parameters: From ae977ce96bfae6b1cb7a3aa84dfe7e9b4db7db89 Mon Sep 17 00:00:00 2001 From: igonzf Date: Wed, 17 Jul 2024 11:43:52 +0200 Subject: [PATCH 13/14] map updated for receptionist --- robocup_bringup/launch/receptionist_dependencies.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robocup_bringup/launch/receptionist_dependencies.launch.py b/robocup_bringup/launch/receptionist_dependencies.launch.py index 198861c..13d2c5b 100644 --- a/robocup_bringup/launch/receptionist_dependencies.launch.py +++ b/robocup_bringup/launch/receptionist_dependencies.launch.py @@ -80,7 +80,7 @@ def generate_launch_description(): ), launch_arguments={ 'rviz': 'True', - 'map': package_dir + '/maps/ir_lab.yaml', + 'map': package_dir + '/maps/robocup_arena_1.yaml', 'params_file': package_dir + '/config/receptionist/tiago_nav_params.yaml', 'slam_params_file': package_dir + From fb6dfe7af9e24e795647dbfac1c10f91e64e5ca3 Mon Sep 17 00:00:00 2001 From: igonzf Date: Wed, 17 Jul 2024 11:45:31 +0200 Subject: [PATCH 14/14] looking for person fixed --- robocup_bringup/bt_xml/receptionist.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robocup_bringup/bt_xml/receptionist.xml b/robocup_bringup/bt_xml/receptionist.xml index f2793ab..9c7d18f 100644 --- a/robocup_bringup/bt_xml/receptionist.xml +++ b/robocup_bringup/bt_xml/receptionist.xml @@ -274,7 +274,7 @@ order="color" best_detection="{best_detection}" /> - +