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/carry_my_luggage.xml b/robocup_bringup/bt_xml/carry_my_luggage.xml index 97cf0f3..e8ed21c 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" /> @@ -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"/> - - + 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}" /> - + 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 @@ - + diff --git a/robocup_bringup/bt_xml/storing_groceries.xml b/robocup_bringup/bt_xml/storing_groceries.xml index f791de4..63f18ef 100644 --- a/robocup_bringup/bt_xml/storing_groceries.xml +++ b/robocup_bringup/bt_xml/storing_groceries.xml @@ -8,14 +8,17 @@ - + + + + @@ -33,15 +36,18 @@ max_depth="5" max_entities="1" order="depth" - best_detection="{best_detection}"/> + best_detection="{best_detection}" + pub_bb_img="true"/> + + 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" 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 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: 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', 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 + 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',