HiepLM/AMR_T800
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Compare commits

base: HiepLM:78b11b60feef7ba7e8aa304eb17b8ebf360456d1
Branches Tags
HiepLM:v2
..
compare: HiepLM:7cb2a3085d094a7a1ff4a2e45936f21c4d2b6920
Branches Tags
HiepLM:v2

No commits in common. "78b11b60feef7ba7e8aa304eb17b8ebf360456d1" and "7cb2a3085d094a7a1ff4a2e45936f21c4d2b6920" have entirely different histories.
78b11b60fe ... 7cb2a3085d

12 changed files with 28 additions and 595 deletions
Show Stats Expand all files Collapse all files

2
Controllers/Packages/amr_control/CMakeLists.txt
View File

@ -75,7 +75,7 @@ add_library(${PROJECT_NAME}
src/amr_make_plan_with_order.cpp
)
add_library(${PROJECT_NAME}_converter src/tf_converter.cpp src/sensor_converter.cpp src/amr_publiser.cpp)
add_library(${PROJECT_NAME}_converter src/tf_converter.cpp src/sensor_converter.cpp)
add_dependencies(${PROJECT_NAME}_converter ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(${PROJECT_NAME}_converter ${catkin_LIBRARIES} ${Boost_LIBRARIES})

2
Controllers/Packages/amr_control/include/amr_control/amr_control.h
View File

@ -28,7 +28,6 @@
#include <amr_control/amr_safety.h>
#include <amr_control/tf_converter.h>
#include <amr_control/sensor_converter.h>
#include <amr_control/amr_publiser.h>
#include <boost/dll/import.hpp>
@ -147,7 +146,6 @@ namespace amr_control
robot::TFListenerPtr tf_core_ptr_;
std::shared_ptr<amr_control::TfConverter> tf_converter_ptr_;
std::shared_ptr<amr_control::SensorConverter> sensor_converter_ptr_;
std::shared_ptr<amr_control::AmrPublisher> costmap_publisher_ptr_;
ros::Subscriber is_detected_maker_sub_;
std::shared_ptr<amr_control::OpcUAServerAPI> opc_ua_server_api_ptr_;

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

@ -1,155 +0,0 @@
#ifndef __AMR_PUBLISHER_H_INCLUDED_
#define __AMR_PUBLISHER_H_INCLUDED_
#include <ros/ros.h>
#include <nav_msgs/OccupancyGrid.h>
#include <geometry_msgs/PolygonStamped.h>
#include <robot_nav_msgs/OccupancyGrid.h>
#include <robot_map_msgs/OccupancyGridUpdate.h>
#include <map_msgs/OccupancyGridUpdate.h>
#include <move_base_core/navigation.h>
#include <robot/time.h>
#include <memory>
#include <string>
namespace amr_control
{
struct CostmapObject
{
// ROS publishers
ros::Publisher pub_;
ros::Publisher update_pub_;
ros::Publisher footprint_pub_;
ros::Timer timer_;
std::string topic_;
bool active_;
};
/**
* @class AmrPublisher
* @brief Publisher class for publishing costmap data from move_base_core to RViz
*
* This class converts robot_nav_msgs::OccupancyGrid to nav_msgs::OccupancyGrid
* and publishes global and local costmaps to ROS topics that RViz can visualize.
*/
class AmrPublisher
{
public:
/**
* @brief Constructor
* @param nh ROS NodeHandle for creating publishers
* @param move_base_ptr Shared pointer to BaseNavigation instance
*/
AmrPublisher(ros::NodeHandle &nh, robot::move_base_core::BaseNavigation::Ptr move_base_ptr);
/**
* @brief Destructor
*/
virtual ~AmrPublisher();
/**
* @brief Publish global costmap to RViz
* @param publish_rate Rate at which to publish (Hz). If 0, publish immediately once.
*/
void publishGlobalCostmap(double publish_rate = 0.0);
/**
* @brief Publish local costmap to RViz
* @param publish_rate Rate at which to publish (Hz). If 0, publish immediately once.
*/
void publishLocalCostmap(double publish_rate = 0.0);
/**
* @brief Publish both global and local costmaps
* @param publish_rate Rate at which to publish (Hz). If 0, publish immediately once.
*/
void publishCostmaps(double publish_rate = 0.0);
/**
* @brief Publish global footprint to RViz
*/
void publishGlobalFootprint();
/**
* @brief Publish local footprint to RViz
*/
void publishLocalFootprint();
/**
* @brief Publish both global and local footprints
*/
void publishFootprints();
/**
* @brief Start publishing costmaps at a fixed rate
* @param global_rate Publishing rate for global costmap (Hz). If 0, don't publish global.
* @param local_rate Publishing rate for local costmap (Hz). If 0, don't publish local.
*/
void startPublishing(double global_rate = 1.0, double local_rate = 5.0);
/**
* @brief Stop publishing costmaps
*/
void stopPublishing();
/**
* @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_; }
private:
/**
* @brief Callback for new subscription to global costmap
* @param pub SingleSubscriberPublisher for the new subscription
*/
void onNewSubscriptionGlobal(const ros::SingleSubscriberPublisher& pub);
/**
* @brief Callback for new subscription to local costmap
* @param pub SingleSubscriberPublisher for the new subscription
*/
void onNewSubscriptionLocal(const ros::SingleSubscriberPublisher& pub);
/**
* @brief Convert robot_nav_msgs::OccupancyGrid to nav_msgs::OccupancyGrid
* @param robot_grid Input robot_nav_msgs::OccupancyGrid
* @param nav_grid Output nav_msgs::OccupancyGrid
*/
void convertToNavOccupancyGrid(const robot_nav_msgs::OccupancyGrid &robot_grid,
nav_msgs::OccupancyGrid &nav_grid);
void convertToRobotOccupancyGridUpdate(const robot_map_msgs::OccupancyGridUpdate &robot_grid_update,
map_msgs::OccupancyGridUpdate &grid_update);
/**
* @brief Convert robot_geometry_msgs::PolygonStamped to geometry_msgs::PolygonStamped
* @param robot_footprint Input robot_geometry_msgs::PolygonStamped
* @param nav_footprint Output geometry_msgs::PolygonStamped
*/
void convertToNavPolygonStamped(const robot_geometry_msgs::PolygonStamped &robot_footprint,
geometry_msgs::PolygonStamped &nav_footprint);
/**
* @brief Timer callback for publishing global costmap
* @param event Timer event
*/
void globalCostmapTimerCallback(const ros::TimerEvent &event);
/**
* @brief Timer callback for publishing local costmap
* @param event Timer event
*/
void localCostmapTimerCallback(const ros::TimerEvent &event);
ros::NodeHandle nh_;
robot::move_base_core::BaseNavigation::Ptr move_base_ptr_;
CostmapObject global_costmap_obj_;
CostmapObject local_costmap_obj_;
};
} // namespace amr_control
#endif // __AMR_PUBLISHER_H_INCLUDED_

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

@ -5,8 +5,6 @@
#include <robot/node_handle.h>
#include <tf3/buffer_core.h>
#include <geometry_msgs/Vector3.h>
#include <xmlrpcpp/XmlRpcValue.h>
#include <robot_geometry_msgs/Point.h>
namespace amr_control
{
@ -78,13 +76,6 @@ namespace amr_control
{
vda_5050_client_api_ptr_.reset();
}
// Stop costmap publishing
if (costmap_publisher_ptr_)
{
costmap_publisher_ptr_->stopPublishing();
costmap_publisher_ptr_.reset();
}
}
void AmrController::init(ros::NodeHandle &nh, std::shared_ptr<tf2_ros::Buffer> buffer)
@ -250,10 +241,10 @@ namespace amr_control
move_base_ptr_ = move_base_loader_();
sensor_converter_ptr_ = std::make_shared<amr_control::SensorConverter>(nh, move_base_ptr_);
move_base_ptr_->initialize(tf_core_ptr_);
// 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_);
// Request map từ service hoặc đợi message (vì map server chỉ publish một lần)
robot::log_info("[%s:%d]\n Requesting map from server...", __FILE__, __LINE__);
sensor_converter_ptr_->requestMapFromServer(nh);
ros::Rate r(3);
do
@ -266,75 +257,11 @@ namespace amr_control
move_base_ptr_->getFeedback() != nullptr &&
move_base_ptr_->getFeedback()->is_ready)
{
// Read footprint from parameter server and set it
std::vector<robot_geometry_msgs::Point> footprint;
if (nh.hasParam("footprint"))
{
XmlRpc::XmlRpcValue footprint_xmlrpc;
nh.getParam("footprint", footprint_xmlrpc);
if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeArray && footprint_xmlrpc.size() >= 3)
{
footprint.reserve(footprint_xmlrpc.size());
for (int i = 0; i < footprint_xmlrpc.size(); ++i)
{
if (footprint_xmlrpc[i].getType() == XmlRpc::XmlRpcValue::TypeArray && footprint_xmlrpc[i].size() == 2)
{
robot_geometry_msgs::Point pt;
pt.x = static_cast<double>(footprint_xmlrpc[i][0]);
pt.y = static_cast<double>(footprint_xmlrpc[i][1]);
pt.z = 0.0;
footprint.push_back(pt);
}
}
if (footprint.size() >= 3)
{
move_base_ptr_->setRobotFootprint(footprint);
robot::log_info("[%s:%d] Set robot footprint with %zu points", __FILE__, __LINE__, footprint.size());
for (size_t i = 0; i < footprint.size(); ++i)
{
robot::log_info("[%s:%d] Footprint point[%zu]: (%.3f, %.3f)",
__FILE__, __LINE__, i, footprint[i].x, footprint[i].y);
}
}
else
{
robot::log_warning("[%s:%d] Footprint must have at least 3 points, got %zu. Using default footprint.",
__FILE__, __LINE__, footprint.size());
}
}
else
{
robot::log_warning("[%s:%d] Invalid footprint parameter format. Expected array of arrays with at least 3 points.",
__FILE__, __LINE__);
}
}
else
{
robot::log_info("[%s:%d] No footprint parameter found, using default footprint from move_base config.",
__FILE__, __LINE__);
}
robot_geometry_msgs::Vector3 linear;
linear.x = 0.3;
move_base_ptr_->setTwistLinear(linear);
linear.x = -0.3;
move_base_ptr_->setTwistLinear(linear);
// Request map từ service hoặc đợi message (vì map server chỉ publish một lần)
robot::log_info("[%s:%d]\n Requesting map from server...", __FILE__, __LINE__);
sensor_converter_ptr_->requestMapFromServer(nh);
// Start publishing costmaps to RViz
// Global costmap: 1 Hz, Local costmap: 5 Hz
if (costmap_publisher_ptr_)
{
robot::log_info("[%s:%d]\n Starting costmap publishing to RViz...", __FILE__, __LINE__);
costmap_publisher_ptr_->startPublishing(1.0, 5.0);
}
}
}
catch (const std::exception &e)

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

