update

Controllers/Packages/amr_control/src/amr_publiser.cpp

@@ -441,7 +441,6 @@ void AmrPublisher::publishGlobalPlan()
   {
     return;
   }
-  
   robot::Time now = robot::Time::now();
   robot::Time plan_stamp = cached_global_plan_.header.stamp;
   
@@ -450,7 +449,6 @@ void AmrPublisher::publishGlobalPlan()
   {
     return;
   }
-  
   robot::Duration age = now - plan_stamp;
   robot::Duration max_age(0.5);
   
@@ -459,10 +457,8 @@ void AmrPublisher::publishGlobalPlan()
   {
     return;
   }
-  
   nav_msgs::Path nav_path;
   convertToNavPath(cached_global_plan_, nav_path);
-  
   global_planner_obj_.plan_pub_.publish(nav_path);
 }
 

Controllers/Packages/amr_control/src/amr_vda_5050_client_api.cpp

@@ -287,18 +287,17 @@ namespace amr_control
         if (!paths.empty())
           return;
 
-        std::vector<geometry_msgs::Pose2D> poses;
+        std::vector<robot_geometry_msgs::Pose2D> poses;
         for (auto &p : paths)
         {
-          geometry_msgs::Pose2D p_2d;
+          robot_geometry_msgs::Pose2D p_2d;
           p_2d.x = p.getX();
           p_2d.y = p.getY();
           p_2d.theta = p.getYaw();
           poses.push_back(p_2d);
         }
-        // const nav_msgs::Path path = nav_2d_utils::poses2DToPath(poses, "map", ros::Time::now());
-        // goal = path.poses.back();
-        // VDA5050ClientAPI::plan_pub_.publish(path);
+        const robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(poses, "map", robot::Time::now());
+        goal = path.poses.back();
       }
       else if (global_plan_msg_type_ == std::string("vda5050_msgs::Order"))
       {
@@ -371,14 +370,17 @@ namespace amr_control
         goal.header.frame_id = "map";
         goal.pose.position.x = position.x;
         goal.pose.position.y = position.y;
-        // tf3::Transform transform;
-        // transform.setRotation(tf3::Quaternion(0.0, 0.0, position.theta));
-        // goal = tf3::toMsg(transform);
+        goal.pose.position.z = 0;
+        tf3::Quaternion quat;
+        quat.setRPY(0.0, 0.0, position.theta + M_PI);
+        goal.pose.orientation.x = quat.x();
+        goal.pose.orientation.y = quat.y();
+        goal.pose.orientation.z = quat.z();
+        goal.pose.orientation.w = quat.w();
       }
       else
         return;
 
-      ros::Duration(0.1).sleep();
       VDA5050ClientAPI::move_base_ptr_->dockTo("trolley", goal, 0.2, 0.1);
       cmd_vel_max_.x = 0.2;
       cmd_vel_max_.theta = 0.5;

Controllers/Packages/amr_control/src/tf_converter.cpp

@@ -28,7 +28,16 @@ void amr_control::TfConverter::tfWorker()
   std::string line;
 
   int count_tf_receive_done = 0;
-  ros::Rate rate(20);
+  ros::Rate rate(50);
+
+  while (ros::ok() && !stop_tf_thread_)
+  {
+    if (!tf_ || !tf3_buffer_)
+    {
+      rate.sleep();
+      continue;
+    }
+
     while (ros::ok() && !stop_tf_thread_)
     {
       ros::spinOnce();
@@ -44,7 +53,7 @@ void amr_control::TfConverter::tfWorker()
 
       if(count_tf_receive_done > 2)
       {
-      ROS_WARN("TF tree stabilized: \n%s", tree.c_str());
+        // ROS_WARN("TF tree stabilized: \n%s", tree.c_str());
         break;
       }
 
@@ -70,18 +79,10 @@ void amr_control::TfConverter::tfWorker()
             parent.pop_back();
 
             edges.push_back({parent, child});
-          ROS_WARN("parent: %s | child: %s", parent.c_str(), child.c_str());
+            // ROS_WARN("parent: %s | child: %s", parent.c_str(), child.c_str());
         }
     }
 
-  while (ros::ok() && !stop_tf_thread_)
-  {
-    if (!tf_ || !tf3_buffer_)
-    {
-      rate.sleep();
-      continue;
-    }
-
     try
     {
       for (const auto& e : edges)

Controllers/Packages/amr_startup/launch/includes/spawn_trolley.launch

@@ -1,8 +1,8 @@
 <?xml version="1.0"?>
 <launch>
-    <arg name="robot_x"   default="17.5" />
-    <arg name="robot_y"   default="34.5" />
-    <arg name="robot_yaw" default="1.57079" />
+    <arg name="robot_x"   default="1.0" />
+    <arg name="robot_y"   default="-7.7" />
+    <arg name="robot_yaw" default="0.08" />
     <arg name="model" default="trolley" />
     <node name="spawn_maze" pkg="gazebo_ros" type="spawn_model" 
           args="-sdf -file $(find factory_ss_demo)models/sehc_trolley/model.sdf -model $(arg model) 

Controllers/Packages/robot_gazebo_plugins/steer_drive_controller/src/odometry.cpp

@@ -40,7 +40,7 @@
  */
 
 #include <steer_drive_controller/odometry.h>
-
+#include <ros/ros.h>
 #include <boost/bind.hpp>
 
 namespace steer_drive_controller
@@ -86,10 +86,9 @@ namespace steer_drive_controller
     rear_wheel_old_pos_ = rear_wheel_cur_pos;
 
     /// Compute linear and angular diff:
-    const double linear  = rear_wheel_est_vel * cos(front_steer_pos);//(right_wheel_est_vel + left_wheel_est_vel) * 0.5;
-    //const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_w_;
+    const double linear  = rear_wheel_est_vel * cos(front_steer_pos);
     const double angular = tan(front_steer_pos) * linear / wheel_separation_h_;
-
+    // ROS_INFO("front_steer_pos: %.3f tan(front_steer_pos) %.3f, angular %.3f", front_steer_pos, tan(front_steer_pos), angular);
     /// Integrate odometry:
     integrate_fun_(linear, angular);