diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 0671c79d90abe96dc7d82a479d108b8f0cf2fb8c..fe42cd49086dedf99160ac1a62c9ba420c3244e7 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -406,7 +406,7 @@ AmclNode::checkLaserReceived() RCLCPP_WARN( get_logger(), "Laser scan has not been received" " (and thus no pose updates have been published)." - " Verify that data is being published on the %s topic.", scan_topic_); + " Verify that data is being published on the %s topic.", scan_topic_.c_str()); return; } @@ -415,7 +415,7 @@ AmclNode::checkLaserReceived() RCLCPP_WARN( get_logger(), "No laser scan received (and thus no pose updates have been published) for %f" " seconds. Verify that data is being published on the %s topic.", - d.nanoseconds() * 1e-9, scan_topic_); + d.nanoseconds() * 1e-9, scan_topic_.c_str()); } } diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp index eed3626601eb9306dd057f1f02dc7a6423cf13ee..77aa8836d4490274449f6879b34876e79b8dcdfd 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp @@ -36,11 +36,11 @@ public: const BT::NodeConfiguration & conf) : BT::ActionNodeBase(xml_tag_name, conf), action_name_(action_name) { - node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node"); + node_ = config().blackboard->template get<rclcpp::Node::SharedPtr>("node"); // Get the required items from the blackboard server_timeout_ = - config().blackboard->get<std::chrono::milliseconds>("server_timeout"); + config().blackboard->template get<std::chrono::milliseconds>("server_timeout"); getInput<std::chrono::milliseconds>("server_timeout", server_timeout_); // Initialize the input and output messages @@ -245,9 +245,9 @@ protected: void increment_recovery_count() { int recovery_count = 0; - config().blackboard->get<int>("number_recoveries", recovery_count); // NOLINT + config().blackboard->template get<int>("number_recoveries", recovery_count); // NOLINT recovery_count += 1; - config().blackboard->set<int>("number_recoveries", recovery_count); // NOLINT + config().blackboard->template set<int>("number_recoveries", recovery_count); // NOLINT } std::string action_name_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp index 2b7b16bfb9c114c64a732323bfc7253c86bff09b..8931783df6cf532bf01eaccc72fc23511e688ca0 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp @@ -35,11 +35,11 @@ public: const BT::NodeConfiguration & conf) : BT::SyncActionNode(service_node_name, conf), service_node_name_(service_node_name) { - node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node"); + node_ = config().blackboard->template get<rclcpp::Node::SharedPtr>("node"); // Get the required items from the blackboard server_timeout_ = - config().blackboard->get<std::chrono::milliseconds>("server_timeout"); + config().blackboard->template get<std::chrono::milliseconds>("server_timeout"); getInput<std::chrono::milliseconds>("server_timeout", server_timeout_); // Now that we have node_ to use, create the service client for this BT service @@ -126,9 +126,9 @@ protected: void increment_recovery_count() { int recovery_count = 0; - config().blackboard->get<int>("number_recoveries", recovery_count); // NOLINT + config().blackboard->template get<int>("number_recoveries", recovery_count); // NOLINT recovery_count += 1; - config().blackboard->set<int>("number_recoveries", recovery_count); // NOLINT + config().blackboard->template set<int>("number_recoveries", recovery_count); // NOLINT } std::string service_name_, service_node_name_; diff --git a/nav2_bt_navigator/src/ros_topic_logger.cpp b/nav2_bt_navigator/src/ros_topic_logger.cpp index 5a11724db5d1f45ed1a72610aaff606f5e3ade22..e85bae5fa81c31c2790b6ba201c0f86febbba5d0 100644 --- a/nav2_bt_navigator/src/ros_topic_logger.cpp +++ b/nav2_bt_navigator/src/ros_topic_logger.cpp @@ -42,12 +42,7 @@ void RosTopicLogger::callback( // BT timestamps are a duration since the epoch. Need to convert to a time_point // before converting to a msg. -#ifndef _WIN32 - event.timestamp = - tf2_ros::toMsg(std::chrono::time_point<std::chrono::high_resolution_clock>(timestamp)); -#else - event.timestamp = tf2_ros::toMsg(timestamp); -#endif + event.timestamp = tf2_ros::toMsg(tf2::TimePoint(timestamp)); event.node_name = node.name(); event.previous_status = toStr(prev_status, false); event.current_status = toStr(status, false); diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/observation.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/observation.hpp index dbbfb2caa930b5ad8d9603b535e52988c27def63..e3195020780a1f985e55bc25156e05be7783c131 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/observation.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/observation.hpp @@ -59,6 +59,11 @@ public: delete cloud_; } + /** + * @brief Explicitly define copy assignment operator for Observation as it has a user-declared destructor + */ + Observation & operator=(const Observation &) = default; + /** * @brief Creates an observation from an origin point and a point cloud * @param origin The origin point of the observation diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.hpp index ba619e14b138ff6f256c523f7f1e096dc81ffc03..715231668d4ea1a2c54f6cd834510f57a94bb049 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.hpp @@ -49,6 +49,7 @@ #include "nav2_costmap_2d/observation.hpp" #include "nav2_util/lifecycle_node.hpp" + namespace nav2_costmap_2d { /** @@ -80,7 +81,7 @@ public: double min_obstacle_height, double max_obstacle_height, double obstacle_range, double raytrace_range, tf2_ros::Buffer & tf2_buffer, std::string global_frame, std::string sensor_frame, - double tf_tolerance); + tf2::Duration tf_tolerance); /** * @brief Destructor... cleans up @@ -146,7 +147,7 @@ private: double min_obstacle_height_, max_obstacle_height_; std::recursive_mutex lock_; ///< @brief A lock for accessing data in callbacks safely double obstacle_range_, raytrace_range_; - double tf_tolerance_; + tf2::Duration tf_tolerance_; }; } // namespace nav2_costmap_2d #endif // NAV2_COSTMAP_2D__OBSERVATION_BUFFER_HPP_ diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index c25097b30487e547c91b743d8aaf02f3c9924ae0..d456be34700ec012efa4c7c029b7d72fc6dd1aa5 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -179,7 +179,7 @@ void ObstacleLayer::onInitialize() node, topic, observation_keep_time, expected_update_rate, min_obstacle_height, max_obstacle_height, obstacle_range, raytrace_range, *tf_, global_frame_, - sensor_frame, transform_tolerance))); + sensor_frame, tf2::durationFromSec(transform_tolerance)))); // check if we'll add this buffer to our marking observation buffers if (marking) { diff --git a/nav2_costmap_2d/src/observation_buffer.cpp b/nav2_costmap_2d/src/observation_buffer.cpp index 8ac5b3958b8e0760b008215924a7239044f76a05..85708043b160be439d6246f4fedef0ec513f0674 100644 --- a/nav2_costmap_2d/src/observation_buffer.cpp +++ b/nav2_costmap_2d/src/observation_buffer.cpp @@ -55,7 +55,7 @@ ObservationBuffer::ObservationBuffer( double expected_update_rate, double min_obstacle_height, double max_obstacle_height, double obstacle_range, double raytrace_range, tf2_ros::Buffer & tf2_buffer, std::string global_frame, - std::string sensor_frame, double tf_tolerance) + std::string sensor_frame, tf2::Duration tf_tolerance) : tf2_buffer_(tf2_buffer), observation_keep_time_(rclcpp::Duration::from_seconds(observation_keep_time)), expected_update_rate_(rclcpp::Duration::from_seconds(expected_update_rate)), @@ -95,7 +95,7 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::msg::PointCloud2 & cloud) local_origin.point.x = 0; local_origin.point.y = 0; local_origin.point.z = 0; - tf2_buffer_.transform(local_origin, global_origin, global_frame_); + tf2_buffer_.transform(local_origin, global_origin, global_frame_, tf_tolerance_); tf2::convert(global_origin.point, observation_list_.front().origin_); // make sure to pass on the raytrace/obstacle range @@ -106,7 +106,7 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::msg::PointCloud2 & cloud) sensor_msgs::msg::PointCloud2 global_frame_cloud; // transform the point cloud - tf2_buffer_.transform(cloud, global_frame_cloud, global_frame_); + tf2_buffer_.transform(cloud, global_frame_cloud, global_frame_, tf_tolerance_); global_frame_cloud.header.stamp = cloud.header.stamp; // now we need to remove observations from the cloud that are below diff --git a/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt b/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt index e475d72fa863722498446e03de1433a42462a494..9d45a06ab828cf05beabf098b76b74f41000ff18 100644 --- a/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt +++ b/nav2_dwb_controller/nav_2d_utils/CMakeLists.txt @@ -25,6 +25,7 @@ set(dependencies tf2_geometry_msgs nav2_msgs nav2_util + nav_2d_msgs ) include_directories( @@ -53,6 +54,8 @@ ament_target_dependencies(tf_help ${dependencies} ) +target_link_libraries(tf_help conversions) + install(TARGETS conversions path_ops tf_help ARCHIVE DESTINATION lib LIBRARY DESTINATION lib diff --git a/nav2_recoveries/include/nav2_recoveries/recovery.hpp b/nav2_recoveries/include/nav2_recoveries/recovery.hpp index 829fc44385a798dd821aca4bcec4fb35a3ec8e26..edf7dbff0ff56cedc000bb5019e94117f2796c17 100644 --- a/nav2_recoveries/include/nav2_recoveries/recovery.hpp +++ b/nav2_recoveries/include/nav2_recoveries/recovery.hpp @@ -111,7 +111,7 @@ public: collision_checker_ = collision_checker; - vel_pub_ = node->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 1); + vel_pub_ = node->template create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 1); onConfigure(); } diff --git a/nav2_util/src/dump_params.cpp b/nav2_util/src/dump_params.cpp index a29a21c37b0da8aa12d71705f33a350b64fc66e6..1908d65eadaa4662e8b937fed5831507128d1ec4 100644 --- a/nav2_util/src/dump_params.cpp +++ b/nav2_util/src/dump_params.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include <libgen.h> + #include <iomanip> #include <iostream> #include <memory> diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp index 7d264b7181ea2d8a49e9f6c93a6d840fd0440906..fc4aee5c5e387dccaeafad25958abb92c00d952c 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/plugins/photo_at_waypoint.hpp @@ -15,6 +15,13 @@ #ifndef NAV2_WAYPOINT_FOLLOWER__PLUGINS__PHOTO_AT_WAYPOINT_HPP_ #define NAV2_WAYPOINT_FOLLOWER__PLUGINS__PHOTO_AT_WAYPOINT_HPP_ +/** + * While C++17 isn't the project standard. We have to force LLVM/CLang + * to ignore deprecated declarations + */ +#define _LIBCPP_NO_EXPERIMENTAL_DEPRECATION_WARNING_FILESYSTEM + + #include <experimental/filesystem> #include <mutex> #include <string> diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml index 0e914f0188b71cdc50d5870e896db1b778575fc5..f4e28a6357a18c48c59de4e3a9ae2f80ab466d78 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -7,10 +7,12 @@ <maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer> <license>Apache-2.0</license> - <depend>tf2_ros</depend> - <buildtool_depend>ament_cmake</buildtool_depend> + <depend>nav2_common</depend> + <depend>cv_bridge</depend> + <depend>pluginlib</depend> + <depend>image_transport</depend> <depend>rclcpp</depend> <depend>rclcpp_action</depend> <depend>rclcpp_lifecycle</depend> @@ -18,6 +20,7 @@ <depend>nav2_msgs</depend> <depend>nav2_util</depend> <depend>nav2_core</depend> + <depend>tf2_ros</depend> <test_depend>ament_lint_common</test_depend> <test_depend>ament_lint_auto</test_depend> diff --git a/smac_planner/src/a_star.cpp b/smac_planner/src/a_star.cpp index e3e3190bffaa3ed3fb2e32c0f605197f8a30d6e3..fae4694f75813ffa16ff86e8f70a2bedee09ce26 100644 --- a/smac_planner/src/a_star.cpp +++ b/smac_planner/src/a_star.cpp @@ -352,7 +352,7 @@ AStarAlgorithm<NodeSE2>::NodePtr AStarAlgorithm<NodeSE2>::getAnalyticPath( float d = node->motion_table.state_space->distance(from(), to()); NodePtr prev(node); // A move of sqrt(2) is guaranteed to be in a new cell - constexpr float sqrt_2 = std::sqrt(2.); + static const float sqrt_2 = std::sqrt(2.); unsigned int num_intervals = std::floor(d / sqrt_2); using PossibleNode = std::pair<NodePtr, Coordinates>;