@ -1,335 +0,0 @@
#include <amr_control/amr_publiser.h>
#include <robot/console.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <robot_costmap_2d/cost_values.h>
#include <boost/bind.hpp>
namespace amr_control
{
AmrPublisher::AmrPublisher(ros::NodeHandle &nh, robot::move_base_core::BaseNavigation::Ptr move_base_ptr)
: nh_(nh),
move_base_ptr_(move_base_ptr)
{
// Initialize default topic names
std::string global_costmap_topic = "/global_costmap/costmap";
std::string local_costmap_topic = "/local_costmap/costmap";
// 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);
// Initialize publishers with callback for new subscriptions
global_costmap_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);
local_costmap_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);
// Initialize topic names in CostmapObject
global_costmap_obj_.topic_ = global_costmap_topic;
local_costmap_obj_.topic_ = local_costmap_topic;
// Initialize other CostmapObject members
global_costmap_obj_.active_ = false;
local_costmap_obj_.active_ = false;
robot::log_info("[AmrPublisher] Initialized. Global costmap topic: %s, Local costmap topic: %s",
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();
}
void AmrPublisher::convertToNavOccupancyGrid(const robot_nav_msgs::OccupancyGrid &robot_grid,
nav_msgs::OccupancyGrid &nav_grid)
{
// Convert header
nav_grid.header.seq = robot_grid.header.seq;
nav_grid.header.frame_id = robot_grid.header.frame_id;
nav_grid.header.stamp = ros::Time::now();
// Convert map metadata
nav_grid.info.map_load_time = ros::Time::now();
nav_grid.info.resolution = robot_grid.info.resolution;
nav_grid.info.width = robot_grid.info.width;
nav_grid.info.height = robot_grid.info.height;
// Convert origin
nav_grid.info.origin.position.x = robot_grid.info.origin.position.x;
nav_grid.info.origin.position.y = robot_grid.info.origin.position.y;
nav_grid.info.origin.position.z = robot_grid.info.origin.position.z;
nav_grid.info.origin.orientation.x = robot_grid.info.origin.orientation.x;
nav_grid.info.origin.orientation.y = robot_grid.info.origin.orientation.y;
nav_grid.info.origin.orientation.z = robot_grid.info.origin.orientation.z;
nav_grid.info.origin.orientation.w = robot_grid.info.origin.orientation.w;
// Copy occupancy data (both use int8_t, so direct copy is safe)
nav_grid.data = robot_grid.data;
}
void AmrPublisher::convertToRobotOccupancyGridUpdate(const robot_map_msgs::OccupancyGridUpdate &robot_grid_update,
map_msgs::OccupancyGridUpdate &grid_update)
{
// Convert header
grid_update.header.stamp = ros::Time::now();
grid_update.header.frame_id = robot_grid_update.header.frame_id;
grid_update.x = robot_grid_update.x;
grid_update.y = robot_grid_update.y;
grid_update.width = robot_grid_update.width;
grid_update.height = robot_grid_update.height;
grid_update.data = robot_grid_update.data;
}
void AmrPublisher::convertToNavPolygonStamped(const robot_geometry_msgs::PolygonStamped &robot_footprint,
geometry_msgs::PolygonStamped &nav_footprint)
{
// Convert header
nav_footprint.header.seq = robot_footprint.header.seq;
nav_footprint.header.frame_id = robot_footprint.header.frame_id;
nav_footprint.header.stamp = ros::Time::now();
// Convert polygon points
nav_footprint.polygon.points.clear();
nav_footprint.polygon.points.reserve(robot_footprint.polygon.points.size());
for (const auto &robot_point : robot_footprint.polygon.points)
{
geometry_msgs::Point32 nav_point;
nav_point.x = robot_point.x;
nav_point.y = robot_point.y;
nav_point.z = robot_point.z;
nav_footprint.polygon.points.push_back(nav_point);
}
}
void AmrPublisher::publishGlobalCostmap(double publish_rate)
{
if (!move_base_ptr_)
{
robot::log_warning("[AmrPublisher] move_base_ptr_ is null, cannot publish global costmap");
return;
}
try
{
// Get global costmap data from move_base
robot::move_base_core::PlannerDataOutput global_data = move_base_ptr_->getGlobalData();
if(!global_data.is_costmap_updated)
{
nav_msgs::OccupancyGrid nav_grid;
convertToNavOccupancyGrid(global_data.costmap, nav_grid);
global_costmap_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);
}
// Publish footprint
publishGlobalFootprint();
// robot::log_debug("[AmrPublisher] Published global costmap: %dx%d, frame_id='%s'",
// nav_grid.info.width, nav_grid.info.height, nav_grid.header.frame_id.c_str());
}
catch (const std::exception &e)
{
robot::log_error("[AmrPublisher] Error publishing global costmap: %s", e.what());
}
}
void AmrPublisher::publishLocalCostmap(double publish_rate)
{
if (!move_base_ptr_)
{
robot::log_warning("[AmrPublisher] move_base_ptr_ is null, cannot publish local costmap");
return;
}
try
{
// Get local costmap data from move_base
robot::move_base_core::PlannerDataOutput local_data = move_base_ptr_->getLocalData();
if(!local_data.is_costmap_updated)
{
nav_msgs::OccupancyGrid nav_grid;
convertToNavOccupancyGrid(local_data.costmap, nav_grid);
local_costmap_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);
}
// Publish footprint
publishLocalFootprint();
// robot::log_debug("[AmrPublisher] Published local costmap: %dx%d, frame_id='%s'",
// nav_grid.info.width, nav_grid.info.height, nav_grid.header.frame_id.c_str());
}
catch (const std::exception &e)
{
robot::log_error("[AmrPublisher] Error publishing local costmap: %s", e.what());
}
}
void AmrPublisher::publishCostmaps(double publish_rate)
{
publishGlobalCostmap(publish_rate);
publishLocalCostmap(publish_rate);
}
void AmrPublisher::publishGlobalFootprint()
{
if (!move_base_ptr_)
{
robot::log_warning("[AmrPublisher] move_base_ptr_ is null, cannot publish global footprint");
return;
}
try
{
// Get global data from move_base
robot::move_base_core::PlannerDataOutput global_data = move_base_ptr_->getGlobalData();
// Convert and publish footprint
geometry_msgs::PolygonStamped nav_footprint;
convertToNavPolygonStamped(global_data.footprint, nav_footprint);
global_costmap_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());
}
catch (const std::exception &e)
{
robot::log_error("[AmrPublisher] Error publishing global footprint: %s", e.what());
}
}
void AmrPublisher::publishLocalFootprint()
{
if (!move_base_ptr_)
{
robot::log_warning("[AmrPublisher] move_base_ptr_ is null, cannot publish local footprint");
return;
}
try
{
// Get local data from move_base
robot::move_base_core::PlannerDataOutput local_data = move_base_ptr_->getLocalData();
// Convert and publish footprint
geometry_msgs::PolygonStamped nav_footprint;
convertToNavPolygonStamped(local_data.footprint, nav_footprint);
local_costmap_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());
}
catch (const std::exception &e)
{
robot::log_error("[AmrPublisher] Error publishing local footprint: %s", e.what());
}
}
void AmrPublisher::publishFootprints()
{
publishGlobalFootprint();
publishLocalFootprint();
}
void AmrPublisher::startPublishing(double global_rate, double local_rate)
{
stopPublishing(); // Stop any existing publishing first
// Start global costmap timer if rate > 0
if (global_rate > 0.0)
{
global_costmap_obj_.active_ = true;
global_costmap_obj_.timer_ = nh_.createTimer(
ros::Duration(1.0 / global_rate),
&AmrPublisher::globalCostmapTimerCallback,
this
);
robot::log_info("[AmrPublisher] Started publishing global costmap at %.2f Hz", global_rate);
}
// Start local costmap timer if rate > 0
if (local_rate > 0.0)
{
local_costmap_obj_.active_ = true;
local_costmap_obj_.timer_ = nh_.createTimer(
ros::Duration(1.0 / local_rate),
&AmrPublisher::localCostmapTimerCallback,
this
);
robot::log_info("[AmrPublisher] Started publishing local costmap at %.2f Hz", local_rate);
}
}
void AmrPublisher::stopPublishing()
{
// Stop timers
if (global_costmap_obj_.active_)
{
global_costmap_obj_.timer_.stop();
global_costmap_obj_.active_ = false;
}
if (local_costmap_obj_.active_)
{
local_costmap_obj_.timer_.stop();
local_costmap_obj_.active_ = false;
}
robot::log_info("[AmrPublisher] Stopped publishing costmaps");
}
void AmrPublisher::globalCostmapTimerCallback(const ros::TimerEvent &event)
{
if (global_costmap_obj_.active_)
{
publishGlobalCostmap();
}
}
void AmrPublisher::localCostmapTimerCallback(const ros::TimerEvent &event)
{
if (local_costmap_obj_.active_)
{
publishLocalCostmap();
}
}
void AmrPublisher::onNewSubscriptionLocal(const ros::SingleSubscriberPublisher& pub)
{
robot::log_info("[AmrPublisher] New subscription: %s", pub.getSubscriberName().c_str());
publishLocalCostmap();
}
void AmrPublisher::onNewSubscriptionGlobal(const ros::SingleSubscriberPublisher& pub)
{
robot::log_info("[AmrPublisher] New subscription: %s", pub.getSubscriberName().c_str());
publishGlobalCostmap();
}
} // namespace amr_control

