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

update c_api

Browse Source
This commit is contained in:
HiepLM
2026-02-10 14:39:43 +07:00
parent 3f3425228c
commit 99f014e14c
142 changed files with 5108 additions and 2323 deletions
Download Patch File Download Diff File Expand all files Collapse all files

69
src/Navigations/Packages/move_base/src/move_base.cpp
View File

@@ -416,6 +416,24 @@ void move_base::MoveBase::addStaticMap(const std::string &map_name, robot_nav_ms
{
it->second = map;
}
robot::log_info("map header: %s", map.header.frame_id.c_str());
robot::log_info("map header: %ld.%ld", map.header.stamp.sec, map.header.stamp.nsec);
robot::log_info("map header: %d", map.header.seq);
robot::log_info("map info resolution: %f", map.info.resolution);
robot::log_info("map info width: %d", map.info.width);
robot::log_info("map info height: %d", map.info.height);
robot::log_info("map info origin position x: %f", map.info.origin.position.x);
robot::log_info("map info origin position y: %f", map.info.origin.position.y);
robot::log_info("map info origin position z: %f", map.info.origin.position.z);
robot::log_info("map info origin orientation x: %f", map.info.origin.orientation.x);
robot::log_info("map info origin orientation y: %f", map.info.origin.orientation.y);
robot::log_info("map info origin orientation z: %f", map.info.origin.orientation.z);
robot::log_info("map info origin orientation w: %f", map.info.origin.orientation.w);
robot::log_info("map data size: %zu", map.data.size());
for(size_t i = 0; i < map.data.size(); i++) {
robot::log_info("map data[%zu]: %d", i, map.data[i]);
}
robot::log_info("--------------------------------");
updateGlobalCostmap<robot_nav_msgs::OccupancyGrid>(map, robot_costmap_2d::LayerType::STATIC_LAYER, map_name);
updateLocalCostmap<robot_nav_msgs::OccupancyGrid>(map, robot_costmap_2d::LayerType::STATIC_LAYER, map_name);
}
@@ -432,7 +450,6 @@ robot_nav_msgs::OccupancyGrid move_base::MoveBase::getStaticMap(const std::strin
void move_base::MoveBase::addLaserScan(const std::string &laser_scan_name, robot_sensor_msgs::LaserScan laser_scan)
{
auto it = laser_scans_.find(laser_scan_name);
if (it == laser_scans_.end())
{
@@ -442,6 +459,29 @@ void move_base::MoveBase::addLaserScan(const std::string &laser_scan_name, robot
{
it->second = laser_scan;
}
robot::log_info("stamp: %ld.%ld", laser_scan.header.stamp.sec, laser_scan.header.stamp.nsec);
robot::log_info("frame_id: %s", laser_scan.header.frame_id.c_str());
robot::log_info("angle_min: %f", laser_scan.angle_min);
robot::log_info("angle_max: %f", laser_scan.angle_max);
robot::log_info("angle_increment: %f", laser_scan.angle_increment);
robot::log_info("time_increment: %f", laser_scan.time_increment);
robot::log_info("scan_time: %f", laser_scan.scan_time);
robot::log_info("range_min: %f", laser_scan.range_min);
robot::log_info("range_max: %f", laser_scan.range_max);
robot::log_info("ranges_size: %zu", laser_scan.ranges.size());
robot::log_info("intensities_size: %zu", laser_scan.intensities.size());
std::stringstream ranges_str;
for (size_t i = 0; i < laser_scan.ranges.size(); i++) {
ranges_str << laser_scan.ranges[i] << " ";
}
robot::log_info("ranges: %s", ranges_str.str().c_str());
std::stringstream intensities_str;
for (size_t i = 0; i < laser_scan.intensities.size(); i++) {
intensities_str << laser_scan.intensities[i] << " ";
}
robot::log_info("intensities: %s", intensities_str.str().c_str());
robot::log_info("--------------------------------");
updateLocalCostmap<robot_sensor_msgs::LaserScan>(laser_scan, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name);
updateGlobalCostmap<robot_sensor_msgs::LaserScan>(laser_scan, robot_costmap_2d::LayerType::VOXEL_LAYER, laser_scan_name);
}
@@ -659,6 +699,33 @@ void move_base::MoveBase::updateLocalCostmap(const T& value, robot_costmap_2d::L
void move_base::MoveBase::addOdometry(const std::string &odometry_name, robot_nav_msgs::Odometry odometry)
{
robot::log_info("addOdometry: %s", odometry_name.c_str());
robot::log_info("odometry header: %s", odometry.header.frame_id.c_str());
robot::log_info("odometry child_frame_id: %s", odometry.child_frame_id.c_str());
robot::log_info("odometry pose position x: %f", odometry.pose.pose.position.x);
robot::log_info("odometry pose position y: %f", odometry.pose.pose.position.y);
robot::log_info("odometry pose position z: %f", odometry.pose.pose.position.z);
robot::log_info("odometry pose orientation x: %f", odometry.pose.pose.orientation.x);
robot::log_info("odometry pose orientation y: %f", odometry.pose.pose.orientation.y);
robot::log_info("odometry pose orientation z: %f", odometry.pose.pose.orientation.z);
robot::log_info("odometry pose orientation w: %f", odometry.pose.pose.orientation.w);
std::stringstream pose_covariance_str;
for(int i = 0; i < 36; i++) {
pose_covariance_str << odometry.pose.covariance[i] << " ";
}
robot::log_info("odometry pose covariance: %s", pose_covariance_str.str().c_str());
robot::log_info("odometry twist linear x: %f", odometry.twist.twist.linear.x);
robot::log_info("odometry twist linear y: %f", odometry.twist.twist.linear.y);
robot::log_info("odometry twist linear z: %f", odometry.twist.twist.linear.z);
robot::log_info("odometry twist angular x: %f", odometry.twist.twist.angular.x);
robot::log_info("odometry twist angular y: %f", odometry.twist.twist.angular.y);
robot::log_info("odometry twist angular z: %f", odometry.twist.twist.angular.z);
std::stringstream twist_covariance_str;
for(int i = 0; i < 36; i++) {
twist_covariance_str << odometry.twist.covariance[i] << " ";
}
robot::log_info("odometry twist covariance: %s", twist_covariance_str.str().c_str());
odometry_ = odometry;
}