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