6
Controllers/Packages/amr_control/src/sensor_converter.cpp
View File

@ -75,7 +75,7 @@ void amr_control::SensorConverter::convertAndAddMap(const nav_msgs::OccupancyGri
map.info.origin.orientation.w = nav_map.info.origin.orientation.w;
map.data = nav_map.data;
move_base_ptr_->addStaticMap("/map", map);
move_base_ptr_->addStaticMap("navigation_map", map);
}
}
@ -104,7 +104,7 @@ void amr_control::SensorConverter::laserFScanCallback(const sensor_msgs::LaserSc
robot_scan.ranges = msg->ranges;
robot_scan.intensities = msg->intensities;
move_base_ptr_->addLaserScan("/f_scan", robot_scan);
move_base_ptr_->addLaserScan("f_scan", robot_scan);
}
}
@ -127,6 +127,6 @@ void amr_control::SensorConverter::laserBScanCallback(const sensor_msgs::LaserSc
robot_scan.ranges = msg->ranges;
robot_scan.intensities = msg->intensities;
move_base_ptr_->addLaserScan("/b_scan", robot_scan);
move_base_ptr_->addLaserScan("b_scan", robot_scan);
}
}

9
Controllers/Packages/amr_control/src/tf_converter.cpp
View File

@ -5,7 +5,7 @@ amr_control::TfConverter::TfConverter(std::shared_ptr<tf2_ros::Buffer> tf)
{
tf3_buffer_ = std::make_shared<tf3::BufferCore>();
tf3_buffer_->setUsingDedicatedThread(true);
tf_thread_ = std::thread(&amr_control::TfConverter::tfWorker, this);
tf_thread_ = std::thread([this]() { this->tfWorker(); });
}
amr_control::TfConverter::~TfConverter()
@ -16,8 +16,8 @@ amr_control::TfConverter::~TfConverter()
void amr_control::TfConverter::tfWorker()
{
struct TFEdge {
std::string parent;
std::string child;
std::string parent;
std::string child;
};
std::vector<TFEdge> edges;
tf2_ros::Buffer tfBuffer;
@ -104,8 +104,7 @@ void amr_control::TfConverter::tfWorker()
catch (tf2::TransformException &ex)
{
ROS_WARN("TF %s -> %s failed: %s",
e.parent.c_str(),
e.child.c_str(),
e.parent.c_str(),e.child.c_str(),
ex.what());
}
}

