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:30:31 +07:00
parent 78b11b60fe
commit acc6fd38ab
14 changed files with 335 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,8 +1,6 @@
#include "amr_control/amr_control.h"
#include <robot/plugin_loader_helper.h>
#include <robot/console.h>
#include <robot/node_handle.h>
#include <robot/robot.h>
#include <tf3/buffer_core.h>
#include <geometry_msgs/Vector3.h>
#include <xmlrpcpp/XmlRpcValue.h>
@@ -79,11 +77,12 @@ namespace amr_control
vda_5050_client_api_ptr_.reset();
}
// Stop costmap publishing
if (costmap_publisher_ptr_)
// Stop costmap and cmd_vel publishing
if (amr_publisher_ptr_)
{
costmap_publisher_ptr_->stopPublishing();
costmap_publisher_ptr_.reset();
amr_publisher_ptr_->stopPublishing();
amr_publisher_ptr_->stopCmdVelPublishing();
amr_publisher_ptr_.reset();
}
}
@@ -243,8 +242,7 @@ namespace amr_control
robot::PluginLoaderHelper loader;
std::string path_file_so = loader.findLibraryPath("MoveBase");
move_base_loader_ = boost::dll::import_alias<robot::move_base_core::BaseNavigation::Ptr()>(
path_file_so,
"MoveBase", boost::dll::load_mode::append_decorations);
path_file_so, "MoveBase", boost::dll::load_mode::append_decorations);
robot::log_info("[%s:%d]\n Creating MoveBase instance...", __FILE__, __LINE__);
move_base_ptr_ = move_base_loader_();
@@ -253,7 +251,8 @@ namespace amr_control
// Initialize costmap publisher for RViz visualization
robot::log_info("[%s:%d]\n Initializing costmap publisher...", __FILE__, __LINE__);
costmap_publisher_ptr_ = std::make_shared<amr_control::AmrPublisher>(nh, move_base_ptr_);
amr_publisher_ptr_ = std::make_shared<amr_control::AmrPublisher>(nh, move_base_ptr_);
amr_subscriber_ptr_ = std::make_shared<amr_control::AmrSubscriber>(nh, move_base_ptr_);
ros::Rate r(3);
do
@@ -330,10 +329,16 @@ namespace amr_control
// Start publishing costmaps to RViz
// Global costmap: 1 Hz, Local costmap: 5 Hz
if (costmap_publisher_ptr_)
if (amr_publisher_ptr_)
{
robot::log_info("[%s:%d]\n Starting costmap publishing to RViz...", __FILE__, __LINE__);
costmap_publisher_ptr_->startPublishing(1.0, 5.0);
amr_publisher_ptr_->startPublishing(1.0, 5.0);
// Start publishing cmd_vel using WallTimer
double cmd_vel_rate = 20.0; // Default: 20 Hz
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);
}
}
}

2
Controllers/Packages/amr_control/src/amr_opc_ua_server_api.cpp
View File

@@ -8,7 +8,7 @@
#include <angles/angles.h>
// pnkx core
#include <robot/time.h>
#include <robot/robot.h>
#include <robot_nav_2d_utils/conversions.h>
#include <robot_nav_2d_msgs/Twist2D.h>

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

