diff --git a/CMakeLists.txt b/CMakeLists.txt index cda8307..cb7f620 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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) project(costmap_2d) @@ -132,7 +17,7 @@ find_package(Eigen3 REQUIRED) find_package(Boost REQUIRED COMPONENTS system thread filesystem) find_package(GTest REQUIRED) 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 --- # if (NOT TARGET sensor_msgs) @@ -160,7 +45,6 @@ include_directories( ${Boost_INCLUDE_DIRS} ${GTEST_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} - /usr/include ) link_directories(${PCL_LIBRARY_DIRS}) @@ -187,7 +71,8 @@ target_link_libraries(costmap_2d_new map_msgs laser_geometry voxel_grid - ${TF2_LIBRARY} + # ${TF2_LIBRARY} + tf3 ) target_include_directories(costmap_2d_new PRIVATE ${Boost_INCLUDE_DIRS}) @@ -268,6 +153,15 @@ target_link_libraries(unpreferred_layer 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 --- add_executable(test_array_parser test/array_parser_test.cpp) add_executable(test_costmap test/coordinates_test.cpp) diff --git a/include/costmap_2d/costmap_2d_robot.h b/include/costmap_2d/costmap_2d_robot.h index bfd1cb8..7100c74 100644 --- a/include/costmap_2d/costmap_2d_robot.h +++ b/include/costmap_2d/costmap_2d_robot.h @@ -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 #include -#include -#include +// #include +// #include #include #include #include #include -#include +// #include // #include -#include +#include +#include +#include // 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 ¶m_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 ¶m_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 plugin_loader_; - Costmap2DPublisher* publisher_; - dynamic_reconfigure::Server *dsrv_; + robot::Time last_publish_; + robot::Duration publish_cycle; + // pluginlib::ClassLoader plugin_loader_; + // Costmap2DPublisher* publisher_; + // dynamic_reconfigure::Server *dsrv_; boost::recursive_mutex configuration_mutex_; - ros::Subscriber footprint_sub_; - ros::Publisher footprint_pub_; + // ros::Subscriber footprint_sub_; + // ros::Publisher footprint_pub_; std::vector unpadded_footprint_; std::vector 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 diff --git a/include/costmap_2d/data_convert.h b/include/costmap_2d/data_convert.h index c9cfcae..617844a 100644 --- a/include/costmap_2d/data_convert.h +++ b/include/costmap_2d/data_convert.h @@ -5,8 +5,8 @@ #include #include #include -#include -#include +#include +#include #include #include @@ -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; diff --git a/include/costmap_2d/layer.h b/include/costmap_2d/layer.h index 35b6c81..d023fde 100644 --- a/include/costmap_2d/layer.h +++ b/include/costmap_2d/layer.h @@ -40,7 +40,7 @@ #include #include #include -#include +#include #include #include @@ -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 footprint_spec_; diff --git a/include/costmap_2d/observation_buffer.h b/include/costmap_2d/observation_buffer.h index 0572103..5036b0a 100644 --- a/include/costmap_2d/observation_buffer.h +++ b/include/costmap_2d/observation_buffer.h @@ -9,7 +9,7 @@ #include #include #include -#include +#include #include #include #include @@ -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_; diff --git a/plugins/directional_layer.cpp b/plugins/directional_layer.cpp index 2e22c71..c3852a4 100644 --- a/plugins/directional_layer.cpp +++ b/plugins/directional_layer.cpp @@ -1,7 +1,7 @@ #include #include -#include -#include +#include +#include #include @@ -134,8 +134,8 @@ namespace costmap_2d return false; } // Convert to yaw - tf2::Quaternion quaternion = convertQuaternion(p.pose.orientation); - double theta = tf2::getYaw(quaternion); + tf3::Quaternion quaternion = convertQuaternion(p.pose.orientation); + double theta = tf3::getYaw(quaternion); unsigned char value = laneFilter(mx, my, theta); if (get_success) { @@ -399,27 +399,27 @@ namespace costmap_2d bool DirectionalLayer::getRobotPose(double &x, double &y, double &yaw) { - tf2::TransformStampedMsg transformMsg; + tf3::TransformStampedMsg transformMsg; 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()); return false; } // Copy map data given proper transformations - tf2::Transform tf2_transform; + tf3::Transform tf2_transform; tf2_transform = convertToTf2Transform(transformMsg.transform); x = tf2_transform.getOrigin().x(); y = tf2_transform.getOrigin().y(); // 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) - yaw = tf2::getYaw(rotation); + yaw = tf3::getYaw(rotation); return true; } diff --git a/plugins/obstacle_layer.cpp b/plugins/obstacle_layer.cpp index cf72446..b88704f 100644 --- a/plugins/obstacle_layer.cpp +++ b/plugins/obstacle_layer.cpp @@ -2,8 +2,8 @@ #include #include #include -#include -#include +#include +#include #include 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_); } - 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(), ex.what()); @@ -197,7 +197,7 @@ void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScan& raw_ { 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(), ex.what()); diff --git a/plugins/static_layer.cpp b/plugins/static_layer.cpp index fbc1ffa..5842e83 100644 --- a/plugins/static_layer.cpp +++ b/plugins/static_layer.cpp @@ -3,8 +3,8 @@ #include #include #include -#include -#include +#include +#include #include @@ -283,25 +283,25 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int double wx, wy; // Might even be in a different frame // geometry_msgs::TransformStamped transform; - tf2::TransformStampedMsg transformMsg; + tf3::TransformStampedMsg transformMsg; try { // transformMsg = tf_->lookupTransform(map_frame_, global_frame_, ros::Time(0)); - bool status =tf_->canTransform(map_frame_, global_frame_, tf2::Time()); - if(!status) throw tf2::TransformException("[static_layer] Cannot transform"); + bool status =tf_->canTransform(map_frame_, global_frame_, tf3::Time()); + if(!status) throw tf3::TransformException("[static_layer] Cannot transform"); transformMsg = tf_->lookupTransform(map_frame_, global_frame_, - tf2::Time()); + tf3::Time()); } - catch (tf2::TransformException ex) + catch (tf3::TransformException ex) { printf("%s", ex.what()); return; } // Copy map data given proper transformations - tf2::Transform tf2_transform; + tf3::Transform tf2_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 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 layered_costmap_->getCostmap()->mapToWorld(i, j, wx, wy); // Transform from global_frame_ to map_frame_ - tf2::Vector3 p(wx, wy, 0); + tf3::Vector3 p(wx, wy, 0); p = tf2_transform*p; // Set master_grid with cell from map if (worldToMap(p.x(), p.y(), mx, my)) diff --git a/src/costmap_2d_robot.cpp b/src/costmap_2d_robot.cpp new file mode 100644 index 0000000..a50d36a --- /dev/null +++ b/src/costmap_2d_robot.cpp @@ -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 +#include +#include +#include +#include +#include +#include +#include +// #include + +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(my_list[i]["name"]); +// std::string type = static_cast(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 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(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(ros::NodeHandle("~/" + name)); +// dynamic_reconfigure::Server::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 ¶m_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 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& 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::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 *plugins = layered_costmap_->getPlugins(); + // unsubscribe from topics + for (vector::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 *plugins = layered_costmap_->getPlugins(); + for (vector::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& 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 diff --git a/src/layer.cpp b/src/layer.cpp index 06e2c0d..93a7cc7 100644 --- a/src/layer.cpp +++ b/src/layer.cpp @@ -13,7 +13,7 @@ Layer::Layer() , 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; name_ = name; diff --git a/src/observation_buffer.cpp b/src/observation_buffer.cpp index 87b8d38..8794e98 100644 --- a/src/observation_buffer.cpp +++ b/src/observation_buffer.cpp @@ -2,7 +2,7 @@ #include // #include // #include -#include +#include #include #include @@ -10,13 +10,13 @@ using namespace std; -using namespace tf2; +using namespace tf3; 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, tf2::BufferCore& tf2_buffer, string global_frame, + double raytrace_range, tf3::BufferCore& tf2_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), last_updated_(robot::Time::now()), @@ -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_, 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(), 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_; // 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, 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 - tf2::doTransform(*(obs.cloud_), *(obs.cloud_), + tf3::doTransform(*(obs.cloud_), *(obs.cloud_), tf2_buffer_.lookupTransform(new_global_frame, obs.cloud_->header.frame_id, 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.y = 0; local_origin.point.z = 0; - tf2::doTransform(local_origin, global_origin, + tf3::doTransform(local_origin, global_origin, tf2_buffer_.lookupTransform(global_frame_, local_origin.header.frame_id, 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 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; // transform the point cloud - tf2::doTransform(cloud, global_frame_cloud, + tf3::doTransform(cloud, global_frame_cloud, tf2_buffer_.lookupTransform(global_frame_, (cloud.header.frame_id), convertTime(cloud.header.stamp))); diff --git a/test/static_layer_test.cpp b/test/static_layer_test.cpp index e3c9a8b..3c9513a 100644 --- a/test/static_layer_test.cpp +++ b/test/static_layer_test.cpp @@ -2,8 +2,8 @@ #include #include #include -#include -#include +#include +#include #include "nav_msgs/OccupancyGrid.h" #include #include @@ -35,7 +35,7 @@ int main(int argc, char* argv[]) { << ", resolution: " << layered_costmap.getCostmap()->getResolution() << "m/cell" << std::endl; - tf2::BufferCore tf_buffer; + tf3::BufferCore tf_buffer; // // ✅ FIX 2: Lưu tất cả plugins để tránh bị destroy // std::vector plugin_instances; @@ -131,7 +131,7 @@ int main(int argc, char* argv[]) { // // << " m/cell" << std::endl; // // // 2️⃣ TF buffer - // // tf2::BufferCore tf_buffer; + // // tf3::BufferCore tf_buffer; // // // 3️⃣ Thiết lập footprint robot trước khi load inflation_layer // // std::vector footprint = { @@ -287,8 +287,8 @@ int main(int argc, char* argv[]) { // #include // #include // #include -// #include -// #include +// #include +// #include // #include "nav_msgs/OccupancyGrid.h" // #include // #include @@ -311,9 +311,9 @@ int main(int argc, char* argv[]) { // bool track_unknown = true; // LayeredCostmap layered_costmap(global_frame, rolling_window, track_unknown); -// tf2::BufferCore tf_buffer; -// // tf2::Duration cache_time(10.0); -// // tf2::BufferCore tf_buffer(cache_time); +// tf3::BufferCore tf_buffer; +// // tf3::Duration cache_time(10.0); +// // tf3::BufferCore tf_buffer(cache_time); // plugin->initialize(&layered_costmap, "static_layer", &tf_buffer);