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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

130
CMakeLists.txt
View File

@ -1,118 +1,3 @@
# cmake_minimum_required(VERSION 3.10)
# project(costmap_2d)
# set(CMAKE_CXX_STANDARD 17)
# set(CMAKE_POSITION_INDEPENDENT_CODE ON)
# find_package(tf2 REQUIRED)
# if (NOT TARGET sensor_msgs)
# add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../common_msgs/sensor_msgs ${CMAKE_BINARY_DIR}/sensor_msgs_build)
# endif()
# if (NOT TARGET geometry_msgs)
# add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../common_msgs/geometry_msgs ${CMAKE_BINARY_DIR}/geometry_msgs_build)
# endif()
# if (NOT TARGET nav_msgs)
# add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../common_msgs/nav_msgs ${CMAKE_BINARY_DIR}/nav_msgs_build)
# endif()
# if (NOT TARGET map_msgs)
# add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../map_msgs ${CMAKE_BINARY_DIR}/map_msgs_build)
# endif()
# # ---- Dependencies ----
# find_package(Eigen3 REQUIRED)
# find_package(Boost REQUIRED COMPONENTS system thread)
# find_package(GTest REQUIRED)
# find_package(PCL REQUIRED COMPONENTS common io)
# include_directories(
# include
# ${EIGEN3_INCLUDE_DIRS}
# ${Boost_INCLUDE_DIRS}
# ${GTEST_INCLUDE_DIRS}
# ${PCL_INCLUDE_DIRS}
# /usr/include
# )
# link_directories(${PCL_LIBRARY_DIRS})
# add_definitions(${EIGEN3_DEFINITIONS}
# ${PCL_DEFINITIONS})
# # ---- Core costmap_2d library ----
# add_library(costmap_2d
# src/array_parser.cpp
# src/costmap_2d.cpp
# src/observation_buffer.cpp
# src/layer.cpp
# src/layered_costmap.cpp
# src/costmap_math.cpp
# src/footprint.cpp
# src/costmap_layer.cpp
# )
# target_link_libraries(costmap_2d
# ${Boost_LIBRARIES}
# std_msgs
# sensor_msgs
# geometry_msgs
# nav_msgs
# map_msgs
# # tf2
# )
# target_include_directories(costmap_2d PRIVATE ${Boost_INCLUDE_DIRS})
# # # ---- Layer plugins ----
# add_library(layers SHARED
# # plugins/inflation_layer.cpp
# # plugins/obstacle_layer.cpp
# plugins/static_layer.cpp
# # plugins/voxel_layer.cpp
# # plugins/preferred_layer.cpp
# # plugins/unpreferred_layer.cpp
# # plugins/critical_layer.cpp
# # plugins/directional_layer.cpp
# )
# target_link_libraries(layers
# costmap_2d
# ${Boost_LIBRARIES}
# yaml-cpp
# )
# # ---- Example Executable ----
# add_executable(test_array_parser test/array_parser_test.cpp)
# add_executable(test_costmap test/coordinates_test.cpp)
# add_executable(test_plugin test/static_layer_test.cpp)
# target_link_libraries(test_array_parser PRIVATE costmap_2d
# PRIVATE GTest::GTest
# PRIVATE GTest::Main
# PRIVATE pthread)
# target_link_libraries(test_costmap PRIVATE costmap_2d
# PRIVATE GTest::GTest
# PRIVATE GTest::Main
# PRIVATE pthread)
# target_link_libraries(test_plugin PRIVATE
# # /usr/lib/libtf2.so
# tf2
# costmap_2d
# ${Boost_LIBRARIES}
# Boost::filesystem
# Boost::system
# dl
# pthread
# yaml-cpp)
# set_target_properties(test_plugin PROPERTIES
# BUILD_RPATH "${CMAKE_BINARY_DIR}/costmap_2d"
# INSTALL_RPATH "${CMAKE_BINARY_DIR}/costmap_2d"
# )
cmake_minimum_required(VERSION 3.10) cmake_minimum_required(VERSION 3.10)
project(costmap_2d) project(costmap_2d)
@ -132,7 +17,7 @@ find_package(Eigen3 REQUIRED)
find_package(Boost REQUIRED COMPONENTS system thread filesystem) find_package(Boost REQUIRED COMPONENTS system thread filesystem)
find_package(GTest REQUIRED) find_package(GTest REQUIRED)
find_package(PCL REQUIRED COMPONENTS common io) find_package(PCL REQUIRED COMPONENTS common io)
set(TF2_LIBRARY /usr/lib/libtf2.so) # set(TF2_LIBRARY /usr/lib/libtf2.so)
# --- Include other message packages if needed --- # --- Include other message packages if needed ---
# if (NOT TARGET sensor_msgs) # if (NOT TARGET sensor_msgs)
@ -160,7 +45,6 @@ include_directories(
${Boost_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}
${GTEST_INCLUDE_DIRS} ${GTEST_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}
/usr/include
) )
link_directories(${PCL_LIBRARY_DIRS}) link_directories(${PCL_LIBRARY_DIRS})
@ -187,7 +71,8 @@ target_link_libraries(costmap_2d_new
map_msgs map_msgs
laser_geometry laser_geometry
voxel_grid voxel_grid
${TF2_LIBRARY} # ${TF2_LIBRARY}
tf3
) )
target_include_directories(costmap_2d_new PRIVATE ${Boost_INCLUDE_DIRS}) target_include_directories(costmap_2d_new PRIVATE ${Boost_INCLUDE_DIRS})
@ -268,6 +153,15 @@ target_link_libraries(unpreferred_layer
yaml-cpp yaml-cpp
) )
add_library(costmap_2d_robot SHARED
src/costmap_2d_robot.cpp
)
target_link_libraries(costmap_2d_robot
costmap_2d_new
${Boost_LIBRARIES}
yaml-cpp
)
# --- Test executables --- # --- Test executables ---
add_executable(test_array_parser test/array_parser_test.cpp) add_executable(test_array_parser test/array_parser_test.cpp)
add_executable(test_costmap test/coordinates_test.cpp) add_executable(test_costmap test/coordinates_test.cpp)

69
include/costmap_2d/costmap_2d_robot.h
View File