@@ -1,8 +1,10 @@
#include <amr_control/amr_publiser.h>
#include <robot/console.h>
#include <robot/robot.h>
#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
{
@@ -14,11 +16,11 @@ AmrPublisher::AmrPublisher(ros::NodeHandle &nh, robot::move_base_core::BaseNavig
// Initialize default topic names
std::string global_costmap_topic = "/global_costmap/costmap";
std::string local_costmap_topic = "/local_costmap/costmap";
std::string cmd_vel_topic = "/cmd_vel";
// Get topic names from parameter server if available
nh_.param("global_costmap_topic", global_costmap_topic, global_costmap_topic);
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_costmap_topic, 1,
@@ -32,6 +34,10 @@ AmrPublisher::AmrPublisher(ros::NodeHandle &nh, robot::move_base_core::BaseNavig
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);
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;
@@ -45,11 +51,13 @@ AmrPublisher::AmrPublisher(ros::NodeHandle &nh, robot::move_base_core::BaseNavig
global_costmap_topic.c_str(), local_costmap_topic.c_str());
robot::log_info("[AmrPublisher] Global footprint topic: %s/footprint, Local footprint topic: %s/footprint",
global_costmap_topic.c_str(), local_costmap_topic.c_str());
}
AmrPublisher::~AmrPublisher()
{
stopPublishing();
stopCmdVelPublishing();
}
void AmrPublisher::convertToNavOccupancyGrid(const robot_nav_msgs::OccupancyGrid &robot_grid,
@@ -331,5 +339,71 @@ void AmrPublisher::onNewSubscriptionGlobal(const ros::SingleSubscriberPublisher&
publishGlobalCostmap();
}
void AmrPublisher::publishCmdVel()
{
if(!move_base_ptr_)
{
return;
}
if(!move_base_ptr_->getFeedback()->is_ready)
{
return;
}
robot_nav_2d_msgs::Twist2DStamped twist = move_base_ptr_->getTwist();
if(twist.header.stamp < robot::Time::now() - robot::Duration(0.05))
{
return;
}
geometry_msgs::Twist cmd_vel;
cmd_vel.linear.x = twist.velocity.x;
cmd_vel.linear.y = twist.velocity.y;
cmd_vel.angular.z = twist.velocity.theta;
cmd_vel_pub_.publish(cmd_vel);
}
void AmrPublisher::startCmdVelPublishing(double rate)
{
stopCmdVelPublishing(); // Stop any existing publishing first
if (rate <= 0.0)
{
robot::log_warning("[AmrPublisher] Invalid cmd_vel publishing rate: %.2f Hz. Must be > 0", rate);
return;
}
ros::WallDuration period(1.0 / rate);
cmd_vel_timer_ = nh_.createWallTimer(
period,
&AmrPublisher::cmdVelTimerCallback,
this,
false // oneshot = false
);
cmd_vel_publishing_active_ = true;
robot::log_info("[AmrPublisher] Started publishing cmd_vel at %.2f Hz using ros::WallTimer", rate);
}
void AmrPublisher::stopCmdVelPublishing()
{
if (cmd_vel_publishing_active_)
{
cmd_vel_timer_.stop();
cmd_vel_publishing_active_ = false;
robot::log_info("[AmrPublisher] Stopped publishing cmd_vel");
}
}
void AmrPublisher::cmdVelTimerCallback(const ros::WallTimerEvent &event)
{
(void)event;
if (cmd_vel_publishing_active_)
{
publishCmdVel();
}
}
} // namespace amr_control

28
Controllers/Packages/amr_control/src/amr_subcriber.cpp Normal file
View File

@@ -0,0 +1,28 @@
#include "amr_control/amr_subcriber.h"
#include <geometry_msgs/PoseStamped.h>
amr_control::AmrSubscriber::AmrSubscriber(ros::NodeHandle &nh, robot::move_base_core::BaseNavigation::Ptr move_base_ptr)
: nh_(nh), move_base_ptr_(move_base_ptr)
{
robot::log_info("[%s:%d] Initializing AmrSubscriber namespace: %s", __FILE__, __LINE__, nh_.getNamespace().c_str());
ros::NodeHandle nh_move_base_simple;
move_base_simple_sub_ = nh_move_base_simple.subscribe("/move_base_simple/goal", 1, &AmrSubscriber::moveBaseSimpleCallback, this);
}
amr_control::AmrSubscriber::~AmrSubscriber()
{
}
void amr_control::AmrSubscriber::moveBaseSimpleCallback(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
robot_geometry_msgs::PoseStamped goal;
goal.header.stamp = robot::Time::now();
goal.header.frame_id = msg->header.frame_id;
goal.pose.position.x = msg->pose.position.x;
goal.pose.position.y = msg->pose.position.y;
goal.pose.orientation.x = msg->pose.orientation.x;
goal.pose.orientation.y = msg->pose.orientation.y;
goal.pose.orientation.z = msg->pose.orientation.z;
goal.pose.orientation.w = msg->pose.orientation.w;
move_base_ptr_->moveTo(goal, 0.02, 0.02);
}

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

@@ -3,6 +3,9 @@
#include <functional>
#include <optional>
#include "vda5050_msgs/Order.h"
#include "robot_nav_msgs/Path.h"
#include "robot_geometry_msgs/Pose2D.h"
#include "robot_nav_2d_utils/conversions.h"
namespace amr_control
{
@@ -141,17 +144,17 @@ namespace amr_control
if (!paths.empty())
return;
std::vector<geometry_msgs::Pose2D> poses;
std::vector<robot_geometry_msgs::Pose2D> poses;
for (auto &p : paths)
{
geometry_msgs::Pose2D p_2d;
robot_geometry_msgs::Pose2D p_2d;
p_2d.x = p.getX();
p_2d.y = p.getY();
p_2d.theta = p.getYaw();
poses.push_back(p_2d);
}
// const nav_msgs::Path path = nav_2d_utils::poses2DToPath(poses, "map", ros::Time::now());
// goal = path.poses.back();
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);
}
else if (global_plan_msg_type_ == std::string("vda5050_msgs::Order"))
@@ -226,14 +229,20 @@ namespace amr_control
goal.header.frame_id = "map";
goal.pose.position.x = position.x;
goal.pose.position.y = position.y;
// tf3::Transform transform;
// transform.setRotation(tf3::Quaternion(0.0, 0.0, position.theta));
// goal = tf3::toMsg(transform);
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();
}
else
return;
ros::Duration(0.1).sleep();
VDA5050ClientAPI::move_base_ptr_->moveTo(goal);
cmd_vel_max_.x = 0.2;
cmd_vel_max_.theta = 0.5;