update
This commit is contained in:
parent
acc6fd38ab
commit
87acd2dd33
|
|
@ -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<Action> 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<Action> 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_;
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user