@ -35,20 +35,22 @@
* Author: Eitan Marder-Eppstein * Author: Eitan Marder-Eppstein
* David V. Lu!! * David V. Lu!!
*********************************************************************/ *********************************************************************/
#ifndef COSTMAP_2D_COSTMAP_2D_ROS_H_ #ifndef COSTMAP_2D_COSTMAP_2D_ROBOT_H_
#define COSTMAP_2D_COSTMAP_2D_ROS_H_ #define COSTMAP_2D_COSTMAP_2D_ROBOT_H_
#include <costmap_2d/layered_costmap.h> #include <costmap_2d/layered_costmap.h>
#include <costmap_2d/layer.h> #include <costmap_2d/layer.h>
#include <costmap_2d/costmap_2d_publisher.h> // #include <costmap_2d/costmap_2d_publisher.h>
#include <costmap_2d/Costmap2DConfig.h> // #include <costmap_2d/Costmap2DConfig.h>
#include <costmap_2d/footprint.h> #include <costmap_2d/footprint.h>
#include <geometry_msgs/Polygon.h> #include <geometry_msgs/Polygon.h>
#include <geometry_msgs/PolygonStamped.h> #include <geometry_msgs/PolygonStamped.h>
#include <geometry_msgs/PoseStamped.h> #include <geometry_msgs/PoseStamped.h>
#include <dynamic_reconfigure/server.h> // #include <dynamic_reconfigure/server.h>
// #include <pluginlib/class_loader.hpp> // #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 // class SuperValue : public XmlRpc::XmlRpcValue
// { // {
@ -71,7 +73,7 @@ namespace costmap_2d
/** @brief A ROS wrapper for a 2D Costmap. Handles subscribing to /** @brief A ROS wrapper for a 2D Costmap. Handles subscribing to
* topics that provide observations about obstacles in either the form * topics that provide observations about obstacles in either the form
* of PointCloud or LaserScan messages. */ * of PointCloud or LaserScan messages. */
class Costmap2DROS class Costmap2DROBOT
{ {
public: public:
/** /**
@ -79,8 +81,8 @@ public:
* @param name The name for this costmap * @param name The name for this costmap
* @param tf A reference to a TransformListener * @param tf A reference to a TransformListener
*/ */
Costmap2DROS(const std::string &name, tf2_ros::Buffer& tf); Costmap2DROBOT(const std::string &name, tf3::BufferCore& tf);
~Costmap2DROS(); ~Costmap2DROBOT();
/** /**
* @brief Subscribes to sensor topics if necessary and starts costmap * @brief Subscribes to sensor topics if necessary and starts costmap
@ -238,45 +240,48 @@ public:
protected: protected:
LayeredCostmap* layered_costmap_; LayeredCostmap* layered_costmap_;
std::string name_; 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 global_frame_; ///< @brief The global frame for the costmap
std::string robot_base_frame_; ///< @brief The frame_id of the robot base std::string robot_base_frame_; ///< @brief The frame_id of the robot base
double transform_tolerance_; ///< timeout before transform errors double transform_tolerance_; ///< timeout before transform errors
private: private:
/** @brief Set the footprint from the new_config object. // /** @brief Set the footprint from the new_config object.
* // *
* If the values of footprint and robot_radius are the same in // * If the values of footprint and robot_radius are the same in
* new_config and old_config, nothing is changed. */ // * new_config and old_config, nothing is changed. */
void readFootprintFromConfig(const costmap_2d::Costmap2DConfig &new_config, // void readFootprintFromConfig(const costmap_2d::Costmap2DConfig &new_config,
const costmap_2d::Costmap2DConfig &old_config); // const costmap_2d::Costmap2DConfig &old_config);
void loadOldParameters(ros::NodeHandle& nh); // void loadOldParameters(ros::NodeHandle& nh);
void warnForOldParameters(ros::NodeHandle& nh); // void warnForOldParameters(ros::NodeHandle& nh);
void checkOldParam(ros::NodeHandle& nh, const std::string &param_name); // 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 copyParentParameters(const std::string& plugin_name, const std::string& plugin_type, ros::NodeHandle& nh);
void reconfigureCB(costmap_2d::Costmap2DConfig &config, uint32_t level); // void reconfigureCB(costmap_2d::Costmap2DConfig &config, uint32_t level);
void movementCB(const ros::TimerEvent &event); // void movementCB(const ros::TimerEvent &event);
void mapUpdateLoop(double frequency); void mapUpdateLoop(double frequency);
bool map_update_thread_shutdown_; bool map_update_thread_shutdown_;
bool stop_updates_, initialized_, stopped_; bool stop_updates_, initialized_, stopped_;
boost::thread* map_update_thread_; ///< @brief A thread for updating the map boost::thread* map_update_thread_; ///< @brief A thread for updating the map
ros::Time last_publish_; robot::Time last_publish_;
ros::Duration publish_cycle; robot::Duration publish_cycle;
pluginlib::ClassLoader<costmap_2d::Layer> plugin_loader_; // pluginlib::ClassLoader<costmap_2d::Layer> plugin_loader_;
Costmap2DPublisher* publisher_; // Costmap2DPublisher* publisher_;
dynamic_reconfigure::Server<costmap_2d::Costmap2DConfig> *dsrv_; // dynamic_reconfigure::Server<costmap_2d::Costmap2DConfig> *dsrv_;
boost::recursive_mutex configuration_mutex_; boost::recursive_mutex configuration_mutex_;
ros::Subscriber footprint_sub_; // ros::Subscriber footprint_sub_;
ros::Publisher footprint_pub_; // ros::Publisher footprint_pub_;
std::vector<geometry_msgs::Point> unpadded_footprint_; std::vector<geometry_msgs::Point> unpadded_footprint_;
std::vector<geometry_msgs::Point> padded_footprint_; std::vector<geometry_msgs::Point> padded_footprint_;
float footprint_padding_; float footprint_padding_;
costmap_2d::Costmap2DConfig old_config_; // costmap_2d::Costmap2DConfig old_config_;
private:
void getParams();
}; };
// class Costmap2DROS // class Costmap2DROBOT
} // namespace costmap_2d } // 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/Quaternion.h>
#include <geometry_msgs/Pose.h> #include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseStamped.h> #include <geometry_msgs/PoseStamped.h>
#include <tf2/utils.h> #include <tf3/utils.h>
#include <tf2/compat.h> #include <tf3/compat.h>
#include <robot/time.h> #include <robot/time.h>
#include <cmath> #include <cmath>
@ -56,7 +56,7 @@ namespace costmap_2d
return q; return q;
} }
robot::Time convertTime(tf2::Time time) robot::Time convertTime(tf3::Time time)
{ {
robot::Time time_tmp; robot::Time time_tmp;
time_tmp.sec = time.sec; time_tmp.sec = time.sec;
@ -64,66 +64,66 @@ namespace costmap_2d
return time_tmp; 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.sec = time.sec;
time_tmp.nsec = time.nsec; time_tmp.nsec = time.nsec;
return time_tmp; 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; 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()); 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.x,
msg.rotation.y, msg.rotation.y,
msg.rotation.z, msg.rotation.z,
msg.rotation.w msg.rotation.w
); );
tf2::Vector3 t( tf3::Vector3 t(
msg.translation.x, msg.translation.x,
msg.translation.y, msg.translation.y,
msg.translation.z msg.translation.z
); );
tf2::Transform tf(q, t); tf3::Transform tf(q, t);
return tf; 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.x,
msg.rotation.y, msg.rotation.y,
msg.rotation.z, msg.rotation.z,
msg.rotation.w msg.rotation.w
); );
tf2::Vector3 t( tf3::Vector3 t(
msg.translation.x, msg.translation.x,
msg.translation.y, msg.translation.y,
msg.translation.z msg.translation.z
); );
tf2::Transform tf(q, t); tf3::Transform tf(q, t);
return tf; 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.seq = msg.header.seq;
out.header.stamp = convertTime(msg.header.stamp); out.header.stamp = convertTime(msg.header.stamp);
out.header.frame_id = msg.header.frame_id; out.header.frame_id = msg.header.frame_id;
@ -137,7 +137,7 @@ namespace costmap_2d
out.transform.rotation.w = msg.transform.rotation.w; out.transform.rotation.w = msg.transform.rotation.w;
return out; return out;
} }
geometry_msgs::TransformStamped convertToTransformStamped(const tf2::TransformStampedMsg& msg) geometry_msgs::TransformStamped convertToTransformStamped(const tf3::TransformStampedMsg& msg)
{ {
geometry_msgs::TransformStamped out; geometry_msgs::TransformStamped out;
out.header.seq = msg.header.seq; 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/costmap_2d.h>
#include <costmap_2d/layered_costmap.h> #include <costmap_2d/layered_costmap.h>
#include <string> #include <string>
#include <tf2/buffer_core.h> #include <tf3/buffer_core.h>
#include <costmap_2d/layered_costmap.h> #include <costmap_2d/layered_costmap.h>
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
@ -53,7 +53,7 @@ class Layer
public: public:
Layer(); 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 * @brief This is called by the LayeredCostmap to poll this plugin as to how
@ -157,7 +157,7 @@ protected:
bool current_; bool current_;
bool enabled_; bool enabled_;
std::string name_; std::string name_;
tf2::BufferCore *tf_; tf3::BufferCore *tf_;
private: private:
std::vector<geometry_msgs::Point> footprint_spec_; 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/time.h>
#include <robot/duration.h> #include <robot/duration.h>
#include <costmap_2d/observation.h> #include <costmap_2d/observation.h>
#include <tf2/buffer_core.h> #include <tf3/buffer_core.h>
#include <sensor_msgs/PointCloud2.h> #include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/PointStamped.h> #include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/Point.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 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 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 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 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 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 * @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, 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 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); std::string sensor_frame, double tf_tolerance);
/** /**
@ -107,7 +107,7 @@ private:
*/ */
void purgeStaleObservations(); void purgeStaleObservations();
tf2::BufferCore& tf2_buffer_; tf3::BufferCore& tf2_buffer_;
// const ros::Duration observation_keep_time_; // const ros::Duration observation_keep_time_;
// const ros::Duration expected_update_rate_; // const ros::Duration expected_update_rate_;
// ros::Time last_updated_; // ros::Time last_updated_;

22
plugins/directional_layer.cpp
View File

@ -1,7 +1,7 @@
#include <costmap_2d/directional_layer.h> #include <costmap_2d/directional_layer.h>
#include <costmap_2d/data_convert.h> #include <costmap_2d/data_convert.h>
#include <tf2/convert.h> #include <tf3/convert.h>
#include <tf2/utils.h> #include <tf3/utils.h>
#include <boost/dll/alias.hpp> #include <boost/dll/alias.hpp>
@ -134,8 +134,8 @@ namespace costmap_2d
return false; return false;
} }
// Convert to yaw // Convert to yaw
tf2::Quaternion quaternion = convertQuaternion(p.pose.orientation); tf3::Quaternion quaternion = convertQuaternion(p.pose.orientation);
double theta = tf2::getYaw(quaternion); double theta = tf3::getYaw(quaternion);
unsigned char value = laneFilter(mx, my, theta); unsigned char value = laneFilter(mx, my, theta);
if (get_success) if (get_success)
{ {
@ -399,27 +399,27 @@ namespace costmap_2d
bool DirectionalLayer::getRobotPose(double &x, double &y, double &yaw) bool DirectionalLayer::getRobotPose(double &x, double &y, double &yaw)
{ {
tf2::TransformStampedMsg transformMsg; tf3::TransformStampedMsg transformMsg;
try try
{ {
transformMsg = tf_->lookupTransform(map_frame_, base_frame_id_, tf2::Time()); transformMsg = tf_->lookupTransform(map_frame_, base_frame_id_, tf3::Time());
// listener_.lookupTransform(map_frame_, base_frame_id_, tf2::Time(), transform); // listener_.lookupTransform(map_frame_, base_frame_id_, tf3::Time(), transform);
} }
catch (tf2::TransformException &ex) catch (tf3::TransformException &ex)
{ {
printf("%s\n", ex.what()); printf("%s\n", ex.what());
return false; return false;
} }
// Copy map data given proper transformations // Copy map data given proper transformations
tf2::Transform tf2_transform; tf3::Transform tf2_transform;
tf2_transform = convertToTf2Transform(transformMsg.transform); tf2_transform = convertToTf2Transform(transformMsg.transform);
x = tf2_transform.getOrigin().x(); x = tf2_transform.getOrigin().x();
y = tf2_transform.getOrigin().y(); y = tf2_transform.getOrigin().y();
// Extract the rotation as a quaternion from the transform // Extract the rotation as a quaternion from the transform
tf2::Quaternion rotation = tf2_transform.getRotation(); tf3::Quaternion rotation = tf2_transform.getRotation();
// Convert the quaternion to a yaw angle (in radians) // Convert the quaternion to a yaw angle (in radians)
yaw = tf2::getYaw(rotation); yaw = tf3::getYaw(rotation);
return true; return true;
} }

8
plugins/obstacle_layer.cpp
View File

@ -2,8 +2,8 @@
#include <costmap_2d/costmap_math.h> #include <costmap_2d/costmap_math.h>
#include <costmap_2d/utils.h> #include <costmap_2d/utils.h>
#include <sensor_msgs/point_cloud2_iterator.h> #include <sensor_msgs/point_cloud2_iterator.h>
#include <tf2/convert.h> #include <tf3/convert.h>
#include <tf2/utils.h> #include <tf3/utils.h>
#include <boost/dll/alias.hpp> #include <boost/dll/alias.hpp>
using costmap_2d::NO_INFORMATION; using costmap_2d::NO_INFORMATION;
@ -154,7 +154,7 @@ void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScan& message,
{ {
projector_.transformLaserScanToPointCloud(message.header.frame_id, message, cloud, *tf_); projector_.transformLaserScanToPointCloud(message.header.frame_id, message, cloud, *tf_);
} }
catch (tf2::TransformException &ex) catch (tf3::TransformException &ex)
{ {
printf("High fidelity enabled, but TF returned a transform exception to frame %s: %s\n", global_frame_.c_str(), printf("High fidelity enabled, but TF returned a transform exception to frame %s: %s\n", global_frame_.c_str(),
ex.what()); ex.what());
@ -197,7 +197,7 @@ void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScan& raw_
{ {
projector_.transformLaserScanToPointCloud(message.header.frame_id, message, cloud, *tf_); projector_.transformLaserScanToPointCloud(message.header.frame_id, message, cloud, *tf_);
} }
catch (tf2::TransformException &ex) catch (tf3::TransformException &ex)
{ {
printf("High fidelity enabled, but TF returned a transform exception to frame %s: %s\n", printf("High fidelity enabled, but TF returned a transform exception to frame %s: %s\n",
global_frame_.c_str(), ex.what()); global_frame_.c_str(), ex.what());

20
plugins/static_layer.cpp
View File

@ -3,8 +3,8 @@
#include <costmap_2d/costmap_math.h> #include <costmap_2d/costmap_math.h>
#include <costmap_2d/data_convert.h> #include <costmap_2d/data_convert.h>
#include <costmap_2d/utils.h> #include <costmap_2d/utils.h>
#include <tf2/convert.h> #include <tf3/convert.h>
#include <tf2/utils.h> #include <tf3/utils.h>
#include <boost/dll/alias.hpp> #include <boost/dll/alias.hpp>
@ -283,25 +283,25 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int
double wx, wy; double wx, wy;
// Might even be in a different frame // Might even be in a different frame
// geometry_msgs::TransformStamped transform; // geometry_msgs::TransformStamped transform;
tf2::TransformStampedMsg transformMsg; tf3::TransformStampedMsg transformMsg;
try try
{ {
// transformMsg = tf_->lookupTransform(map_frame_, global_frame_, ros::Time(0)); // transformMsg = tf_->lookupTransform(map_frame_, global_frame_, ros::Time(0));
bool status =tf_->canTransform(map_frame_, global_frame_, tf2::Time()); bool status =tf_->canTransform(map_frame_, global_frame_, tf3::Time());
if(!status) throw tf2::TransformException("[static_layer] Cannot transform"); if(!status) throw tf3::TransformException("[static_layer] Cannot transform");
transformMsg = tf_->lookupTransform(map_frame_, transformMsg = tf_->lookupTransform(map_frame_,
global_frame_, global_frame_,
tf2::Time()); tf3::Time());
} }
catch (tf2::TransformException ex) catch (tf3::TransformException ex)
{ {
printf("%s", ex.what()); printf("%s", ex.what());
return; return;
} }
// Copy map data given proper transformations // Copy map data given proper transformations
tf2::Transform tf2_transform; tf3::Transform tf2_transform;
tf2_transform = convertToTf2Transform(transformMsg.transform); tf2_transform = convertToTf2Transform(transformMsg.transform);
// tf2::convert(transformMsg.transform, tf2_transform); // tf3::convert(transformMsg.transform, tf2_transform);
for (unsigned int i = min_i; i < max_i; ++i) for (unsigned int i = min_i; i < max_i; ++i)
{ {
for (unsigned int j = min_j; j < max_j; ++j) for (unsigned int j = min_j; j < max_j; ++j)
@ -309,7 +309,7 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int
// Convert master_grid coordinates (i,j) into global_frame_(wx,wy) coordinates // Convert master_grid coordinates (i,j) into global_frame_(wx,wy) coordinates
layered_costmap_->getCostmap()->mapToWorld(i, j, wx, wy); layered_costmap_->getCostmap()->mapToWorld(i, j, wx, wy);
// Transform from global_frame_ to map_frame_ // Transform from global_frame_ to map_frame_
tf2::Vector3 p(wx, wy, 0); tf3::Vector3 p(wx, wy, 0);
p = tf2_transform*p; p = tf2_transform*p;
// Set master_grid with cell from map // Set master_grid with cell from map
if (worldToMap(p.x(), p.y(), mx, my)) if (worldToMap(p.x(), p.y(), mx, my))

630
src/costmap_2d_robot.cpp Normal file
View File

@ -0,0 +1,630 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, 2013, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* 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 <tf3/convert.h>
#include <tf3/utils.h>
// #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
using namespace std;
namespace costmap_2d
{
// void move_parameter(ros::NodeHandle& old_h, ros::NodeHandle& new_h, std::string name, bool should_delete = true)
// {
// if (!old_h.hasParam(name))
// return;
// XmlRpc::XmlRpcValue value;
// old_h.getParam(name, value);
// new_h.setParam(name, value);
// if (should_delete) old_h.deleteParam(name);
// }
Costmap2DROBOT::Costmap2DROBOT(const std::string& name, tf3::BufferCore& tf) :
layered_costmap_(NULL),
name_(name),
tf_(tf),
transform_tolerance_(0.3),
map_update_thread_shutdown_(false),
stop_updates_(false),
initialized_(true),
stopped_(false),
map_update_thread_(NULL),
footprint_padding_(0.0)
{
// ros::NodeHandle private_nh("~/" + name);
// ros::NodeHandle g_nh;
// // get global and robot base frame names
// private_nh.param("global_frame", global_frame_, std::string("map"));
// private_nh.param("robot_base_frame", robot_base_frame_, std::string("base_link"));
// 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
// while (ros::ok()
// && !tf_.canTransform(global_frame_, robot_base_frame_, ros::Time(), ros::Duration(0.1), &tf_error))
// {
// ros::spinOnce();
// if (last_error + ros::Duration(5.0) < ros::Time::now())
// {
// ROS_WARN("Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s",
// robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str());
// last_error = ros::Time::now();
// }
// // The error string will accumulate and errors will typically be the same, so the last
// // will do for the warning above. Reset the string here to avoid accumulation.
// tf_error.clear();
// }
// // check if we want a rolling window version of the costmap
// bool rolling_window, track_unknown_space, always_send_full_costmap;
// private_nh.param("rolling_window", rolling_window, false);
// private_nh.param("track_unknown_space", track_unknown_space, false);
// private_nh.param("always_send_full_costmap", always_send_full_costmap, false);
// layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);
// if (!private_nh.hasParam("plugins"))
// {
// loadOldParameters(private_nh);
// } else {
// warnForOldParameters(private_nh);
// }
// if (private_nh.hasParam("plugins"))
// {
// XmlRpc::XmlRpcValue my_list;
// private_nh.getParam("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"]);
// ROS_INFO("%s: Using plugin \"%s\" with type %s", name_.c_str(), pname.c_str(), type.c_str());
// try
// {
// copyParentParameters(pname, type, private_nh);
// boost::shared_ptr<costmap_2d::Layer> plugin = plugin_loader_.createInstance(type);
// layered_costmap_->addPlugin(plugin);
// plugin->initialize(layered_costmap_, name + "/" + pname, &tf_);
// }
// catch (pluginlib::PluginlibException &ex)
// {
// ROS_FATAL("Failed to create the %s, are you sure it is properly registered and that the containing library is built? Exception: %s", type.c_str(), ex.what());
// }
// }
// }
// // subscribe to the footprint topic
// std::string topic_param, topic;
// if (!private_nh.searchParam("footprint_topic", topic_param))
// {
// topic_param = "footprint_topic";
// }
// private_nh.param(topic_param, topic, std::string("footprint"));
// footprint_sub_ = private_nh.subscribe(topic, 1, &Costmap2DROBOT::setUnpaddedRobotFootprintPolygon, this);
// if (!private_nh.searchParam("published_footprint_topic", topic_param))
// {
// topic_param = "published_footprint";
// }
// private_nh.param(topic_param, topic, std::string("footprint")); // TODO: revert to oriented_footprint in N-turtle
// footprint_pub_ = private_nh.advertise<geometry_msgs::PolygonStamped>(topic, 1);
// setUnpaddedRobotFootprint(makeFootprintFromParams(private_nh));
// publisher_ = new Costmap2DPublisher(&private_nh, layered_costmap_->getCostmap(), global_frame_, "costmap",
// always_send_full_costmap);
// // create a thread to handle updating the map
// stop_updates_ = false;
// initialized_ = true;
// stopped_ = false;
// dsrv_ = new dynamic_reconfigure::Server<Costmap2DConfig>(ros::NodeHandle("~/" + name));
// dynamic_reconfigure::Server<Costmap2DConfig>::CallbackType cb = [this](auto& config, auto level){ reconfigureCB(config, level); };
// dsrv_->setCallback(cb);
}
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"));
}
void Costmap2DROBOT::setUnpaddedRobotFootprintPolygon(const geometry_msgs::Polygon& footprint)
{
setUnpaddedRobotFootprint(toPointVector(footprint));
}
Costmap2DROBOT::~Costmap2DROBOT()
{
map_update_thread_shutdown_ = true;
if (map_update_thread_ != NULL)
{
map_update_thread_->join();
delete map_update_thread_;
}
// if (publisher_ != NULL)
// delete publisher_;
delete layered_costmap_;
// delete dsrv_;
}
// void Costmap2DROBOT::loadOldParameters(ros::NodeHandle& nh)
// {
// // if(name_.find("global_costmap")) ROS_WARN("Costmap2DROBOT::loadOldParameters");
// ROS_WARN("%s: Parameter \"plugins\" not provided, loading pre-Hydro parameters", name_.c_str());
// bool flag;
// std::string s;
// std::vector < XmlRpc::XmlRpcValue > plugins;
// XmlRpc::XmlRpcValue::ValueStruct map;
// SuperValue super_map;
// SuperValue super_array;
// if (nh.getParam("static_map", flag) && flag)
// {
// map["name"] = XmlRpc::XmlRpcValue("static_layer");
// map["type"] = XmlRpc::XmlRpcValue("costmap_2d::StaticLayer");
// super_map.setStruct(&map);
// plugins.push_back(super_map);
// ros::NodeHandle map_layer(nh, "static_layer");
// move_parameter(nh, map_layer, "map_topic");
// move_parameter(nh, map_layer, "unknown_cost_value");
// move_parameter(nh, map_layer, "lethal_cost_threshold");
// move_parameter(nh, map_layer, "track_unknown_space", false);
// }
// ros::NodeHandle obstacles(nh, "obstacle_layer");
// if (nh.getParam("map_type", s) && s == "voxel")
// {
// map["name"] = XmlRpc::XmlRpcValue("obstacle_layer");
// map["type"] = XmlRpc::XmlRpcValue("costmap_2d::VoxelLayer");
// super_map.setStruct(&map);
// plugins.push_back(super_map);
// move_parameter(nh, obstacles, "origin_z");
// move_parameter(nh, obstacles, "z_resolution");
// move_parameter(nh, obstacles, "z_voxels");
// move_parameter(nh, obstacles, "mark_threshold");
// move_parameter(nh, obstacles, "unknown_threshold");
// move_parameter(nh, obstacles, "publish_voxel_map");
// }
// else
// {
// map["name"] = XmlRpc::XmlRpcValue("obstacle_layer");
// map["type"] = XmlRpc::XmlRpcValue("costmap_2d::ObstacleLayer");
// super_map.setStruct(&map);
// plugins.push_back(super_map);
// }
// move_parameter(nh, obstacles, "max_obstacle_height");
// move_parameter(nh, obstacles, "raytrace_range");
// move_parameter(nh, obstacles, "obstacle_range");
// move_parameter(nh, obstacles, "track_unknown_space", true);
// nh.param("observation_sources", s, std::string(""));
// std::stringstream ss(s);
// std::string source;
// while (ss >> source)
// {
// move_parameter(nh, obstacles, source);
// }
// move_parameter(nh, obstacles, "observation_sources");
// ros::NodeHandle inflation(nh, "inflation_layer");
// move_parameter(nh, inflation, "cost_scaling_factor");
// move_parameter(nh, inflation, "inflation_radius");
// map["name"] = XmlRpc::XmlRpcValue("inflation_layer");
// map["type"] = XmlRpc::XmlRpcValue("costmap_2d::InflationLayer");
// super_map.setStruct(&map);
// plugins.push_back(super_map);
// super_array.setArray(&plugins);
// nh.setParam("plugins", super_array);
// }
// void Costmap2DROBOT::copyParentParameters(const std::string& plugin_name, const std::string& plugin_type, ros::NodeHandle& nh)
// {
// ros::NodeHandle target_layer(nh, plugin_name);
// if(plugin_type == "costmap_2d::StaticLayer")
// {
// move_parameter(nh, target_layer, "map_topic", false);
// move_parameter(nh, target_layer, "unknown_cost_value", false);
// move_parameter(nh, target_layer, "lethal_cost_threshold", false);
// move_parameter(nh, target_layer, "track_unknown_space", false);
// }
// else if(plugin_type == "costmap_2d::VoxelLayer")
// {
// move_parameter(nh, target_layer, "origin_z", false);
// move_parameter(nh, target_layer, "z_resolution", false);
// move_parameter(nh, target_layer, "z_voxels", false);
// move_parameter(nh, target_layer, "mark_threshold", false);
// move_parameter(nh, target_layer, "unknown_threshold", false);
// move_parameter(nh, target_layer, "publish_voxel_map", false);
// }
// else if(plugin_type == "costmap_2d::ObstacleLayer")
// {
// move_parameter(nh, target_layer, "max_obstacle_height", false);
// move_parameter(nh, target_layer, "raytrace_range", false);
// move_parameter(nh, target_layer, "obstacle_range", false);
// move_parameter(nh, target_layer, "track_unknown_space", false);
// }
// else if(plugin_type == "costmap_2d::InflationLayer")
// {
// move_parameter(nh, target_layer, "cost_scaling_factor", false);
// move_parameter(nh, target_layer, "inflation_radius", false);
// }
// }
// void Costmap2DROBOT::warnForOldParameters(ros::NodeHandle& nh)
// {
// // if(name_.find("global_costmap")) ROS_WARN("Costmap2DROBOT::warnForOldParameters");
// checkOldParam(nh, "static_map");
// checkOldParam(nh, "map_type");
// }
// void Costmap2DROBOT::checkOldParam(ros::NodeHandle& nh, const std::string &param_name){
// // if(name_.find("global_costmap")) ROS_WARN("Costmap2DROBOT::checkOldParam");
// if(nh.hasParam(param_name)){
// ROS_WARN("%s: Pre-Hydro parameter \"%s\" unused since \"plugins\" is provided", name_.c_str(), param_name.c_str());
// }
// }
// void Costmap2DROBOT::reconfigureCB(costmap_2d::Costmap2DConfig &config, uint32_t level)
// {
// // if(name_.find("global_costmap")) ROS_WARN("Costmap2DROBOT::reconfigureCB");
// transform_tolerance_ = config.transform_tolerance;
// if (map_update_thread_ != NULL)
// {
// map_update_thread_shutdown_ = true;
// map_update_thread_->join();
// delete map_update_thread_;
// map_update_thread_ = NULL;
// }
// map_update_thread_shutdown_ = false;
// double map_update_frequency = config.update_frequency;
// double map_publish_frequency = config.publish_frequency;
// if (map_publish_frequency > 0)
// publish_cycle = ros::Duration(1 / map_publish_frequency);
// else
// publish_cycle = ros::Duration(-1);
// // find size parameters
// double map_width_meters = config.width, map_height_meters = config.height, resolution = config.resolution, origin_x =
// config.origin_x,
// origin_y = config.origin_y;
// if (!layered_costmap_->isSizeLocked())
// {
// layered_costmap_->resizeMap((unsigned int)(map_width_meters / resolution),
// (unsigned int)(map_height_meters / resolution), resolution, origin_x, origin_y);
// }
// // If the padding has changed, call setUnpaddedRobotFootprint() to
// // re-apply the padding.
// if (footprint_padding_ != config.footprint_padding)
// {
// footprint_padding_ = config.footprint_padding;
// setUnpaddedRobotFootprint(unpadded_footprint_);
// }
// readFootprintFromConfig(config, old_config_);
// old_config_ = config;
// // only construct the thread if the frequency is positive
// if(map_update_frequency > 0.0)
// map_update_thread_ = new boost::thread(boost::bind(&Costmap2DROBOT::mapUpdateLoop, this, map_update_frequency));
// }
// void Costmap2DROBOT::readFootprintFromConfig(const costmap_2d::Costmap2DConfig &new_config,
// const costmap_2d::Costmap2DConfig &old_config)
// {
// // Only change the footprint if footprint or robot_radius has
// // changed. Otherwise we might overwrite a footprint sent on a
// // topic by a dynamic_reconfigure call which was setting some other
// // variable.
// // if(name_.find("global_costmap")) ROS_WARN("Costmap2DROBOT::readFootprintFromConfig");
// if (new_config.footprint == old_config.footprint &&
// new_config.robot_radius == old_config.robot_radius)
// {
// return;
// }
// if (new_config.footprint != "" && new_config.footprint != "[]")
// {
// std::vector<geometry_msgs::Point> new_footprint;
// if (makeFootprintFromString(new_config.footprint, new_footprint))
// {
// setUnpaddedRobotFootprint(new_footprint);
// }
// else
// {
// ROS_ERROR("Invalid footprint string from dynamic reconfigure");
// }
// }
// else
// {
// // robot_radius may be 0, but that must be intended at this point.
// setUnpaddedRobotFootprint(makeFootprintFromRadius(new_config.robot_radius));
// }
// }
void Costmap2DROBOT::setUnpaddedRobotFootprint(const std::vector<geometry_msgs::Point>& points)
{
unpadded_footprint_ = points;
padded_footprint_ = points;
padFootprint(padded_footprint_, footprint_padding_);
layered_costmap_->setFootprint(padded_footprint_);
}
// void Costmap2DROBOT::movementCB(const ros::TimerEvent &event)
// {
// // don't allow configuration to happen while this check occurs
// // boost::recursive_mutex::scoped_lock mcl(configuration_mutex_);
// // if(name_.find("global_costmap")) ROS_WARN("Costmap2DROBOT::movementCB");
// geometry_msgs::PoseStamped new_pose;
// if (!getRobotPose(new_pose))
// {
// ROS_WARN_THROTTLE(1.0, "Could not get robot pose, cancelling reconfiguration");
// }
// }
// void Costmap2DROBOT::mapUpdateLoop(double frequency)
// {
// ros::NodeHandle nh;
// ros::Rate r(frequency);
// while (nh.ok() && !map_update_thread_shutdown_)
// {
// #ifdef HAVE_SYS_TIME_H
// struct timeval start, end;
// double start_t, end_t, t_diff;
// gettimeofday(&start, NULL);
// #endif
// updateMap();
// #ifdef HAVE_SYS_TIME_H
// gettimeofday(&end, NULL);
// start_t = start.tv_sec + double(start.tv_usec) / 1e6;
// end_t = end.tv_sec + double(end.tv_usec) / 1e6;
// t_diff = end_t - start_t;
// ROS_DEBUG("Map update time: %.9f", t_diff);
// #endif
// if (publish_cycle.toSec() > 0 && layered_costmap_->isInitialized())
// {
// unsigned int x0, y0, xn, yn;
// layered_costmap_->getBounds(&x0, &xn, &y0, &yn);
// publisher_->updateBounds(x0, xn, y0, yn);
// ros::Time now = ros::Time::now();
// if (last_publish_ + publish_cycle < now)
// {
// publisher_->publishCostmap();
// last_publish_ = now;
// }
// }
// ros::spinOnce();
// r.sleep();
// // make sure to sleep for the remainder of our cycle time
// if (r.cycleTime() > ros::Duration(1 / frequency))
// ROS_WARN("Map update loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds", frequency,
// r.cycleTime().toSec());
// }
// }
void Costmap2DROBOT::updateMap()
{
if (!stop_updates_)
{
// get global pose
geometry_msgs::PoseStamped pose;
if (getRobotPose (pose))
{
double x = pose.pose.position.x,
y = pose.pose.position.y,
yaw = getYaw(pose.pose.orientation);
layered_costmap_->updateMap(x, y, yaw);
// geometry_msgs::PolygonStamped footprint;
// footprint.header.frame_id = global_frame_;
// footprint.header.stamp = ros::Time::now();
// transformFootprint(x, y, yaw, padded_footprint_, footprint);
// footprint_pub_.publish(footprint);
initialized_ = true;
}
}
}
void Costmap2DROBOT::start()
{
// if(name_.find("global_costmap")) ROS_WARN("Costmap2DROBOT::start");
std::vector < PluginLayerPtr> *plugins = layered_costmap_->getPlugins();
// check if we're stopped or just paused
if (stopped_)
{
// if we're stopped we need to re-subscribe to topics
for (vector<PluginLayerPtr>::iterator plugin = plugins->begin(); plugin != plugins->end();
++plugin)
{
(*plugin)->activate();
}
stopped_ = false;
}
stop_updates_ = false;
// block until the costmap is re-initialized.. meaning one update cycle has run
// note: this does not hold, if the user has disabled map-updates allgother
// robot::Rate r(100.0);
// while (ros::ok() && !initialized_ && map_update_thread_)
// r.sleep();
}
void Costmap2DROBOT::stop()
{
// if(name_.find("global_costmap")) ROS_WARN("Costmap2DROBOT::stop");
stop_updates_ = true;
std::vector <PluginLayerPtr> *plugins = layered_costmap_->getPlugins();
// unsubscribe from topics
for (vector<PluginLayerPtr>::iterator plugin = plugins->begin(); plugin != plugins->end();
++plugin)
{
(*plugin)->deactivate();
}
initialized_ = false;
stopped_ = true;
}
void Costmap2DROBOT::pause()
{
// if(name_.find("global_costmap")) ROS_WARN("Costmap2DROBOT::pause");
stop_updates_ = true;
initialized_ = false;
}
void Costmap2DROBOT::resume()
{
// if(name_.find("global_costmap")) ROS_WARN("Costmap2DROBOT::resume");
stop_updates_ = false;
// block until the costmap is re-initialized.. meaning one update cycle has run
// robot::Rate r(100.0);
// while (!initialized_)
// r.sleep();
}
void Costmap2DROBOT::resetLayers()
{
// if(name_.find("global_costmap")) ROS_WARN("Costmap2DROBOT::resetLayers");
Costmap2D* top = layered_costmap_->getCostmap();
top->resetMap(0, 0, top->getSizeInCellsX(), top->getSizeInCellsY());
std::vector <PluginLayerPtr> *plugins = layered_costmap_->getPlugins();
for (vector<PluginLayerPtr>::iterator plugin = plugins->begin(); plugin != plugins->end();
++plugin)
{
(*plugin)->reset();
}
}
bool Costmap2DROBOT::getRobotPose(geometry_msgs::PoseStamped& global_pose) const
{
// if(name_.find("global_costmap")) ROS_WARN("Costmap2DROBOT::getRobotPose");
// tf3::toMsg(tf3::Transform::getIdentity(), global_pose.pose);
geometry_msgs::PoseStamped robot_pose;
// tf3::toMsg(tf3::Transform::getIdentity(), robot_pose.pose);
robot_pose.header.frame_id = robot_base_frame_;
robot_pose.header.stamp = robot::Time();
robot::Time current_time = robot::Time::now(); // save time for checking tf delay later
// 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_);
// }
}
catch (tf3::LookupException& ex)
{
printf("Cost Map No Transform available Error looking up robot pose: %s\n", ex.what());
return false;
}
catch (tf3::ConnectivityException& ex)
{
printf("Connectivity Error looking up robot pose: %s\n", ex.what());
return false;
}
catch (tf3::ExtrapolationException& ex)
{
printf("Extrapolation Error looking up robot pose: %s\n", ex.what());
return false;
}
// ROS_INFO_THROTTLE(1.0, "Time Delay %f , p %f %f", current_time.toSec() - global_pose.header.stamp.toSec(), global_pose.pose.position.x, global_pose.pose.position.y);
// check global_pose timeout
if (!global_pose.header.stamp.isZero() && current_time.toSec() - global_pose.header.stamp.toSec() > transform_tolerance_)
{
printf(
"Costmap2DROBOT transform timeout. Current time: %.4f, global_pose stamp: %.4f, tolerance: %.4f\n",
current_time.toSec(), global_pose.header.stamp.toSec(), transform_tolerance_);
return false;
}
return true;
}
void Costmap2DROBOT::getOrientedFootprint(std::vector<geometry_msgs::Point>& oriented_footprint) const
{
// if(name_.find("global_costmap")) ROS_WARN("Costmap2DROBOT::getOrientedFootprint");
geometry_msgs::PoseStamped global_pose;
if (!getRobotPose(global_pose))
return;
double yaw = getYaw(global_pose.pose.orientation);
transformFootprint(global_pose.pose.position.x, global_pose.pose.position.y, yaw,
padded_footprint_, oriented_footprint);
}
} // namespace costmap_2d

2
src/layer.cpp
View File

@ -13,7 +13,7 @@ Layer::Layer()
, tf_(NULL) , tf_(NULL)
{} {}
void Layer::initialize(LayeredCostmap* parent, std::string name, tf2::BufferCore *tf) void Layer::initialize(LayeredCostmap* parent, std::string name, tf3::BufferCore *tf)
{ {
layered_costmap_ = parent; layered_costmap_ = parent;
name_ = name; name_ = name;

18
src/observation_buffer.cpp
View File

@ -2,7 +2,7 @@
#include <costmap_2d/data_convert.h> #include <costmap_2d/data_convert.h>
// #include <tf2_geometry_msgs/tf2_geometry_msgs.h> // #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
// #include <tf2_sensor_msgs/tf2_sensor_msgs.h> // #include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include<tf2/convert.h> #include<tf3/convert.h>
#include <sensor_msgs/point_cloud2_iterator.h> #include <sensor_msgs/point_cloud2_iterator.h>
#include <cstdint> #include <cstdint>
@ -10,13 +10,13 @@
using namespace std; using namespace std;
using namespace tf2; using namespace tf3;
namespace costmap_2d namespace costmap_2d
{ {
ObservationBuffer::ObservationBuffer(string topic_name, double observation_keep_time, double expected_update_rate, 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 min_obstacle_height, double max_obstacle_height, double obstacle_range,
double raytrace_range, tf2::BufferCore& tf2_buffer, string global_frame, double raytrace_range, tf3::BufferCore& tf2_buffer, string global_frame,
string sensor_frame, double tf_tolerance) : string sensor_frame, double tf_tolerance) :
tf2_buffer_(tf2_buffer), observation_keep_time_(observation_keep_time), expected_update_rate_(expected_update_rate), tf2_buffer_(tf2_buffer), observation_keep_time_(observation_keep_time), expected_update_rate_(expected_update_rate),
last_updated_(robot::Time::now()), last_updated_(robot::Time::now()),
@ -36,7 +36,7 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
std::string tf_error; std::string tf_error;
if (!tf2_buffer_.canTransform(new_global_frame, global_frame_, tf2::Time::now(), &tf_error)) if (!tf2_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(), 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()); global_frame_.c_str(), tf_tolerance_, tf_error.c_str());
@ -56,14 +56,14 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
origin.point = obs.origin_; origin.point = obs.origin_;
// we need to transform the origin of the observation to the new global frame // we need to transform the origin of the observation to the new global frame
tf2::doTransform(origin, origin, tf3::doTransform(origin, origin,
tf2_buffer_.lookupTransform(new_global_frame, tf2_buffer_.lookupTransform(new_global_frame,
origin.header.frame_id, origin.header.frame_id,
convertTime(origin.header.stamp))); convertTime(origin.header.stamp)));
obs.origin_ = origin.point; obs.origin_ = origin.point;
// we also need to transform the cloud of the observation to the new global frame // we also need to transform the cloud of the observation to the new global frame
tf2::doTransform(*(obs.cloud_), *(obs.cloud_), tf3::doTransform(*(obs.cloud_), *(obs.cloud_),
tf2_buffer_.lookupTransform(new_global_frame, tf2_buffer_.lookupTransform(new_global_frame,
obs.cloud_->header.frame_id, obs.cloud_->header.frame_id,
convertTime(obs.cloud_->header.stamp))); convertTime(obs.cloud_->header.stamp)));
@ -100,11 +100,11 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
local_origin.point.x = 0; local_origin.point.x = 0;
local_origin.point.y = 0; local_origin.point.y = 0;
local_origin.point.z = 0; local_origin.point.z = 0;
tf2::doTransform(local_origin, global_origin, tf3::doTransform(local_origin, global_origin,
tf2_buffer_.lookupTransform(global_frame_, tf2_buffer_.lookupTransform(global_frame_,
local_origin.header.frame_id, local_origin.header.frame_id,
convertTime(local_origin.header.stamp))); convertTime(local_origin.header.stamp)));
tf2::convert(global_origin.point, observation_list_.front().origin_); tf3::convert(global_origin.point, observation_list_.front().origin_);
// make sure to pass on the raytrace/obstacle range of the observation buffer to the observations // make sure to pass on the raytrace/obstacle range of the observation buffer to the observations
observation_list_.front().raytrace_range_ = raytrace_range_; observation_list_.front().raytrace_range_ = raytrace_range_;
@ -113,7 +113,7 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
sensor_msgs::PointCloud2 global_frame_cloud; sensor_msgs::PointCloud2 global_frame_cloud;
// transform the point cloud // transform the point cloud
tf2::doTransform(cloud, global_frame_cloud, tf3::doTransform(cloud, global_frame_cloud,
tf2_buffer_.lookupTransform(global_frame_, tf2_buffer_.lookupTransform(global_frame_,
(cloud.header.frame_id), (cloud.header.frame_id),
convertTime(cloud.header.stamp))); convertTime(cloud.header.stamp)));

18
test/static_layer_test.cpp
View File

@ -2,8 +2,8 @@
#include <iostream> #include <iostream>
#include <costmap_2d/costmap_layer.h> #include <costmap_2d/costmap_layer.h>
#include <costmap_2d/layered_costmap.h> #include <costmap_2d/layered_costmap.h>
#include <tf2/buffer_core.h> #include <tf3/buffer_core.h>
#include <tf2/time.h> #include <tf3/time.h>
#include "nav_msgs/OccupancyGrid.h" #include "nav_msgs/OccupancyGrid.h"
#include <map_msgs/OccupancyGridUpdate.h> #include <map_msgs/OccupancyGridUpdate.h>
#include <costmap_2d/obstacle_layer.h> #include <costmap_2d/obstacle_layer.h>
@ -35,7 +35,7 @@ int main(int argc, char* argv[]) {
<< ", resolution: " << layered_costmap.getCostmap()->getResolution() << ", resolution: " << layered_costmap.getCostmap()->getResolution()
<< "m/cell" << std::endl; << "m/cell" << std::endl;
tf2::BufferCore tf_buffer; tf3::BufferCore tf_buffer;
// // ✅ FIX 2: Lưu tất cả plugins để tránh bị destroy // // ✅ FIX 2: Lưu tất cả plugins để tránh bị destroy
// std::vector<costmap_2d::PluginLayerPtr> plugin_instances; // std::vector<costmap_2d::PluginLayerPtr> plugin_instances;
@ -131,7 +131,7 @@ int main(int argc, char* argv[]) {
// // << " m/cell" << std::endl; // // << " m/cell" << std::endl;
// // // 2️⃣ TF buffer // // // 2️⃣ TF buffer
// // tf2::BufferCore tf_buffer; // // tf3::BufferCore tf_buffer;
// // // 3️⃣ Thiết lập footprint robot trước khi load inflation_layer // // // 3️⃣ Thiết lập footprint robot trước khi load inflation_layer
// // std::vector<geometry_msgs::Point> footprint = { // // std::vector<geometry_msgs::Point> footprint = {
@ -287,8 +287,8 @@ int main(int argc, char* argv[]) {
// #include <iostream> // #include <iostream>
// #include <costmap_2d/costmap_layer.h> // #include <costmap_2d/costmap_layer.h>
// #include <costmap_2d/layered_costmap.h> // #include <costmap_2d/layered_costmap.h>
// #include <tf2/buffer_core.h> // #include <tf3/buffer_core.h>
// #include <tf2/time.h> // #include <tf3/time.h>
// #include "nav_msgs/OccupancyGrid.h" // #include "nav_msgs/OccupancyGrid.h"
// #include <map_msgs/OccupancyGridUpdate.h> // #include <map_msgs/OccupancyGridUpdate.h>
// #include <costmap_2d/obstacle_layer.h> // #include <costmap_2d/obstacle_layer.h>
@ -311,9 +311,9 @@ int main(int argc, char* argv[]) {
// bool track_unknown = true; // bool track_unknown = true;
// LayeredCostmap layered_costmap(global_frame, rolling_window, track_unknown); // LayeredCostmap layered_costmap(global_frame, rolling_window, track_unknown);
// tf2::BufferCore tf_buffer; // tf3::BufferCore tf_buffer;
// // tf2::Duration cache_time(10.0); // // tf3::Duration cache_time(10.0);
// // tf2::BufferCore tf_buffer(cache_time); // // tf3::BufferCore tf_buffer(cache_time);
// plugin->initialize(&layered_costmap, "static_layer", &tf_buffer); // plugin->initialize(&layered_costmap, "static_layer", &tf_buffer);