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

update to tf3

Browse Source
This commit is contained in:
duongtd
2025-11-17 15:04:11 +07:00
parent 42840e3bbc
commit 0c61984d3e
12 changed files with 749 additions and 220 deletions
Download Patch File Download Diff File Expand all files Collapse all files

69
include/costmap_2d/costmap_2d_robot.h
View File

@@ -35,20 +35,22 @@
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#ifndef COSTMAP_2D_COSTMAP_2D_ROS_H_
#define COSTMAP_2D_COSTMAP_2D_ROS_H_
#ifndef COSTMAP_2D_COSTMAP_2D_ROBOT_H_
#define COSTMAP_2D_COSTMAP_2D_ROBOT_H_
#include <costmap_2d/layered_costmap.h>
#include <costmap_2d/layer.h>
#include <costmap_2d/costmap_2d_publisher.h>
#include <costmap_2d/Costmap2DConfig.h>
// #include <costmap_2d/costmap_2d_publisher.h>
// #include <costmap_2d/Costmap2DConfig.h>
#include <costmap_2d/footprint.h>
#include <geometry_msgs/Polygon.h>
#include <geometry_msgs/PolygonStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <dynamic_reconfigure/server.h>
// #include <dynamic_reconfigure/server.h>
// #include <pluginlib/class_loader.hpp>
#include <tf2/LinearMath/Transform.h>
#include <tf3/LinearMath/Transform.h>
#include <robot/rate.h>
#include <costmap_2d/data_convert.h>
// class SuperValue : public XmlRpc::XmlRpcValue
// {
@@ -71,7 +73,7 @@ namespace costmap_2d
/** @brief A ROS wrapper for a 2D Costmap. Handles subscribing to
* topics that provide observations about obstacles in either the form
* of PointCloud or LaserScan messages. */
class Costmap2DROS
class Costmap2DROBOT
{
public:
/**
@@ -79,8 +81,8 @@ public:
* @param name The name for this costmap
* @param tf A reference to a TransformListener
*/
Costmap2DROS(const std::string &name, tf2_ros::Buffer& tf);
~Costmap2DROS();
Costmap2DROBOT(const std::string &name, tf3::BufferCore& tf);
~Costmap2DROBOT();
/**
* @brief Subscribes to sensor topics if necessary and starts costmap
@@ -238,45 +240,48 @@ public:
protected:
LayeredCostmap* layered_costmap_;
std::string name_;
tf2_ros::Buffer& tf_; ///< @brief Used for transforming point clouds
tf3::BufferCore& tf_; ///< @brief Used for transforming point clouds
std::string global_frame_; ///< @brief The global frame for the costmap
std::string robot_base_frame_; ///< @brief The frame_id of the robot base
double transform_tolerance_; ///< timeout before transform errors
private:
/** @brief Set the footprint from the new_config object.
*
* If the values of footprint and robot_radius are the same in
* new_config and old_config, nothing is changed. */
void readFootprintFromConfig(const costmap_2d::Costmap2DConfig &new_config,
const costmap_2d::Costmap2DConfig &old_config);
// /** @brief Set the footprint from the new_config object.
// *
// * If the values of footprint and robot_radius are the same in
// * new_config and old_config, nothing is changed. */
// void readFootprintFromConfig(const costmap_2d::Costmap2DConfig &new_config,
// const costmap_2d::Costmap2DConfig &old_config);
void loadOldParameters(ros::NodeHandle& nh);
void warnForOldParameters(ros::NodeHandle& nh);
void checkOldParam(ros::NodeHandle& nh, const std::string &param_name);
void copyParentParameters(const std::string& plugin_name, const std::string& plugin_type, ros::NodeHandle& nh);
void reconfigureCB(costmap_2d::Costmap2DConfig &config, uint32_t level);
void movementCB(const ros::TimerEvent &event);
// void loadOldParameters(ros::NodeHandle& nh);
// void warnForOldParameters(ros::NodeHandle& nh);
// void checkOldParam(ros::NodeHandle& nh, const std::string &param_name);
// void copyParentParameters(const std::string& plugin_name, const std::string& plugin_type, ros::NodeHandle& nh);
// void reconfigureCB(costmap_2d::Costmap2DConfig &config, uint32_t level);
// void movementCB(const ros::TimerEvent &event);
void mapUpdateLoop(double frequency);
bool map_update_thread_shutdown_;
bool stop_updates_, initialized_, stopped_;
boost::thread* map_update_thread_; ///< @brief A thread for updating the map
ros::Time last_publish_;
ros::Duration publish_cycle;
pluginlib::ClassLoader<costmap_2d::Layer> plugin_loader_;
Costmap2DPublisher* publisher_;
dynamic_reconfigure::Server<costmap_2d::Costmap2DConfig> *dsrv_;
robot::Time last_publish_;
robot::Duration publish_cycle;
// pluginlib::ClassLoader<costmap_2d::Layer> plugin_loader_;
// Costmap2DPublisher* publisher_;
// dynamic_reconfigure::Server<costmap_2d::Costmap2DConfig> *dsrv_;
boost::recursive_mutex configuration_mutex_;
ros::Subscriber footprint_sub_;
ros::Publisher footprint_pub_;
// ros::Subscriber footprint_sub_;
// ros::Publisher footprint_pub_;
std::vector<geometry_msgs::Point> unpadded_footprint_;
std::vector<geometry_msgs::Point> padded_footprint_;
float footprint_padding_;
costmap_2d::Costmap2DConfig old_config_;
// costmap_2d::Costmap2DConfig old_config_;
private:
void getParams();
};
// class Costmap2DROS
// class Costmap2DROBOT
} // namespace costmap_2d
#endif // COSTMAP_2D_COSTMAP_2D_ROS_H
#endif // COSTMAP_2D_COSTMAP_2D_ROBOT_H

