DuongTD/costmap_2d
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add file costmap_2d_robot

Browse Source
This commit is contained in:
duongtd
2025-11-18 11:44:01 +07:00
parent 0c61984d3e
commit 59ec58a018
18 changed files with 203 additions and 101 deletions
Download Patch File Download Diff File Expand all files Collapse all files

89
src/costmap_2d_robot.cpp
View File

@@ -35,15 +35,20 @@
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#include <costmap_2d/layered_costmap.h>
#include <costmap_2d/costmap_2d_robot.h>
#include <cstdio>
#include <string>
#include <algorithm>
#include <vector>
#include <costmap_2d/layered_costmap.h>
#include <costmap_2d/costmap_2d_robot.h>
#include <costmap_2d/utils.h>
#include <tf3/convert.h>
#include <tf3/utils.h>
// #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <exception>
// #include <tf3_geometry_msgs/tf3_geometry_msgs.h>
using namespace std;
@@ -85,7 +90,7 @@ Costmap2DROBOT::Costmap2DROBOT(const std::string& name, tf3::BufferCore& tf) :
// ros::Time last_error = ros::Time::now();
// std::string tf_error;
// // we need to make sure that the transform between the robot base frame and the global frame is available
// // // we need to make sure that the transform between the robot base frame and the global frame is available
// while (ros::ok()
// && !tf_.canTransform(global_frame_, robot_base_frame_, ros::Time(), ros::Duration(0.1), &tf_error))
// {
@@ -174,8 +179,58 @@ Costmap2DROBOT::Costmap2DROBOT(const std::string& name, tf3::BufferCore& tf) :
void Costmap2DROBOT::getParams()
{
// private_nh.param("global_frame", global_frame_, std::string("map"));
// private_nh.param("robot_base_frame", robot_base_frame_, std::string("base_link"));
YAML::Node config = YAML::LoadFile("../costmap_2d/config/config.yaml");
YAML::Node layer = config["static_layer"];
global_frame_ = loadParam(layer, "global_frame", std::string("map"));
robot_base_frame_ = loadParam(layer, "robot_base_frame", std::string("base_link"));
robot::Time last_error = robot::Time::now();
std::string tf_error;
// check if we want a rolling window version of the costmap
bool rolling_window, track_unknown_space, always_send_full_costmap;
rolling_window = loadParam(layer, "rolling_window", false);
track_unknown_space = loadParam(layer, "track_unknown_space", false);
always_send_full_costmap = loadParam(layer, "always_send_full_costmap", false);
layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);
struct PluginInfo { std::string path; std::string name; };
std::vector<PluginInfo> plugins_to_load = {
{"./costmap_2d/libstatic_layer.so", "static_layer"},
{"./costmap_2d/libinflation_layer.so", "inflation_layer"},
{"./costmap_2d/libobstacle_layer.so", "obstacle_layer"},
{"./costmap_2d/libpreferred_layer.so", "preferred_layer"}
};
// if (private_nh.hasParam("plugins"))
// {
// my_list = loadParam(layer, "plugins", my_list);
// for (int32_t i = 0; i < my_list.size(); ++i)
// {
// std::string pname = static_cast<std::string>(my_list[i]["name"]);
// std::string type = static_cast<std::string>(my_list[i]["type"]);
// printf("%s: Using plugin \"%s\" with type %s\n", name_.c_str(), pname.c_str(), type.c_str());
for (auto& info : plugins_to_load)
{
try
{
// copyParentParameters(pname, type, private_nh);
auto creator = boost::dll::import_alias<PluginLayerPtr()>(
info.path, "create_plugin", boost::dll::load_mode::append_decorations
);
PluginLayerPtr plugin = creator();
std::cout << "Plugin created: " << info.name << std::endl;
plugin->initialize(layered_costmap_, info.name, &tf_);
layered_costmap_->addPlugin(plugin);
}
catch (std::exception &ex)
{
printf("Failed to create the s, are you sure it is properly registered and that the containing library is built? Exception: %s\n", ex.what());
}
}
// }
// }
}
void Costmap2DROBOT::setUnpaddedRobotFootprintPolygon(const geometry_msgs::Polygon& footprint)
@@ -575,17 +630,17 @@ bool Costmap2DROBOT::getRobotPose(geometry_msgs::PoseStamped& global_pose) const
// get the global pose of the robot
try
{
// // use current time if possible (makes sure it's not in the future)
// if (tf_.canTransform(global_frame_, robot_base_frame_, current_time))
// {
// geometry_msgs::TransformStamped transform = tf_.lookupTransform(global_frame_, robot_base_frame_, current_time);
// tf3::doTransform(robot_pose, global_pose, transform);
// }
// // use the latest otherwise
// else
// {
// tf_.transform(robot_pose, global_pose, global_frame_);
// }
// use current time if possible (makes sure it's not in the future)
if (tf_.canTransform(global_frame_, robot_base_frame_, convertTime(current_time)))
{
tf3::TransformStampedMsg transform = tf_.lookupTransform(global_frame_, robot_base_frame_, convertTime(current_time));
// tf3::doTransform(robot_pose, global_pose, transform);
}
// use the latest otherwise
else
{
// tf_.transform(robot_pose, global_pose, global_frame_);
}
}
catch (tf3::LookupException& ex)
{

18
src/observation_buffer.cpp
View File

@@ -1,7 +1,7 @@
#include <costmap_2d/observation_buffer.h>
#include <costmap_2d/data_convert.h>
// #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
// #include <tf2_sensor_msgs/tf2_sensor_msgs.h>
// #include <tf3_geometry_msgs/tf3_geometry_msgs.h>
// #include <tf3_sensor_msgs/tf3_sensor_msgs.h>
#include<tf3/convert.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#include <cstdint>
@@ -16,9 +16,9 @@ namespace costmap_2d
{
ObservationBuffer::ObservationBuffer(string topic_name, double observation_keep_time, double expected_update_rate,
double min_obstacle_height, double max_obstacle_height, double obstacle_range,
double raytrace_range, tf3::BufferCore& tf2_buffer, string global_frame,
double raytrace_range, tf3::BufferCore& tf3_buffer, string global_frame,
string sensor_frame, double tf_tolerance) :
tf2_buffer_(tf2_buffer), observation_keep_time_(observation_keep_time), expected_update_rate_(expected_update_rate),
tf3_buffer_(tf3_buffer), observation_keep_time_(observation_keep_time), expected_update_rate_(expected_update_rate),
last_updated_(robot::Time::now()),
global_frame_(global_frame), sensor_frame_(sensor_frame), topic_name_(topic_name),
min_obstacle_height_(min_obstacle_height), max_obstacle_height_(max_obstacle_height),
@@ -36,7 +36,7 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
std::string tf_error;
if (!tf2_buffer_.canTransform(new_global_frame, global_frame_, tf3::Time::now(), &tf_error))
if (!tf3_buffer_.canTransform(new_global_frame, global_frame_, tf3::Time::now(), &tf_error))
{
printf("Transform between %s and %s with tolerance %.2f failed: %s.\n", new_global_frame.c_str(),
global_frame_.c_str(), tf_tolerance_, tf_error.c_str());
@@ -57,14 +57,14 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
// we need to transform the origin of the observation to the new global frame
tf3::doTransform(origin, origin,
tf2_buffer_.lookupTransform(new_global_frame,
tf3_buffer_.lookupTransform(new_global_frame,
origin.header.frame_id,
convertTime(origin.header.stamp)));
obs.origin_ = origin.point;
// we also need to transform the cloud of the observation to the new global frame
tf3::doTransform(*(obs.cloud_), *(obs.cloud_),
tf2_buffer_.lookupTransform(new_global_frame,
tf3_buffer_.lookupTransform(new_global_frame,
obs.cloud_->header.frame_id,
convertTime(obs.cloud_->header.stamp)));
}
@@ -101,7 +101,7 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
local_origin.point.y = 0;
local_origin.point.z = 0;
tf3::doTransform(local_origin, global_origin,
tf2_buffer_.lookupTransform(global_frame_,
tf3_buffer_.lookupTransform(global_frame_,
local_origin.header.frame_id,
convertTime(local_origin.header.stamp)));
tf3::convert(global_origin.point, observation_list_.front().origin_);
@@ -114,7 +114,7 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
// transform the point cloud
tf3::doTransform(cloud, global_frame_cloud,
tf2_buffer_.lookupTransform(global_frame_,
tf3_buffer_.lookupTransform(global_frame_,
(cloud.header.frame_id),
convertTime(cloud.header.stamp)));
global_frame_cloud.header.stamp = cloud.header.stamp;