From 78b11b60feef7ba7e8aa304eb17b8ebf360456d1 Mon Sep 17 00:00:00 2001 From: HiepLM Date: Mon, 12 Jan 2026 15:49:52 +0700 Subject: [PATCH] upodate --- .../Packages/amr_control/CMakeLists.txt | 2 +- .../include/amr_control/amr_control.h | 2 + .../include/amr_control/amr_publiser.h | 155 ++++++++ .../Packages/amr_control/src/amr_control.cpp | 81 ++++- .../Packages/amr_control/src/amr_publiser.cpp | 335 ++++++++++++++++++ .../amr_control/src/sensor_converter.cpp | 6 +- .../Packages/amr_control/src/tf_converter.cpp | 9 +- ...lobal_params_plugins_no_virtual_walls.yaml | 1 + ...local_params_plugins_no_virtual_walls.yaml | 7 +- .../amr_startup/config/mqtt_general.yaml | 4 +- .../Packages/amr_startup/rviz/navigation.rviz | 19 +- 11 files changed, 594 insertions(+), 27 deletions(-) create mode 100644 Controllers/Packages/amr_control/include/amr_control/amr_publiser.h create mode 100644 Controllers/Packages/amr_control/src/amr_publiser.cpp diff --git a/Controllers/Packages/amr_control/CMakeLists.txt b/Controllers/Packages/amr_control/CMakeLists.txt index 102eeca..a97af0b 100644 --- a/Controllers/Packages/amr_control/CMakeLists.txt +++ b/Controllers/Packages/amr_control/CMakeLists.txt @@ -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) +add_library(${PROJECT_NAME}_converter src/tf_converter.cpp src/sensor_converter.cpp src/amr_publiser.cpp) add_dependencies(${PROJECT_NAME}_converter ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) target_link_libraries(${PROJECT_NAME}_converter ${catkin_LIBRARIES} ${Boost_LIBRARIES}) diff --git a/Controllers/Packages/amr_control/include/amr_control/amr_control.h b/Controllers/Packages/amr_control/include/amr_control/amr_control.h index 5a11b9f..3f00811 100644 --- a/Controllers/Packages/amr_control/include/amr_control/amr_control.h +++ b/Controllers/Packages/amr_control/include/amr_control/amr_control.h @@ -28,6 +28,7 @@ #include #include #include +#include #include @@ -146,6 +147,7 @@ namespace amr_control robot::TFListenerPtr tf_core_ptr_; std::shared_ptr tf_converter_ptr_; std::shared_ptr sensor_converter_ptr_; + std::shared_ptr costmap_publisher_ptr_; ros::Subscriber is_detected_maker_sub_; std::shared_ptr opc_ua_server_api_ptr_; diff --git a/Controllers/Packages/amr_control/include/amr_control/amr_publiser.h b/Controllers/Packages/amr_control/include/amr_control/amr_publiser.h new file mode 100644 index 0000000..3d47c9e --- /dev/null +++ b/Controllers/Packages/amr_control/include/amr_control/amr_publiser.h @@ -0,0 +1,155 @@ +#ifndef __AMR_PUBLISHER_H_INCLUDED_ +#define __AMR_PUBLISHER_H_INCLUDED_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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_ + diff --git a/Controllers/Packages/amr_control/src/amr_control.cpp b/Controllers/Packages/amr_control/src/amr_control.cpp index ae488f1..4f5a5b8 100644 --- a/Controllers/Packages/amr_control/src/amr_control.cpp +++ b/Controllers/Packages/amr_control/src/amr_control.cpp @@ -5,6 +5,8 @@ #include #include #include +#include +#include namespace amr_control { @@ -76,6 +78,13 @@ 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 buffer) @@ -241,10 +250,10 @@ namespace amr_control move_base_ptr_ = move_base_loader_(); sensor_converter_ptr_ = std::make_shared(nh, move_base_ptr_); move_base_ptr_->initialize(tf_core_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); + + // Initialize costmap publisher for RViz visualization + robot::log_info("[%s:%d]\n Initializing costmap publisher...", __FILE__, __LINE__); + costmap_publisher_ptr_ = std::make_shared(nh, move_base_ptr_); ros::Rate r(3); do @@ -257,11 +266,75 @@ namespace amr_control move_base_ptr_->getFeedback() != nullptr && move_base_ptr_->getFeedback()->is_ready) { + // Read footprint from parameter server and set it + std::vector 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(footprint_xmlrpc[i][0]); + pt.y = static_cast(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) diff --git a/Controllers/Packages/amr_control/src/amr_publiser.cpp b/Controllers/Packages/amr_control/src/amr_publiser.cpp new file mode 100644 index 0000000..b5ae73c --- /dev/null +++ b/Controllers/Packages/amr_control/src/amr_publiser.cpp @@ -0,0 +1,335 @@ +#include +#include +#include +#include +#include + +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( + global_costmap_topic, 1, + boost::bind(&AmrPublisher::onNewSubscriptionGlobal, this, _1)); + global_costmap_obj_.update_pub_ = nh_.advertise(global_costmap_topic + "_updates", 1); + global_costmap_obj_.footprint_pub_ = nh_.advertise(global_costmap_topic + "/footprint", 1); + + local_costmap_obj_.pub_ = nh_.advertise( + local_costmap_topic, 1, + boost::bind(&AmrPublisher::onNewSubscriptionLocal, this, _1)); + local_costmap_obj_.update_pub_ = nh_.advertise(local_costmap_topic + "_updates", 1); + local_costmap_obj_.footprint_pub_ = nh_.advertise(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 + diff --git a/Controllers/Packages/amr_control/src/sensor_converter.cpp b/Controllers/Packages/amr_control/src/sensor_converter.cpp index 483500a..5b7a436 100644 --- a/Controllers/Packages/amr_control/src/sensor_converter.cpp +++ b/Controllers/Packages/amr_control/src/sensor_converter.cpp @@ -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("navigation_map", map); + move_base_ptr_->addStaticMap("/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); } } \ No newline at end of file diff --git a/Controllers/Packages/amr_control/src/tf_converter.cpp b/Controllers/Packages/amr_control/src/tf_converter.cpp index 95da0a3..1dcdbc9 100644 --- a/Controllers/Packages/amr_control/src/tf_converter.cpp +++ b/Controllers/Packages/amr_control/src/tf_converter.cpp @@ -5,7 +5,7 @@ amr_control::TfConverter::TfConverter(std::shared_ptr tf) { tf3_buffer_ = std::make_shared(); tf3_buffer_->setUsingDedicatedThread(true); - tf_thread_ = std::thread([this]() { this->tfWorker(); }); + tf_thread_ = std::thread(&amr_control::TfConverter::tfWorker, this); } 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 edges; tf2_ros::Buffer tfBuffer; @@ -104,7 +104,8 @@ 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()); } } diff --git a/Controllers/Packages/amr_startup/config/costmap_global_params_plugins_no_virtual_walls.yaml b/Controllers/Packages/amr_startup/config/costmap_global_params_plugins_no_virtual_walls.yaml index 06d0037..e7e95a0 100755 --- a/Controllers/Packages/amr_startup/config/costmap_global_params_plugins_no_virtual_walls.yaml +++ b/Controllers/Packages/amr_startup/config/costmap_global_params_plugins_no_virtual_walls.yaml @@ -1,4 +1,5 @@ global_costmap: + library_path: libplugins frame_id: map plugins: - {name: navigation_map, type: "costmap_2d::StaticLayer" } diff --git a/Controllers/Packages/amr_startup/config/costmap_local_params_plugins_no_virtual_walls.yaml b/Controllers/Packages/amr_startup/config/costmap_local_params_plugins_no_virtual_walls.yaml index 6ef480b..8f33990 100755 --- a/Controllers/Packages/amr_startup/config/costmap_local_params_plugins_no_virtual_walls.yaml +++ b/Controllers/Packages/amr_startup/config/costmap_local_params_plugins_no_virtual_walls.yaml @@ -1,8 +1,9 @@ local_costmap: - frame_id: odom + library_path: libplugins + global_frame: odom plugins: - - {name: obstacles, type: "costmap_2d::VoxelLayer" } - - {name: inflation, type: "costmap_2d::InflationLayer" } + - {name: obstacles, type: "VoxelLayer" } + - {name: inflation, type: "InflationLayer" } obstacles: enabled: true footprint_clearing_enabled: true diff --git a/Controllers/Packages/amr_startup/config/mqtt_general.yaml b/Controllers/Packages/amr_startup/config/mqtt_general.yaml index 2f172c9..1230673 100644 --- a/Controllers/Packages/amr_startup/config/mqtt_general.yaml +++ b/Controllers/Packages/amr_startup/config/mqtt_general.yaml @@ -1,8 +1,8 @@ MQTT: - Name: T801 + Name: T800 Host: 172.20.235.170 Port: 1885 - Client_ID: T801 + Client_ID: T800 Username: robotics Password: robotics Keep_Alive: 60 \ No newline at end of file diff --git a/Controllers/Packages/amr_startup/rviz/navigation.rviz b/Controllers/Packages/amr_startup/rviz/navigation.rviz index 1f16975..a1f3413 100644 --- a/Controllers/Packages/amr_startup/rviz/navigation.rviz +++ b/Controllers/Packages/amr_startup/rviz/navigation.rviz @@ -7,11 +7,10 @@ Panels: - /Global Options1 - /Grid1/Offset1 - /TF1/Frames1 - - /Global Map1/Straight Path1 - - /Local Map1 + - /Global Map1 + - /Local Map1/Costmap1 - /Local Map1/Trajectory1 - /Local Map1/Trajectory1/transform plan1 - - /Pose1 Splitter Ratio: 0.37295082211494446 Tree Height: 422 - Class: rviz/Selection @@ -260,7 +259,7 @@ Visualization Manager: Draw Behind: true Enabled: true Name: Costmap - Topic: /amr_node/global_costmap/costmap + Topic: /global_costmap/costmap Unreliable: false Use Timestamp: false Value: true @@ -294,7 +293,7 @@ Visualization Manager: Enabled: true Name: Footprint Queue Size: 10 - Topic: move_base_node/global_costmap/footprint + Topic: /global_costmap/costmap/footprint Unreliable: false Value: true - Alpha: 1 @@ -379,7 +378,7 @@ Visualization Manager: Draw Behind: false Enabled: true Name: Costmap - Topic: /amr_node/local_costmap/costmap + Topic: /local_costmap/costmap Unreliable: false Use Timestamp: false Value: true @@ -607,7 +606,7 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Scale: -359.3964538574219 + Scale: -168.75274658203125 Target Frame: base_link X: -0.49439820647239685 Y: 0.196189746260643 @@ -632,7 +631,7 @@ Window Geometry: Height: 573 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd000000040000000000000156000001e3fc0200000008fb000000100044006900730070006c006100790073000000003d000001e3000000c900fffffffb0000001200530065006c0065006300740069006f006e000000003d000002590000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001b500000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000000a0049006d006100670065000000003d0000039e0000000000000000fb0000000c00430061006d006500720061020000007c0000013600000280000001e0fb0000000c00430061006d0065007200610200000644000000b7000002c1000002f9000000010000010f00000259fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000259000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000041800000168fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007520000003efc0100000002fb0000000800540069006d0065000000000000000752000003bc00fffffffb0000000800540069006d0065010000000000000450000000000000000000000418000001e300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000024f000001e3fc0200000008fb000000100044006900730070006c006100790073010000003d000001e3000000c900fffffffb0000001200530065006c0065006300740069006f006e000000003d000002590000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001b500000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000000a0049006d006100670065000000003d0000039e0000000000000000fb0000000c00430061006d006500720061020000007c0000013600000280000001e0fb0000000c00430061006d0065007200610200000644000000b7000002c1000002f9000000010000010f00000259fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000259000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000041800000168fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007520000003efc0100000002fb0000000800540069006d00650000000000000007520000041800fffffffb0000000800540069006d00650100000000000004500000000000000000000001e0000001e300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -641,6 +640,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1048 + Width: 1077 X: 0 - Y: 21 + Y: 0