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());
+        const robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(poses, "map", robot::Time::now());
-        // goal = path.poses.back();
+        goal = path.poses.back();
-        // VDA5050ClientAPI::plan_pub_.publish(path);
       }
       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;
+        goal.pose.position.z = 0;
-        // transform.setRotation(tf3::Quaternion(0.0, 0.0, position.theta));
+        tf3::Quaternion quat;
-        // goal = tf3::toMsg(transform);
+        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,51 +28,7 @@ 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_)
-  {
-    ros::spinOnce();
-    tree = tfBuffer.allFramesAsString();
-    if (!tree.empty() && tree == last_tree)
-    {
-      count_tf_receive_done++;
-    }
-    else
-    {
-      count_tf_receive_done = 0;
-    }
-
-    if(count_tf_receive_done > 2)
-    {
-      ROS_WARN("TF tree stabilized: \n%s", tree.c_str());
-      break;
-    }
-
-    last_tree = tree;
-    
-    rate.sleep();
-  }
-
-  std::istringstream iss(tree);
-
-  while (ros::ok() && std::getline(iss, line) && !stop_tf_thread_)
-  {
-      // Frame X exists with parent Y
-      std::size_t f1 = line.find("Frame ");
-      std::size_t f2 = line.find(" exists with parent ");
-
-      if (f1 != std::string::npos && f2 != std::string::npos)
-      {
-          std::string child  = line.substr(f1 + 6, f2 - (f1 + 6));
-          std::string parent = line.substr(f2 + 20);
-
-          if (!parent.empty() && parent.back() == '.')
-          parent.pop_back();
-
-          edges.push_back({parent, child});
-          ROS_WARN("parent: %s | child: %s", parent.c_str(), child.c_str());
-      }
-  }
 
   while (ros::ok() && !stop_tf_thread_)
   {
@@ -82,6 +38,51 @@ void amr_control::TfConverter::tfWorker()
       continue;
     }
 
+    while (ros::ok() && !stop_tf_thread_)
+    {
+      ros::spinOnce();
+      tree = tfBuffer.allFramesAsString();
+      if (!tree.empty() && tree == last_tree)
+      {
+        count_tf_receive_done++;
+      }
+      else
+      {
+        count_tf_receive_done = 0;
+      }
+
+      if(count_tf_receive_done > 2)
+      {
+        // ROS_WARN("TF tree stabilized: \n%s", tree.c_str());
+        break;
+      }
+
+      last_tree = tree;
+      
+      rate.sleep();
+    }
+
+    std::istringstream iss(tree);
+
+    while (ros::ok() && std::getline(iss, line) && !stop_tf_thread_)
+    {
+        // Frame X exists with parent Y
+        std::size_t f1 = line.find("Frame ");
+        std::size_t f2 = line.find(" exists with parent ");
+
+        if (f1 != std::string::npos && f2 != std::string::npos)
+        {
+            std::string child  = line.substr(f1 + 6, f2 - (f1 + 6));
+            std::string parent = line.substr(f2 + 20);
+
+            if (!parent.empty() && parent.back() == '.')
+            parent.pop_back();
+
+            edges.push_back({parent, child});
+            // ROS_WARN("parent: %s | child: %s", parent.c_str(), child.c_str());
+        }
+    }
+
     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_x"   default="1.0" />
-    <arg name="robot_y"   default="34.5" />
+    <arg name="robot_y"   default="-7.7" />
-    <arg name="robot_yaw" default="1.57079" />
+    <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 linear  = rear_wheel_est_vel * cos(front_steer_pos);
-    //const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_w_;
     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);