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-14 17:53:41 +07:00
parent 06756618e4
commit 8b476879a7
6 changed files with 386 additions and 234 deletions
Download Patch File Download Diff File Expand all files Collapse all files

73
Controllers/Packages/amr_control/include/amr_control/amr_publiser.h
View File

@@ -3,8 +3,17 @@
#include <ros/ros.h>
#include <robot/robot.h>
#include <nav_msgs/OccupancyGrid.h>
#include <geometry_msgs/PolygonStamped.h>
#include <geometry_msgs/Twist.h>
#include <robot_nav_2d_msgs/Twist2DStamped.h>
#include <robot_nav_2d_msgs/Path2D.h>
#include <robot_nav_2d_utils/conversions.h>
#include <nav_2d_utils/conversions.h>
#include <nav_2d_msgs/Path2D.h>
#include <nav_msgs/Path.h>
#include <nav_msgs/OccupancyGrid.h>
#include <robot_nav_msgs/OccupancyGrid.h>
#include <robot_map_msgs/OccupancyGridUpdate.h>
#include <map_msgs/OccupancyGridUpdate.h>
@@ -14,12 +23,13 @@
namespace amr_control
{
struct CostmapObject
struct PlannerObject
{
// ROS publishers
ros::Publisher pub_;
ros::Publisher update_pub_;
ros::Publisher footprint_pub_;
ros::Publisher plan_pub_;
ros::Timer timer_;
std::string topic_;
bool active_;
@@ -47,6 +57,16 @@ namespace amr_control
*/
virtual ~AmrPublisher();
/**
* @brief Publish global plan to RViz
*/
void publishGlobalPlan();
/**
* @brief Publish local plan to RViz
*/
void publishLocalPlan();
/**
* @brief Publish global costmap to RViz
* @param publish_rate Rate at which to publish (Hz). If 0, publish immediately once.
@@ -96,7 +116,7 @@ namespace amr_control
* @brief Check if publishing is active
* @return True if publishing is active (either global or local)
*/
bool isPublishing() const { return global_costmap_obj_.active_ || local_costmap_obj_.active_ || cmd_vel_publishing_active_; }
bool isPublishing() const { return global_planner_obj_.active_ || local_planner_obj_.active_ || cmd_vel_publishing_active_; }
/**
* @brief Start publishing cmd_vel at a fixed rate using ros::WallTimer
@@ -109,7 +129,28 @@ namespace amr_control
*/
void stopCmdVelPublishing();
/**
* @brief Start publishing plans at a fixed rate
* @param global_rate Publishing rate for global plan (Hz). If 0, don't publish global.
* @param local_rate Publishing rate for local plan (Hz). If 0, don't publish local.
*/
void startPlanPublishing(double global_rate = 10.0, double local_rate = 10.0);
/**
* @brief Stop publishing plans
*/
void stopPlanPublishing();
private:
/**
* @brief Convert robot_nav_2d_msgs::Path2D to nav_msgs::Path
* @param robot_path Input robot_nav_2d_msgs::Path2D
* @param nav_path Output nav_msgs::Path
*/
void convertToNavPath(const robot_nav_2d_msgs::Path2D &robot_path,
nav_msgs::Path &nav_path);
/**
* @brief Callback for new subscription to global costmap
* @param pub SingleSubscriberPublisher for the new subscription
@@ -164,16 +205,38 @@ namespace amr_control
*/
void cmdVelTimerCallback(const ros::WallTimerEvent &event);
/**
* @brief Timer callback for updating global plan
* @param event Timer event
*/
void globalPlanTimerCallback(const ros::TimerEvent &event);
/**
* @brief Timer callback for updating local plan
* @param event Timer event
*/
void localPlanTimerCallback(const ros::TimerEvent &event);
ros::NodeHandle nh_;
robot::move_base_core::BaseNavigation::Ptr move_base_ptr_;
CostmapObject global_costmap_obj_;
CostmapObject local_costmap_obj_;
PlannerObject global_planner_obj_;
PlannerObject local_planner_obj_;
ros::Publisher cmd_vel_pub_;
// ros::WallTimer for cmd_vel publishing
ros::WallTimer cmd_vel_timer_;
bool cmd_vel_publishing_active_;
// Timers for plan publishing
ros::Timer global_plan_timer_;
ros::Timer local_plan_timer_;
bool global_plan_publishing_active_;
bool local_plan_publishing_active_;
// Cached plans with timestamps
robot_nav_2d_msgs::Path2D cached_global_plan_;
robot_nav_2d_msgs::Path2D cached_local_plan_;
};
} // namespace amr_control