38
include/costmap_2d/data_convert.h
View File

@@ -5,8 +5,8 @@
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf2/utils.h>
#include <tf2/compat.h>
#include <tf3/utils.h>
#include <tf3/compat.h>
#include <robot/time.h>
#include <cmath>
@@ -56,7 +56,7 @@ namespace costmap_2d
return q;
}
robot::Time convertTime(tf2::Time time)
robot::Time convertTime(tf3::Time time)
{
robot::Time time_tmp;
time_tmp.sec = time.sec;
@@ -64,66 +64,66 @@ namespace costmap_2d
return time_tmp;
}
tf2::Time convertTime(robot::Time time)
tf3::Time convertTime(robot::Time time)
{
tf2::Time time_tmp;
tf3::Time time_tmp;
time_tmp.sec = time.sec;
time_tmp.nsec = time.nsec;
return time_tmp;
}
tf2::Quaternion convertQuaternion(const geometry_msgs::Quaternion& q)
tf3::Quaternion convertQuaternion(const geometry_msgs::Quaternion& q)
{
tf2::Quaternion out(q.x,q.y,q.z,q.w);
tf3::Quaternion out(q.x,q.y,q.z,q.w);
return out;
}
geometry_msgs::Quaternion convertQuaternion(const tf2::Quaternion& q)
geometry_msgs::Quaternion convertQuaternion(const tf3::Quaternion& q)
{
return geometry_msgs::Quaternion(q.x(),q.y(),q.z(),q.w());
}
tf2::Transform convertToTf2Transform(const geometry_msgs::Transform& msg)
tf3::Transform convertToTf2Transform(const geometry_msgs::Transform& msg)
{
tf2::Quaternion q(
tf3::Quaternion q(
msg.rotation.x,
msg.rotation.y,
msg.rotation.z,
msg.rotation.w
);
tf2::Vector3 t(
tf3::Vector3 t(
msg.translation.x,
msg.translation.y,
msg.translation.z
);
tf2::Transform tf(q, t);
tf3::Transform tf(q, t);
return tf;
}
tf2::Transform convertToTf2Transform(const tf2::TransformMsg& msg)
tf3::Transform convertToTf2Transform(const tf3::TransformMsg& msg)
{
tf2::Quaternion q(
tf3::Quaternion q(
msg.rotation.x,
msg.rotation.y,
msg.rotation.z,
msg.rotation.w
);
tf2::Vector3 t(
tf3::Vector3 t(
msg.translation.x,
msg.translation.y,
msg.translation.z
);
tf2::Transform tf(q, t);
tf3::Transform tf(q, t);
return tf;
}
tf2::TransformStampedMsg convertToTransformStampedMsg(const geometry_msgs::TransformStamped& msg)
tf3::TransformStampedMsg convertToTransformStampedMsg(const geometry_msgs::TransformStamped& msg)
{
tf2::TransformStampedMsg out;
tf3::TransformStampedMsg out;
out.header.seq = msg.header.seq;
out.header.stamp = convertTime(msg.header.stamp);
out.header.frame_id = msg.header.frame_id;
@@ -137,7 +137,7 @@ namespace costmap_2d
out.transform.rotation.w = msg.transform.rotation.w;
return out;
}
geometry_msgs::TransformStamped convertToTransformStamped(const tf2::TransformStampedMsg& msg)
geometry_msgs::TransformStamped convertToTransformStamped(const tf3::TransformStampedMsg& msg)
{
geometry_msgs::TransformStamped out;
out.header.seq = msg.header.seq;

6
include/costmap_2d/layer.h
View File

@@ -40,7 +40,7 @@
#include <costmap_2d/costmap_2d.h>
#include <costmap_2d/layered_costmap.h>
#include <string>
#include <tf2/buffer_core.h>
#include <tf3/buffer_core.h>
#include <costmap_2d/layered_costmap.h>
#include <boost/shared_ptr.hpp>
@@ -53,7 +53,7 @@ class Layer
public:
Layer();
void initialize(LayeredCostmap* parent, std::string name, tf2::BufferCore *tf);
void initialize(LayeredCostmap* parent, std::string name, tf3::BufferCore *tf);
/**
* @brief This is called by the LayeredCostmap to poll this plugin as to how
@@ -157,7 +157,7 @@ protected:
bool current_;
bool enabled_;
std::string name_;
tf2::BufferCore *tf_;
tf3::BufferCore *tf_;
private:
std::vector<geometry_msgs::Point> footprint_spec_;

8
include/costmap_2d/observation_buffer.h
View File

@@ -9,7 +9,7 @@
#include <robot/time.h>
#include <robot/duration.h>
#include <costmap_2d/observation.h>
#include <tf2/buffer_core.h>
#include <tf3/buffer_core.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/Point.h>
@@ -37,14 +37,14 @@ public:
* @param max_obstacle_height The minimum height of a hitpoint to be considered legal
* @param obstacle_range The range to which the sensor should be trusted for inserting obstacles
* @param raytrace_range The range to which the sensor should be trusted for raytracing to clear out space
* @param tf2_buffer A reference to a tf2 Buffer
* @param tf2_buffer A reference to a tf3 Buffer
* @param global_frame The frame to transform PointClouds into
* @param sensor_frame The frame of the origin of the sensor, can be left blank to be read from the messages
* @param tf_tolerance The amount of time to wait for a transform to be available when setting a new global frame
*/
ObservationBuffer(std::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, tf2::BufferCore& tf2_buffer, std::string global_frame,
double raytrace_range, tf3::BufferCore& tf2_buffer, std::string global_frame,
std::string sensor_frame, double tf_tolerance);
/**
@@ -107,7 +107,7 @@ private:
*/
void purgeStaleObservations();
tf2::BufferCore& tf2_buffer_;
tf3::BufferCore& tf2_buffer_;
// const ros::Duration observation_keep_time_;
// const ros::Duration expected_update_rate_;
// ros::Time last_updated_;