Merge branch 'humble-devel' of https://github.com/CoreSenseEU/CoreSense4Home into humble-devel

Author: igonzf

bt_nodes/arm/src/manipulation/point_at.cpp

@@ -63,8 +63,7 @@ void PointAt::on_tick()
     // goal_.pose.pose.position.z = 0.5;
     // goal_.pose.pose.orientation = transform_.transform.rotation;
     auto angle = std::atan2(transform_.transform.translation.y, transform_.transform.translation.x);
-    auto pitch = std::atan2(transform_.transform.translation.z, transform_.transform.translation.x);
-    double desired_radius = 0.9;  //0.5
+    double desired_radius = 0.7;  //0.5
     auto x_point = desired_radius * std::cos(angle);
     auto y_point = desired_radius * std::sin(angle);
     goal_.pose.pose.position.x = (std::isnan(x_point) || std::isinf(x_point)) ? 0.0 : (x_point);

bt_nodes/motion/include/motion/head/Pan.hpp

@@ -42,16 +42,15 @@ class Pan : public BT::ActionNodeBase
       {
         BT::InputPort<double>("range"),     // in degrees
         BT::InputPort<double>("period"),    // in seconds
-        BT::InputPort<double>("pitch_angle") // in degrees
+        BT::InputPort<double>("pitch_angle"), // in degrees
+        BT::InputPort<double>("max_vel_yaw") // in rad/s (optional)
       });
   }
 
 private:
-  // std::shared_ptr<rclcpp_cascade_lifecycle::CascadeLifecycleNode> node_;
   std::shared_ptr<rclcpp_cascade_lifecycle::CascadeLifecycleNode> node_;
   rclcpp::Time start_time_;
-  rclcpp_lifecycle::LifecyclePublisher<trajectory_msgs::msg::JointTrajectory>::SharedPtr
-    joint_cmd_pub_;
+  rclcpp_lifecycle::LifecyclePublisher<trajectory_msgs::msg::JointTrajectory>::SharedPtr joint_cmd_pub_;
   double yaw_limit_{1.3};
   double pitch_limit_{0.92};
   double pitch_{0.0};
@@ -64,8 +63,9 @@ class Pan : public BT::ActionNodeBase
   double phase_;
   double phase_offset_{0.0};
   double initial_yaw_{0.0};
-
   int current_position_{0};
+  double max_vel_yaw_{1.0}; // rad/s, default value
+  rclcpp::Time last_tick_time_;
 
   double get_joint_yaw(double period, double range, double time, double phase);
 };

bt_nodes/motion/src/motion/head/Pan.cpp

@@ -81,13 +81,13 @@ double Pan::get_joint_yaw(double period, double range, double time, double phase
 BT::NodeStatus Pan::tick()
 {
   rclcpp::spin_some(node_->get_node_base_interface());
-  bool is_first_tick = false;
+  // bool is_first_tick = false;
 
   if (status() == BT::NodeStatus::IDLE) {
     // node_->remove_activation("attention_server");
     start_time_ = node_->now();
     initial_yaw_ = phase_;  // Store the actual starting position
-    is_first_tick = true;
+    // is_first_tick = true;
 
     // Calculate phase so the sine wave starts exactly at the current position
     // The sine wave equation is: yaw = range * sin(2π/period * t + phase)
@@ -128,8 +128,9 @@ BT::NodeStatus Pan::tick()
   command_msg.points[0].positions[1] = std::clamp(pitch_angle_, -pitch_limit_, pitch_limit_);
   command_msg.points[0].velocities[0] = 0.0;
   command_msg.points[0].velocities[1] = 0.0;
-  // Use 1.0 second for first command to smoothly transition, then 0.1 for rest
-  command_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(is_first_tick ? 1.0 : 0.1);
+  double yaw_diff = std::abs(yaw - phase_);
+  double time_to_reach = yaw_diff / 1.5;  // 1.5 is max velocity
+  command_msg.points[0].time_from_start = rclcpp::Duration::from_seconds(time_to_reach);
   joint_cmd_pub_->publish(command_msg);
   rclcpp::spin_some(node_->get_node_base_interface());
 

robocup_bringup/config/real_time_params.yaml

@@ -6,10 +6,11 @@ attention_server:
   ros__parameters:
     period: 0.05
     max_yaw: 1.3
-    max_pitch: 0.3 #0.185
-    max_vel_yaw: 0.01
-    max_vel_pitch: 0.01
-
+    max_pitch: 0.3
+    max_vel_yaw: 1.75 
+    max_vel_pitch: 1.75
+    rotation_threshold: 0.436332 # 25 degrees
+    
 perception_people_detection:
   ros__parameters:
     target_frame: base_footprint

robocup_bringup/launch/dialog.launch.py

@@ -34,6 +34,14 @@ def generate_launch_description():
     model_repo = LaunchConfiguration('model_repo')
     model_filename = LaunchConfiguration('model_filename')
 
+    declare_model_repo_cmd = DeclareLaunchArgument(
+        'model_repo', default_value='ggerganov/whisper.cpp',
+        description='Hugging Face model repo')
+
+    declare_model_filename_cmd = DeclareLaunchArgument(
+        'model_filename', default_value='ggml-large-v3-q5_0.bin',
+        description='Hugging Face model filename')
+
     # Actions
     llama_cmd = create_llama_launch(
             n_ctx=2048,
@@ -111,7 +119,8 @@ def generate_launch_description():
     )
 
     ld = LaunchDescription()
-
+    ld.add_action(declare_model_repo_cmd)
+    ld.add_action(declare_model_filename_cmd)
     ld.add_action(whisper_cmd)
     ld.add_action(llava_cmd)
     ld.add_action(audio_common_tts_node)

robocup_bringup/maps/maps_experiments.pgm

@@ -0,0 +1,7 @@
+image: maps_experiments.pgm
+mode: trinary
+resolution: 0.05
+origin: [-6.97, -13.2, 0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.25

robocup_bringup/maps/maps_experiments.yaml

@@ -0,0 +1,7 @@
+image: new_lab.pgm
+mode: trinary
+resolution: 0.05
+origin: [-4.11, -9.06, 0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.25

robocup_bringup/maps/new_lab.pgm

@@ -61,7 +61,7 @@ repositories:
     version: main
   systems/attention_system:
     type: git
-    url: https://github.com/aaggj/attention_system
+    url: https://github.com/juandpenan/attention_system
     version: humble
   systems/navigation_system:
     type: git