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
Show Stats 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);
}
}

124
Controllers/Packages/amr_startup/rviz/navigation.rviz
View File

@ -7,6 +7,8 @@ Panels:
- /Global Options1
- /Grid1/Offset1
- /TF1/Frames1
- /Global Map1
- /Local Map1
Splitter Ratio: 0.37295082211494446
Tree Height: 682
- Class: rviz/Selection
@ -363,13 +365,13 @@ Visualization Manager:
Buffer Length: 1
Class: rviz/Path
Color: 25; 255; 0
Enabled: false
Enabled: true
Head Diameter: 0.30000001192092896
Head Length: 0.20000000298023224
Length: 0.20000000298023224
Line Style: Lines
Line Width: 0.05000000074505806
Name: Full Plan
Name: Plan
Offset:
X: 0
Y: 0
@ -380,9 +382,9 @@ Visualization Manager:
Radius: 0.019999999552965164
Shaft Diameter: 0.10000000149011612
Shaft Length: 0.10000000149011612
Topic: /amr_node/SBPLLatticePlanner/plan
Topic: /global_costmap/costmap/plan
Unreliable: false
Value: false
Value: true
- Alpha: 1
Class: rviz/Polygon
Color: 255; 0; 0
@ -392,82 +394,10 @@ Visualization Manager:
Topic: /global_costmap/costmap/footprint
Unreliable: false
Value: true
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 25; 255; 0
Enabled: true
Head Diameter: 0.30000001192092896
Head Length: 0.20000000298023224
Length: 0.00800000037997961
Line Style: Billboards
Line Width: 0.009999999776482582
Name: CustomPath
Offset:
X: 0
Y: 0
Z: 0.8999999761581421
Pose Color: 255; 85; 255
Pose Style: None
Queue Size: 10
Radius: 0.004999999888241291
Shaft Diameter: 0.10000000149011612
Shaft Length: 0.10000000149011612
Topic: /amr_node/CustomPlanner/plan
Unreliable: false
Value: true
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 0; 0; 0
Enabled: false
Head Diameter: 0.30000001192092896
Head Length: 0.20000000298023224
Length: 0.30000001192092896
Line Style: Lines
Line Width: 0.029999999329447746
Name: Straight Path
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 255; 85; 255
Pose Style: Axes
Queue Size: 10
Radius: 0.029999999329447746
Shaft Diameter: 0.10000000149011612
Shaft Length: 0.10000000149011612
Topic: /amr_node/LocalPlannerAdapter/MKTGoStraightLocalPlanner/global_plan
Unreliable: false
Value: false
Enabled: true
Name: Global Map
- Class: rviz/Group
Displays:
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 252; 175; 62
Enabled: false
Head Diameter: 0.30000001192092896
Head Length: 0.20000000298023224
Length: 0.03999999910593033
Line Style: Lines
Line Width: 0.029999999329447746
Name: Global Path
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 255; 85; 255
Pose Style: Axes
Queue Size: 10
Radius: 0.009999999776482582
Shaft Diameter: 0.10000000149011612
Shaft Length: 0.10000000149011612
Topic: /amr_node/LocalPlannerAdapter/global_plan
Unreliable: false
Value: false
- Alpha: 0.699999988079071
Class: rviz/Map
Color Scheme: map
@ -491,13 +421,13 @@ Visualization Manager:
Buffer Length: 1
Class: rviz/Path
Color: 138; 226; 52
Enabled: false
Enabled: true
Head Diameter: 0.30000001192092896
Head Length: 0.20000000298023224
Length: 0.029999999329447746
Line Style: Billboards
Line Width: 0.029999999329447746
Name: Local Plan
Name: Plan
Offset:
X: 0
Y: 0
@ -508,37 +438,9 @@ Visualization Manager:
Radius: 0.009999999776482582
Shaft Diameter: 0.10000000149011612
Shaft Length: 0.10000000149011612
Topic: /amr_node/LocalPlannerAdapter/transformed_global_plan
Topic: /local_costmap/costmap/plan
Unreliable: false
Value: false
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0
Enabled: false
Invert Rainbow: false
Max Color: 255; 255; 255
Min Color: 0; 0; 0
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.009999999776482582
Style: Flat Squares
Topic: /amr_node/PredictiveTrajectory/cost_cloud
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: false
- Class: rviz/Group
Displays:
- Alpha: 1
@ -702,10 +604,10 @@ Visualization Manager:
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Scale: -62.776824951171875
Scale: -105.0660171508789
Target Frame: base_link
X: 1.189023494720459
Y: 0.44669991731643677
X: 2.555877447128296
Y: 1.0760011672973633
Saved:
- Angle: -34.55989074707031
Class: rviz/TopDownOrtho
@ -727,7 +629,7 @@ Window Geometry:
Height: 833
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd000000040000000000000156000002e7fc0200000008fb000000100044006900730070006c006100790073010000003d000002e7000000c900fffffffb0000001200530065006c0065006300740069006f006e000000003d000002590000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001b500000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000000a0049006d006100670065000000003d0000039e0000000000000000fb0000000c00430061006d006500720061020000007c0000013600000280000001e0fb0000000c00430061006d0065007200610200000644000000b7000002c1000002f9000000010000010f00000259fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000259000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000041800000168fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007520000003efc0100000002fb0000000800540069006d00650000000000000007520000041800fffffffb0000000800540069006d00650100000000000004500000000000000000000002d9000002e700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
QMainWindow State: 000000ff00000000fd000000040000000000000156000002e7fc0200000008fb000000100044006900730070006c006100790073000000003d000002e7000000c900fffffffb0000001200530065006c0065006300740069006f006e000000003d000002590000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001b500000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000000a0049006d006100670065000000003d0000039e0000000000000000fb0000000c00430061006d006500720061020000007c0000013600000280000001e0fb0000000c00430061006d0065007200610200000644000000b7000002c1000002f9000000010000010f00000259fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000259000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000041800000168fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007520000003efc0100000002fb0000000800540069006d00650000000000000007520000041800fffffffb0000000800540069006d0065010000000000000450000000000000000000000435000002e700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:

2
pnkx_nav_core

@ -1 +1 @@
Subproject commit 57b77ac14b144acddccc95cc9dd7e8d4d58728dc
Subproject commit 0f58db34810790067177408f37dc0becb91620c1