12
Controllers/Packages/amr_control/src/amr_control.cpp
View File

@@ -77,11 +77,12 @@ namespace amr_control
vda_5050_client_api_ptr_.reset();
}
// Stop costmap and cmd_vel publishing
// Stop costmap, cmd_vel and plan publishing
if (amr_publisher_ptr_)
{
amr_publisher_ptr_->stopPublishing();
amr_publisher_ptr_->stopCmdVelPublishing();
amr_publisher_ptr_->stopPlanPublishing();
amr_publisher_ptr_.reset();
}
}
@@ -339,6 +340,15 @@ namespace amr_control
nh.param("cmd_vel_publish_rate", cmd_vel_rate, cmd_vel_rate);
robot::log_info("[%s:%d]\n Starting cmd_vel publishing at %.2f Hz...", __FILE__, __LINE__, cmd_vel_rate);
amr_publisher_ptr_->startCmdVelPublishing(cmd_vel_rate);
// Start publishing plans using Timer
double global_plan_rate = 10.0; // Default: 10 Hz
double local_plan_rate = 10.0; // Default: 10 Hz
nh.param("global_plan_publish_rate", global_plan_rate, global_plan_rate);
nh.param("local_plan_publish_rate", local_plan_rate, local_plan_rate);
robot::log_info("[%s:%d]\n Starting plan publishing - Global: %.2f Hz, Local: %.2f Hz...",
__FILE__, __LINE__, global_plan_rate, local_plan_rate);
amr_publisher_ptr_->startPlanPublishing(global_plan_rate, local_plan_rate);
}
}
}

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