1
Controllers/Packages/amr_startup/config/costmap_global_params_plugins_no_virtual_walls.yaml
View File

@ -1,5 +1,4 @@
global_costmap:
library_path: libplugins
frame_id: map
plugins:
- {name: navigation_map, type: "costmap_2d::StaticLayer" }

7
Controllers/Packages/amr_startup/config/costmap_local_params_plugins_no_virtual_walls.yaml
View File

@ -1,9 +1,8 @@
local_costmap:
library_path: libplugins
global_frame: odom
frame_id: odom
plugins:
- {name: obstacles, type: "VoxelLayer" }
- {name: inflation, type: "InflationLayer" }
- {name: obstacles, type: "costmap_2d::VoxelLayer" }
- {name: inflation, type: "costmap_2d::InflationLayer" }
obstacles:
enabled: true
footprint_clearing_enabled: true

4
Controllers/Packages/amr_startup/config/mqtt_general.yaml
View File

@ -1,8 +1,8 @@
MQTT:
Name: T800
Name: T801
Host: 172.20.235.170
Port: 1885
Client_ID: T800
Client_ID: T801
Username: robotics
Password: robotics
Keep_Alive: 60

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

@ -7,10 +7,11 @@ Panels:
- /Global Options1
- /Grid1/Offset1
- /TF1/Frames1
- /Global Map1
- /Local Map1/Costmap1
- /Global Map1/Straight Path1
- /Local Map1
- /Local Map1/Trajectory1
- /Local Map1/Trajectory1/transform plan1
- /Pose1
Splitter Ratio: 0.37295082211494446
Tree Height: 422
- Class: rviz/Selection
@ -259,7 +260,7 @@ Visualization Manager:
Draw Behind: true
Enabled: true
Name: Costmap
Topic: /global_costmap/costmap
Topic: /amr_node/global_costmap/costmap
Unreliable: false
Use Timestamp: false
Value: true
@ -293,7 +294,7 @@ Visualization Manager:
Enabled: true
Name: Footprint
Queue Size: 10
Topic: /global_costmap/costmap/footprint
Topic: move_base_node/global_costmap/footprint
Unreliable: false
Value: true
- Alpha: 1
@ -378,7 +379,7 @@ Visualization Manager:
Draw Behind: false
Enabled: true
Name: Costmap
Topic: /local_costmap/costmap
Topic: /amr_node/local_costmap/costmap
Unreliable: false
Use Timestamp: false
Value: true
@ -606,7 +607,7 @@ Visualization Manager:
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Scale: -168.75274658203125
Scale: -359.3964538574219
Target Frame: base_link
X: -0.49439820647239685
Y: 0.196189746260643
@ -631,7 +632,7 @@ Window Geometry:
Height: 573
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd00000004000000000000024f000001e3fc0200000008fb000000100044006900730070006c006100790073010000003d000001e3000000c900fffffffb0000001200530065006c0065006300740069006f006e000000003d000002590000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001b500000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000000a0049006d006100670065000000003d0000039e0000000000000000fb0000000c00430061006d006500720061020000007c0000013600000280000001e0fb0000000c00430061006d0065007200610200000644000000b7000002c1000002f9000000010000010f00000259fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000259000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000041800000168fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007520000003efc0100000002fb0000000800540069006d00650000000000000007520000041800fffffffb0000000800540069006d00650100000000000004500000000000000000000001e0000001e300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
QMainWindow State: 000000ff00000000fd000000040000000000000156000001e3fc0200000008fb000000100044006900730070006c006100790073000000003d000001e3000000c900fffffffb0000001200530065006c0065006300740069006f006e000000003d000002590000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001b500000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000000a0049006d006100670065000000003d0000039e0000000000000000fb0000000c00430061006d006500720061020000007c0000013600000280000001e0fb0000000c00430061006d0065007200610200000644000000b7000002c1000002f9000000010000010f00000259fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000259000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000041800000168fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007520000003efc0100000002fb0000000800540069006d0065000000000000000752000003bc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000418000001e300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
@ -640,6 +641,6 @@ Window Geometry:
collapsed: false
Views:
collapsed: false
Width: 1077
Width: 1048
X: 0
Y: 0
Y: 21

2
pnkx_nav_core

@ -1 +1 @@
Subproject commit 145fb2088ea8145d6a51927142b5df00409a0aa6
Subproject commit f5e7e1f1e06189a181a9c099652e2c95f6166e49