HiepLM/AMR_T800
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update

Browse Source
This commit is contained in:
HiepLM
2026-03-30 17:48:44 +07:00
parent 39bc1796af
commit ca9bfca40c
6 changed files with 64 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
Controllers/Packages/amr_control/src/amr_publiser.cpp
View File

@@ -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);
}

20
Controllers/Packages/amr_control/src/amr_vda_5050_client_api.cpp
View File

@@ -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;

91
Controllers/Packages/amr_control/src/tf_converter.cpp
View File

@@ -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)