@@ -3,8 +3,7 @@
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <robot_costmap_2d/cost_values.h>
#include <boost/bind.hpp>
#include <robot_nav_2d_msgs/Twist2DStamped.h>
#include <geometry_msgs/Twist.h>
namespace amr_control
{
@@ -22,29 +21,34 @@ AmrPublisher::AmrPublisher(ros::NodeHandle &nh, robot::move_base_core::BaseNavig
nh_.param("local_costmap_topic", local_costmap_topic, local_costmap_topic);
nh_.param("cmd_vel_topic", cmd_vel_topic, cmd_vel_topic);
// Initialize publishers with callback for new subscriptions
global_costmap_obj_.pub_ = nh_.advertise<nav_msgs::OccupancyGrid>(
global_planner_obj_.pub_ = nh_.advertise<nav_msgs::OccupancyGrid>(
global_costmap_topic, 1,
boost::bind(&AmrPublisher::onNewSubscriptionGlobal, this, _1));
global_costmap_obj_.update_pub_ = nh_.advertise<map_msgs::OccupancyGridUpdate>(global_costmap_topic + "_updates", 1);
global_costmap_obj_.footprint_pub_ = nh_.advertise<geometry_msgs::PolygonStamped>(global_costmap_topic + "/footprint", 1);
global_planner_obj_.update_pub_ = nh_.advertise<map_msgs::OccupancyGridUpdate>(global_costmap_topic + "_updates", 1);
global_planner_obj_.footprint_pub_ = nh_.advertise<geometry_msgs::PolygonStamped>(global_costmap_topic + "/footprint", 1);
global_planner_obj_.plan_pub_ = nh_.advertise< nav_msgs::Path>(global_costmap_topic + "/plan", 1);
local_costmap_obj_.pub_ = nh_.advertise<nav_msgs::OccupancyGrid>(
local_planner_obj_.pub_ = nh_.advertise<nav_msgs::OccupancyGrid>(
local_costmap_topic, 1,
boost::bind(&AmrPublisher::onNewSubscriptionLocal, this, _1));
local_costmap_obj_.update_pub_ = nh_.advertise<map_msgs::OccupancyGridUpdate>(local_costmap_topic + "_updates", 1);
local_costmap_obj_.footprint_pub_ = nh_.advertise<geometry_msgs::PolygonStamped>(local_costmap_topic + "/footprint", 1);
local_planner_obj_.update_pub_ = nh_.advertise<map_msgs::OccupancyGridUpdate>(local_costmap_topic + "_updates", 1);
local_planner_obj_.footprint_pub_ = nh_.advertise<geometry_msgs::PolygonStamped>(local_costmap_topic + "/footprint", 1);
local_planner_obj_.plan_pub_ = nh_.advertise< nav_msgs::Path>(local_costmap_topic + "/plan", 1);
cmd_vel_pub_ = nh_.advertise<geometry_msgs::Twist>(cmd_vel_topic, 1);
cmd_vel_publishing_active_ = false;
robot::log_info("[AmrPublisher] Initialized. Cmd vel topic: %s", cmd_vel_topic.c_str());
// Initialize topic names in CostmapObject
global_costmap_obj_.topic_ = global_costmap_topic;
local_costmap_obj_.topic_ = local_costmap_topic;
global_planner_obj_.topic_ = global_costmap_topic;
local_planner_obj_.topic_ = local_costmap_topic;
// Initialize other CostmapObject members
global_costmap_obj_.active_ = false;
local_costmap_obj_.active_ = false;
global_planner_obj_.active_ = false;
local_planner_obj_.active_ = false;
// Initialize plan publishing flags
global_plan_publishing_active_ = false;
local_plan_publishing_active_ = false;
robot::log_info("[AmrPublisher] Initialized. Global costmap topic: %s, Local costmap topic: %s",
@@ -58,6 +62,7 @@ AmrPublisher::~AmrPublisher()
{
stopPublishing();
stopCmdVelPublishing();
stopPlanPublishing();
}
void AmrPublisher::convertToNavOccupancyGrid(const robot_nav_msgs::OccupancyGrid &robot_grid,
@@ -138,13 +143,13 @@ void AmrPublisher::publishGlobalCostmap(double publish_rate)
{
nav_msgs::OccupancyGrid nav_grid;
convertToNavOccupancyGrid(global_data.costmap, nav_grid);
global_costmap_obj_.pub_.publish(nav_grid);
global_planner_obj_.pub_.publish(nav_grid);
}
else
{
map_msgs::OccupancyGridUpdate grid_update;
convertToRobotOccupancyGridUpdate(global_data.costmap_update, grid_update);
global_costmap_obj_.update_pub_.publish(grid_update);
global_planner_obj_.update_pub_.publish(grid_update);
}
// Publish footprint
@@ -177,13 +182,13 @@ void AmrPublisher::publishLocalCostmap(double publish_rate)
{
nav_msgs::OccupancyGrid nav_grid;
convertToNavOccupancyGrid(local_data.costmap, nav_grid);
local_costmap_obj_.pub_.publish(nav_grid);
local_planner_obj_.pub_.publish(nav_grid);
}
else
{
map_msgs::OccupancyGridUpdate grid_update;
convertToRobotOccupancyGridUpdate(local_data.costmap_update, grid_update);
local_costmap_obj_.update_pub_.publish(grid_update);
local_planner_obj_.update_pub_.publish(grid_update);
}
// Publish footprint
@@ -220,7 +225,7 @@ void AmrPublisher::publishGlobalFootprint()
// Convert and publish footprint
geometry_msgs::PolygonStamped nav_footprint;
convertToNavPolygonStamped(global_data.footprint, nav_footprint);
global_costmap_obj_.footprint_pub_.publish(nav_footprint);
global_planner_obj_.footprint_pub_.publish(nav_footprint);
// robot::log_debug("[AmrPublisher] Published global footprint with %zu points, frame_id='%s'",
// nav_footprint.polygon.points.size(), nav_footprint.header.frame_id.c_str());
@@ -247,7 +252,7 @@ void AmrPublisher::publishLocalFootprint()
// Convert and publish footprint
geometry_msgs::PolygonStamped nav_footprint;
convertToNavPolygonStamped(local_data.footprint, nav_footprint);
local_costmap_obj_.footprint_pub_.publish(nav_footprint);
local_planner_obj_.footprint_pub_.publish(nav_footprint);
// robot::log_debug("[AmrPublisher] Published local footprint with %zu points, frame_id='%s'",
// nav_footprint.polygon.points.size(), nav_footprint.header.frame_id.c_str());
@@ -271,8 +276,8 @@ void AmrPublisher::startPublishing(double global_rate, double local_rate)
// Start global costmap timer if rate > 0
if (global_rate > 0.0)
{
global_costmap_obj_.active_ = true;
global_costmap_obj_.timer_ = nh_.createTimer(
global_planner_obj_.active_ = true;
global_planner_obj_.timer_ = nh_.createTimer(
ros::Duration(1.0 / global_rate),
&AmrPublisher::globalCostmapTimerCallback,
this
@@ -283,8 +288,8 @@ void AmrPublisher::startPublishing(double global_rate, double local_rate)
// Start local costmap timer if rate > 0
if (local_rate > 0.0)
{
local_costmap_obj_.active_ = true;
local_costmap_obj_.timer_ = nh_.createTimer(
local_planner_obj_.active_ = true;
local_planner_obj_.timer_ = nh_.createTimer(
ros::Duration(1.0 / local_rate),
&AmrPublisher::localCostmapTimerCallback,
this
@@ -296,16 +301,16 @@ void AmrPublisher::startPublishing(double global_rate, double local_rate)
void AmrPublisher::stopPublishing()
{
// Stop timers
if (global_costmap_obj_.active_)
if (global_planner_obj_.active_)
{
global_costmap_obj_.timer_.stop();
global_costmap_obj_.active_ = false;
global_planner_obj_.timer_.stop();
global_planner_obj_.active_ = false;
}
if (local_costmap_obj_.active_)
if (local_planner_obj_.active_)
{
local_costmap_obj_.timer_.stop();
local_costmap_obj_.active_ = false;
local_planner_obj_.timer_.stop();
local_planner_obj_.active_ = false;
}
robot::log_info("[AmrPublisher] Stopped publishing costmaps");
@@ -313,7 +318,7 @@ void AmrPublisher::stopPublishing()
void AmrPublisher::globalCostmapTimerCallback(const ros::TimerEvent &event)
{
if (global_costmap_obj_.active_)
if (global_planner_obj_.active_)
{
publishGlobalCostmap();
}
@@ -321,7 +326,7 @@ void AmrPublisher::globalCostmapTimerCallback(const ros::TimerEvent &event)
void AmrPublisher::localCostmapTimerCallback(const ros::TimerEvent &event)
{
if (local_costmap_obj_.active_)
if (local_planner_obj_.active_)
{
publishLocalCostmap();
}
@@ -333,6 +338,61 @@ void AmrPublisher::onNewSubscriptionLocal(const ros::SingleSubscriberPublisher&
publishLocalCostmap();
}
void AmrPublisher::convertToNavPath(const robot_nav_2d_msgs::Path2D &robot_path,
nav_msgs::Path &nav_path)
{
// Convert header
nav_2d_msgs::Path2D path2d;
path2d.header.seq = robot_path.header.seq;
path2d.header.frame_id = robot_path.header.frame_id;
path2d.header.stamp = ros::Time::now();
path2d.poses.resize(robot_path.poses.size());
for (unsigned int i = 0; i < robot_path.poses.size(); i++)
{
path2d.poses[i].pose.x = robot_path.poses[i].pose.x;
path2d.poses[i].pose.y = robot_path.poses[i].pose.y;
path2d.poses[i].pose.theta = robot_path.poses[i].pose.theta;
}
nav_path = nav_2d_utils::pathToPath(path2d);
}
void AmrPublisher::publishGlobalPlan()
{
if(!move_base_ptr_)
{
return;
}
// Check if plan is empty
if(cached_global_plan_.poses.empty())
{
return;
}
robot::Time now = robot::Time::now();
robot::Time plan_stamp = cached_global_plan_.header.stamp;
// Check if timestamp is zero (uninitialized)
if(plan_stamp.sec == 0 && plan_stamp.nsec == 0)
{
return;
}
robot::Duration age = now - plan_stamp;
robot::Duration max_age(0.5);
// Check if timestamp is not older than 0.5 seconds
if(plan_stamp < now - max_age)
{
return;
}
nav_msgs::Path nav_path;
convertToNavPath(cached_global_plan_, nav_path);
global_planner_obj_.plan_pub_.publish(nav_path);
}
void AmrPublisher::onNewSubscriptionGlobal(const ros::SingleSubscriberPublisher& pub)
{
robot::log_info("[AmrPublisher] New subscription: %s", pub.getSubscriberName().c_str());
@@ -405,5 +465,128 @@ void AmrPublisher::cmdVelTimerCallback(const ros::WallTimerEvent &event)
}
}
void AmrPublisher::publishLocalPlan()
{
if(!move_base_ptr_)
{
return;
}
// Check if plan is empty
if(cached_local_plan_.poses.empty())
{
return;
}
robot::Time now = robot::Time::now();
robot::Time plan_stamp = cached_local_plan_.header.stamp;
// Check if timestamp is zero (uninitialized)
if(plan_stamp.sec == 0 && plan_stamp.nsec == 0)
{
return;
}
robot::Duration age = now - plan_stamp;
robot::Duration max_age(0.5);
// Check if timestamp is not older than 0.5 seconds
if(plan_stamp < now - max_age)
{
return;
}
nav_msgs::Path nav_path;
convertToNavPath(cached_local_plan_, nav_path);
local_planner_obj_.plan_pub_.publish(nav_path);
}
void AmrPublisher::globalPlanTimerCallback(const ros::TimerEvent &event)
{
if (!move_base_ptr_)
{
return;
}
// Update cached global plan
try
{
cached_global_plan_ = move_base_ptr_->getGlobalData().plan;
// Publish if timestamp is valid
publishGlobalPlan();
}
catch (const std::exception &e)
{
robot::log_error("[AmrPublisher] Error updating global plan: %s", e.what());
}
}
void AmrPublisher::localPlanTimerCallback(const ros::TimerEvent &event)
{
if (!move_base_ptr_)
{
return;
}
// Update cached local plan
try
{
cached_local_plan_ = move_base_ptr_->getLocalData().plan;
// Publish if timestamp is valid
publishLocalPlan();
}
catch (const std::exception &e)
{
robot::log_error("[AmrPublisher] Error updating local plan: %s", e.what());
}
}
void AmrPublisher::startPlanPublishing(double global_rate, double local_rate)
{
stopPlanPublishing(); // Stop any existing publishing first
// Start global plan timer if rate > 0
if (global_rate > 0.0)
{
global_plan_publishing_active_ = true;
global_plan_timer_ = nh_.createTimer(
ros::Duration(1.0 / global_rate),
&AmrPublisher::globalPlanTimerCallback,
this
);
robot::log_info("[AmrPublisher] Started publishing global plan at %.2f Hz", global_rate);
}
// Start local plan timer if rate > 0
if (local_rate > 0.0)
{
local_plan_publishing_active_ = true;
local_plan_timer_ = nh_.createTimer(
ros::Duration(1.0 / local_rate),
&AmrPublisher::localPlanTimerCallback,
this
);
robot::log_info("[AmrPublisher] Started publishing local plan at %.2f Hz", local_rate);
}
}
void AmrPublisher::stopPlanPublishing()
{
// Stop timers
if (global_plan_publishing_active_)
{
global_plan_timer_.stop();
global_plan_publishing_active_ = false;
}
if (local_plan_publishing_active_)
{
local_plan_timer_.stop();
local_plan_publishing_active_ = false;
}
robot::log_info("[AmrPublisher] Stopped publishing plans");
}
} // namespace amr_control

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

@@ -160,88 +160,85 @@ namespace amr_control
}
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;
// 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);
// }
// 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);
// }
// // 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;
// // 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;
// 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)
{
robot_protocol_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);
}
for (auto edge : order.edges)
{
robot_protocol_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)
{
robot_protocol_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);
}
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();
VDA5050ClientAPI::move_base_ptr_->moveTo(order_msg, goal);
}
else
@@ -260,7 +257,6 @@ namespace amr_control
robot_geometry_msgs::Vector3 angular;
angular.z = cmd_vel_max_.theta;
VDA5050ClientAPI::move_base_ptr_->setTwistAngular(angular);
}
}
@@ -397,8 +393,6 @@ namespace amr_control
robot_geometry_msgs::Vector3 angular;
angular.z = cmd_vel_max_.theta;
VDA5050ClientAPI::move_base_ptr_->setTwistAngular(angular);
}
}