Merge pull request #45 from igonzf/deploy

Updated parameters

Author: igonzf

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<rclcpp_cascade_lifecycle::CascadeLifecycleNode> node_;
   std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
 
@@ -89,8 +89,8 @@ class IsDetected : public BT::ConditionNode
   std::map<int, std::string> pose_names_;
 
   bool pub_bb_img_{false};
-  rclcpp::Publisher<sensor_msg::msg::Image>::SharedPtr bb_img_pub_;
-  rclcpp::Subscription<sensor_msg::msg::Image>::SharedPtr img_sub_;
+  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr bb_img_pub_;
+  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr img_sub_;
 
   cv::Mat last_image_;
 };

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<sensor_msgs::msg::Image>(
+    bb_img_pub_ = node_->create_publisher<sensor_msgs::msg::Image>(
       "/bb_img_best_detection", 10);
     img_sub_ = node_->create_subscription<sensor_msgs::msg::Image>(
       "/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) {

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 @@
         <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="Speak" param="" say_text="please put the bag in my gripper"/>
+        <Action ID="MoveToPredefined" pose="offer" group_name="arm_torso"/> 
+        <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"/>
         </Delay>
         <ReactiveSequence>
-  
             <RetryUntilSuccessful num_attempts="-1">
                 <Condition ID="IsDetected" cam_frame="head_front_camera_rgb_optical_frame" confidence="0.6" frames="{frames}" interest="person" max_depth="5" max_entities="1" order="depth"/>
             </RetryUntilSuccessful>
             <Action ID="FilterEntity" frame="person_0" lambda="0.1"/>
             <Action ID="LookAt" tf_frame="person_0_filtered" />
         </ReactiveSequence>
-        <Delay delay_msec="140">
+        <Delay delay_msec="300">
             <Action ID="MoveToPredefined" pose="close" group_name="gripper"/>
         </Delay>
-        <Delay delay_msec="300">
+        <Delay delay_msec="500">
             <Action ID="MoveToPredefined" pose="home" group_name="arm_torso"/>
         </Delay>
         <Action ID="Speak" param="" say_text="Perfect, now i will follow you. Please stop at the end "/>
@@ -60,7 +59,13 @@
                     <Fallback>
                         <ReactiveSequence>
                             <RetryUntilSuccessful num_attempts="-1">
-                                <Condition ID="IsDetected" cam_frame="head_front_camera_rgb_optical_frame" confidence="0.3" person_id="{person_id}" interest="person" max_depth="6.7" max_entities="1" order="color"/>
+                                <Condition ID="IsDetected" cam_frame="head_front_camera_rgb_optical_frame" 
+                                                           confidence="0.6" 
+                                                           person_id="{person_id}"
+                                                           interest="person"
+                                                           max_depth="3.0" 
+                                                           max_entities="2"
+                                                           order="color"/>
                             </RetryUntilSuccessful>
                             <Action ID="FilterEntity" frame="person_0" lambda="0.1"/>
                             <Action ID="LookAt" tf_frame="person_0_filtered"/>
@@ -69,7 +74,6 @@
                                                            robot_distance_to_person="1.0"
                                                            frame="person_0_filtered"
                                                            check_time="9.0"/>
-                            <!-- <Condition ID="IsMoving" frame="person_0" position_buffer_dimension="50" threshold_time="4.0" velocity_tolerance="0.003"/> -->
                             <Action ID="FollowEntity" camera_frame="head_front_camera_rgb_optical_frame"
                                                       distance_tolerance="0.2"
                                                       frame_to_follow="person_0_filtered"

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_color_optical_frame"/>
+            <SetBlackboard output_key="cam_frame" value="head_front_camera_link_color_optical_frame"/>
             <!-- <Action ID="SetStartPosition" frame_name="instruction point" /> -->
             <Action ID="SetWp"/>
 

robocup_bringup/bt_xml/receptionist.xml

@@ -274,7 +274,7 @@
                                     order="color"
                                     best_detection="{best_detection}"
                                     />
-                                <Action ID="MoveAlongAxis" speed="0.5" distance="6.28" axis="z"/>
+                                <Action ID="MoveAlongAxis" speed="-0.5" distance="6.28" axis="z"/>
                             </ReactiveFallback>
                             <Action ID="LookAt" tf_frame="{best_detection}"/>
 

robocup_bringup/bt_xml/robot_inspection.xml

@@ -9,7 +9,7 @@
             <RetryUntilSuccessful num_attempts="-1">
                 <Condition ID="IsDoorOpen" door_thfloatreshold="1.5"/>
             </RetryUntilSuccessful>
-            <Action ID="MoveAlongAxis" speed="0.25" distance="1.8" axis="x"/>
+            <Action ID="MoveAlongAxis" speed="0.25" distance="0.8" axis="x"/>
 
             <Action ID="MoveTo" is_truncated="false" distance_tolerance="0" tf_frame="inspection_zone"/>
             <Action ID="Speak" param="" say_text="Please say ready when you finish the inspection"/>

robocup_bringup/bt_xml/storing_groceries.xml

@@ -8,14 +8,17 @@
             <Action ID="InitGroceries"/>
             <Action ID="Speak" say_text="Hi this is gentlebots. Can you please open the door "/>
 
-            <!-- <RetryUntilSuccessful num_attempts="-1">
+            <RetryUntilSuccessful num_attempts="-1">
                 <Condition ID="IsDoorOpen" door_thfloatreshold="1.5"/>
-            </RetryUntilSuccessful> -->
+            </RetryUntilSuccessful>
+
+            <Action ID="MoveAlongAxis" speed="0.25" distance="1.0" axis="x"/>
+            
             <Repeat num_cycles="5"> 
                 <Sequence>                    
                     <Action ID="MoveTo" distance_tolerance="1" tf_frame="table"/>
                     <Action ID="MoveJoint" joint_name="torso_lift_joint"
-                                           joint_value="0.23"
+                                           joint_value="0.3"
                                            group_name="arm_torso"/>
                     <RetryUntilSuccessful num_attempts="-1">
 
@@ -33,15 +36,18 @@
                                                     max_depth="5"
                                                     max_entities="1"
                                                     order="depth"
-                                                    best_detection="{best_detection}"/>
+                                                    best_detection="{best_detection}"
+                                                    pub_bb_img="true"/>
 
                             <Action ID="RemoveStringSuffix" string_to_remove="{best_detection}"
                                                             suffix="_"
                                                             result="{possible_pick}"/>
                         </Sequence>
                     </RetryUntilSuccessful>
+                    
                     <Action ID="ClearOctomap" />
                     <Action ID="Speak" say_text="I am detecting " param="{possible_pick}"/>
+                    <Action ID="Speak" say_text="as you can see it on my chest"/>                    
                     <Action ID="Speak" say_text="Trying to pick it from the table "/>
                      <RetryUntilSuccessful num_attempts="-1">
                         <Sequence>

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"

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

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: