From ca9bfca40cdb159568acff9232f25ea48c1749e0 Mon Sep 17 00:00:00 2001 From: HiepLM Date: Mon, 30 Mar 2026 17:48:44 +0700 Subject: [PATCH] update --- .../Packages/amr_control/src/amr_publiser.cpp | 4 - .../src/amr_vda_5050_client_api.cpp | 20 ++-- .../Packages/amr_control/src/tf_converter.cpp | 91 ++++++++++--------- .../launch/includes/spawn_trolley.launch | 6 +- .../steer_drive_controller/src/odometry.cpp | 7 +- pnkx_nav_core | 2 +- 6 files changed, 64 insertions(+), 66 deletions(-) diff --git a/Controllers/Packages/amr_control/src/amr_publiser.cpp b/Controllers/Packages/amr_control/src/amr_publiser.cpp index 4ec96d5..e8cd385 100644 --- a/Controllers/Packages/amr_control/src/amr_publiser.cpp +++ b/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); } diff --git a/Controllers/Packages/amr_control/src/amr_vda_5050_client_api.cpp b/Controllers/Packages/amr_control/src/amr_vda_5050_client_api.cpp index b113e3c..308e16e 100644 --- a/Controllers/Packages/amr_control/src/amr_vda_5050_client_api.cpp +++ b/Controllers/Packages/amr_control/src/amr_vda_5050_client_api.cpp @@ -287,18 +287,17 @@ namespace amr_control if (!paths.empty()) return; - std::vector poses; + std::vector 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; diff --git a/Controllers/Packages/amr_control/src/tf_converter.cpp b/Controllers/Packages/amr_control/src/tf_converter.cpp index baacd81..4d82e20 100644 --- a/Controllers/Packages/amr_control/src/tf_converter.cpp +++ b/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); - 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()); - } - } + ros::Rate rate(50); 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) diff --git a/Controllers/Packages/amr_startup/launch/includes/spawn_trolley.launch b/Controllers/Packages/amr_startup/launch/includes/spawn_trolley.launch index b223b41..e75ee6a 100644 --- a/Controllers/Packages/amr_startup/launch/includes/spawn_trolley.launch +++ b/Controllers/Packages/amr_startup/launch/includes/spawn_trolley.launch @@ -1,8 +1,8 @@ - - - + + +