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-01-13 14:49:00 +07:00
parent acc6fd38ab
commit 87acd2dd33
1 changed files with 77 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -156,94 +156,97 @@ namespace amr_control
const robot_nav_msgs::Path path = robot_nav_2d_utils::poses2DToPath(poses, "map", robot::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); // VDA5050ClientAPI::plan_pub_.publish(path);
VDA5050ClientAPI::move_base_ptr_->moveTo(goal);
} }
else if (global_plan_msg_type_ == std::string("vda5050_msgs::Order")) else if (global_plan_msg_type_ == std::string("vda5050_msgs::Order"))
{ {
vda5050_msgs::Order order_msg; // vda5050_msgs::Order order_msg;
order_msg.headerId = order.headerId; // order_msg.headerId = order.headerId;
order_msg.timestamp = order.timestamp; // order_msg.timestamp = order.timestamp;
order_msg.version = order.version; // order_msg.version = order.version;
order_msg.manufacturer = order.manufacturer; // order_msg.manufacturer = order.manufacturer;
order_msg.serialNumber = order.serialNumber; // order_msg.serialNumber = order.serialNumber;
order_msg.orderId = order.orderId; // order_msg.orderId = order.orderId;
order_msg.orderUpdateId = order.orderUpdateId; // order_msg.orderUpdateId = order.orderUpdateId;
for (auto node : order.nodes) // for (auto node : order.nodes)
{ // {
vda5050_msgs::Node node_msg; // vda5050_msgs::Node node_msg;
node_msg.nodeId = node.nodeId; // node_msg.nodeId = node.nodeId;
node_msg.sequenceId = node.sequenceId; // node_msg.sequenceId = node.sequenceId;
node_msg.nodeDescription = node.nodeDescription; // node_msg.nodeDescription = node.nodeDescription;
node_msg.released = node.released; // node_msg.released = node.released;
node_msg.nodePosition.x = node.nodePosition.x; // node_msg.nodePosition.x = node.nodePosition.x;
node_msg.nodePosition.y = node.nodePosition.y; // node_msg.nodePosition.y = node.nodePosition.y;
node_msg.nodePosition.theta = node.nodePosition.theta; // node_msg.nodePosition.theta = node.nodePosition.theta;
node_msg.nodePosition.allowedDeviationXY = node.nodePosition.allowedDeviationXY; // node_msg.nodePosition.allowedDeviationXY = node.nodePosition.allowedDeviationXY;
node_msg.nodePosition.allowedDeviationTheta = node.nodePosition.allowedDeviationTheta; // node_msg.nodePosition.allowedDeviationTheta = node.nodePosition.allowedDeviationTheta;
node_msg.nodePosition.mapId = node.nodePosition.mapId; // node_msg.nodePosition.mapId = node.nodePosition.mapId;
node_msg.nodePosition.mapDescription = node.nodePosition.mapDescription; // node_msg.nodePosition.mapDescription = node.nodePosition.mapDescription;
// vda5050_msgs/Action[] actions // // vda5050_msgs/Action[] actions
order_msg.nodes.push_back(node_msg); // order_msg.nodes.push_back(node_msg);
} // }
for (auto edge : order.edges) // for (auto edge : order.edges)
{ // {
vda5050_msgs::Edge edge_msg; // vda5050_msgs::Edge edge_msg;
edge_msg.edgeId = edge.edgeId; // edge_msg.edgeId = edge.edgeId;
edge_msg.sequenceId = edge.sequenceId; // edge_msg.sequenceId = edge.sequenceId;
edge_msg.edgeDescription = edge.edgeDescription; // edge_msg.edgeDescription = edge.edgeDescription;
edge_msg.released = edge.released; // edge_msg.released = edge.released;
edge_msg.startNodeId = edge.startNodeId; // edge_msg.startNodeId = edge.startNodeId;
edge_msg.endNodeId = edge.endNodeId; // edge_msg.endNodeId = edge.endNodeId;
edge_msg.maxSpeed = edge.maxSpeed; // edge_msg.maxSpeed = edge.maxSpeed;
edge_msg.maxHeight = edge.maxHeight; // edge_msg.maxHeight = edge.maxHeight;
edge_msg.minHeight = edge.minHeight; // edge_msg.minHeight = edge.minHeight;
edge_msg.orientation = edge.orientation; // edge_msg.orientation = edge.orientation;
edge_msg.orientationType = edge.orientationType; // edge_msg.orientationType = edge.orientationType;
edge_msg.direction = edge.direction; // edge_msg.direction = edge.direction;
edge_msg.rotationAllowed = edge.rotationAllowed; // edge_msg.rotationAllowed = edge.rotationAllowed;
edge_msg.maxRotationSpeed = edge.maxRotationSpeed; // edge_msg.maxRotationSpeed = edge.maxRotationSpeed;
edge_msg.length = edge.length; // edge_msg.length = edge.length;
edge_msg.trajectory.degree = edge.trajectory.degree; // edge_msg.trajectory.degree = edge.trajectory.degree;
edge_msg.trajectory.knotVector = edge.trajectory.knotVector; // edge_msg.trajectory.knotVector = edge.trajectory.knotVector;
for (auto controlPoint : edge.trajectory.controlPoints) // for (auto controlPoint : edge.trajectory.controlPoints)
{ // {
vda5050_msgs::ControlPoint controlPoint_msg; // vda5050_msgs::ControlPoint controlPoint_msg;
controlPoint_msg.x = controlPoint.x; // controlPoint_msg.x = controlPoint.x;
controlPoint_msg.y = controlPoint.y; // controlPoint_msg.y = controlPoint.y;
controlPoint_msg.weight = controlPoint.weight; // controlPoint_msg.weight = controlPoint.weight;
edge_msg.trajectory.controlPoints.push_back(controlPoint_msg); // edge_msg.trajectory.controlPoints.push_back(controlPoint_msg);
} // }
// Corridor corridor; // // Corridor corridor;
// std::vector<Action> actions; // // std::vector<Action> actions;
order_msg.edges.push_back(edge_msg); // order_msg.edges.push_back(edge_msg);
} // }
// order_msg.edges zoneSetId =VDA5050ClientAPI::client_ptr_->vda5050_order_. // // order_msg.edges zoneSetId =VDA5050ClientAPI::client_ptr_->vda5050_order_.
VDA5050ClientAPI::plan_pub_.publish(order_msg); // VDA5050ClientAPI::plan_pub_.publish(order_msg);
vda_5050::NodePosition position = order.nodes.back().nodePosition; // vda_5050::NodePosition position = order.nodes.back().nodePosition;
goal.header.stamp = robot::Time::now(); // goal.header.stamp = robot::Time::now();
goal.header.frame_id = "map"; // goal.header.frame_id = "map";
goal.pose.position.x = position.x; // goal.pose.position.x = position.x;
goal.pose.position.y = position.y; // goal.pose.position.y = position.y;
goal.pose.position.z = 0.0; // goal.pose.position.z = 0.0;
// Convert theta (yaw) to Quaternion using setRPY // // Convert theta (yaw) to Quaternion using setRPY
// setRPY(roll, pitch, yaw) - for 2D navigation, only yaw is used // // setRPY(roll, pitch, yaw) - for 2D navigation, only yaw is used
tf3::Quaternion quat; // tf3::Quaternion quat;
quat.setRPY(0.0, 0.0, position.theta); // quat.setRPY(0.0, 0.0, position.theta);
goal.pose.orientation.x = quat.x(); // goal.pose.orientation.x = quat.x();
goal.pose.orientation.y = quat.y(); // goal.pose.orientation.y = quat.y();
goal.pose.orientation.z = quat.z(); // goal.pose.orientation.z = quat.z();
goal.pose.orientation.w = quat.w(); // goal.pose.orientation.w = quat.w();
robot_protocol_msgs::Order order_msg;
VDA5050ClientAPI::move_base_ptr_->moveTo(order_msg, goal);
} }
else else
return; return;
VDA5050ClientAPI::move_base_ptr_->moveTo(goal);
cmd_vel_max_.x = 0.2; cmd_vel_max_.x = 0.2;
cmd_vel_max_.theta = 0.5; cmd_vel_max_.theta = 0.5;
cmd_vel_max_saved_ = cmd_vel_max_; cmd_vel_max_saved_ = cmd_vel_max_;