Update updown test (rebased)

nav2_amcl/include/nav2_amcl/amcl_node.hpp

@@ -201,6 +201,11 @@ class AmclNode : public nav2_util::LifecycleNode
   void handleInitialPose(geometry_msgs::msg::PoseWithCovarianceStamped & msg);
   bool init_pose_received_on_inactive{false};
   bool initial_pose_is_known_{false};
+  bool set_initial_pose_{false};
+  double initial_pose_x_;
+  double initial_pose_y_;
+  double initial_pose_z_;
+  double initial_pose_yaw_;
 
   // Node parameters (initialized via initParameters)
   void initParameters();

nav2_amcl/src/amcl_node.cpp

@@ -30,6 +30,7 @@
 
 #include "message_filters/subscriber.h"
 #include "nav2_util/angleutils.hpp"
+#include "nav2_util/geometry_utils.hpp"
 #include "nav2_util/pf/pf.hpp"
 #include "nav2_util/string_utils.hpp"
 #include "nav2_util/sensors/laser/laser.hpp"
@@ -52,6 +53,7 @@ using namespace std::chrono_literals;
 
 namespace nav2_amcl
 {
+using nav2_util::geometry_utils::orientationAroundZAxis;
 
 AmclNode::AmclNode()
 : nav2_util::LifecycleNode("amcl", "", true)
@@ -100,7 +102,23 @@ AmclNode::AmclNode()
 
   add_parameter("laser_model_type", rclcpp::ParameterValue(std::string("likelihood_field")),
     "Which model to use, either beam, likelihood_field, or likelihood_field_prob",
-    "Ssame as likelihood_field but incorporates the beamskip feature, if enabled");
+    "Same as likelihood_field but incorporates the beamskip feature, if enabled");
+
+  add_parameter("set_initial_pose", rclcpp::ParameterValue(false),
+    "Causes AMCL to set initial pose from the initial_pose* parameters instead of "
+    "waiting for the initial_pose message");
+
+  add_parameter("initial_pose.x", rclcpp::ParameterValue(0.0),
+    "X coordinate of the initial robot pose in the map frame");
+
+  add_parameter("initial_pose.y", rclcpp::ParameterValue(0.0),
+    "Y coordinate of the initial robot pose in the map frame");
+
+  add_parameter("initial_pose.z", rclcpp::ParameterValue(0.0),
+    "Z coordinate of the initial robot pose in the map frame");
+
+  add_parameter("initial_pose.yaw", rclcpp::ParameterValue(0.0),
+    "Yaw of the initial robot pose in the map frame");
 
   add_parameter("max_beams", rclcpp::ParameterValue(60),
     "How many evenly-spaced beams in each scan to be used when updating the filter");
@@ -218,7 +236,18 @@ AmclNode::on_activate(const rclcpp_lifecycle::State & /*state*/)
   // process incoming callbacks until we are
   active_ = true;
 
-  if (init_pose_received_on_inactive) {
+  if (set_initial_pose_) {
+    auto msg = std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>();
+
+    msg->header.stamp = now();
+    msg->header.frame_id = global_frame_id_;
+    msg->pose.pose.position.x = initial_pose_x_;
+    msg->pose.pose.position.y = initial_pose_y_;
+    msg->pose.pose.position.z = initial_pose_z_;
+    msg->pose.pose.orientation = orientationAroundZAxis(initial_pose_yaw_);
+
+    initialPoseReceived(msg);
+  } else if (init_pose_received_on_inactive) {
     handleInitialPose(last_published_pose_);
   }
 
@@ -693,16 +722,13 @@ bool AmclNode::updateFilter(
   //
   // Construct min and max angles of laser, in the base_link frame.
   // Here we set the roll pich yaw of the lasers.  We assume roll and pich are zero.
-  tf2::Quaternion q;
-  q.setRPY(0.0, 0.0, laser_scan->angle_min);
   geometry_msgs::msg::QuaternionStamped min_q, inc_q;
   min_q.header.stamp = laser_scan->header.stamp;
   min_q.header.frame_id = nav2_util::strip_leading_slash(laser_scan->header.frame_id);
-  tf2::impl::Converter<false, true>::convert(q, min_q.quaternion);
+  min_q.quaternion = orientationAroundZAxis(laser_scan->angle_min);
 
-  q.setRPY(0.0, 0.0, laser_scan->angle_min + laser_scan->angle_increment);
   inc_q.header = min_q.header;
-  tf2::impl::Converter<false, true>::convert(q, inc_q.quaternion);
+  inc_q.quaternion = orientationAroundZAxis(laser_scan->angle_min + laser_scan->angle_increment);
   try {
     tf_buffer_->transform(min_q, min_q, base_frame_id_);
     tf_buffer_->transform(inc_q, inc_q, base_frame_id_);
@@ -767,9 +793,7 @@ AmclNode::publishParticleCloud(const pf_sample_set_t * set)
     cloud_msg.poses[i].position.x = set->samples[i].pose.v[0];
     cloud_msg.poses[i].position.y = set->samples[i].pose.v[1];
     cloud_msg.poses[i].position.z = 0;
-    tf2::Quaternion q;
-    q.setRPY(0, 0, set->samples[i].pose.v[2]);
-    tf2::impl::Converter<false, true>::convert(q, cloud_msg.poses[i].orientation);
+    cloud_msg.poses[i].orientation = orientationAroundZAxis(set->samples[i].pose.v[2]);
   }
   particlecloud_pub_->publish(cloud_msg);
 }
@@ -837,9 +861,7 @@ AmclNode::publishAmclPose(
   // Copy in the pose
   p.pose.pose.position.x = hyps[max_weight_hyp].pf_pose_mean.v[0];
   p.pose.pose.position.y = hyps[max_weight_hyp].pf_pose_mean.v[1];
-  tf2::Quaternion q;
-  q.setRPY(0, 0, hyps[max_weight_hyp].pf_pose_mean.v[2]);
-  tf2::impl::Converter<false, true>::convert(q, p.pose.pose.orientation);
+  p.pose.pose.orientation = orientationAroundZAxis(hyps[max_weight_hyp].pf_pose_mean.v[2]);
   // Copy in the covariance, converting from 3-D to 6-D
   pf_sample_set_t * set = pf_->sets + pf_->current_set;
   for (int i = 0; i < 2; i++) {
@@ -956,6 +978,11 @@ AmclNode::initParameters()
   get_parameter("laser_max_range", laser_max_range_);
   get_parameter("laser_min_range", laser_min_range_);
   get_parameter("laser_model_type", sensor_model_type_);
+  get_parameter("set_initial_pose", set_initial_pose_);
+  get_parameter("initial_pose.x", initial_pose_x_);
+  get_parameter("initial_pose.y", initial_pose_y_);
+  get_parameter("initial_pose.z", initial_pose_z_);
+  get_parameter("initial_pose.yaw", initial_pose_yaw_);
   get_parameter("max_beams", max_beams_);
   get_parameter("max_particles", max_particles_);
   get_parameter("min_particles", min_particles_);
@@ -1014,7 +1041,7 @@ AmclNode::handleMapMessage(const nav_msgs::msg::OccupancyGrid & msg)
 {
   std::lock_guard<std::recursive_mutex> cfl(configuration_mutex_);
 
-  RCLCPP_INFO(get_logger(), "Received a %d X %d map @ %.3f m/pix\n",
+  RCLCPP_INFO(get_logger(), "Received a %d X %d map @ %.3f m/pix",
     msg.info.width,
     msg.info.height,
     msg.info.resolution);

nav2_costmap_2d/src/costmap_2d_ros.cpp

@@ -232,8 +232,8 @@ Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
   delete layered_costmap_;
   layered_costmap_ = nullptr;
 
-  tf_buffer_.reset();
   tf_listener_.reset();
+  tf_buffer_.reset();
 
   footprint_sub_.reset();
   footprint_pub_.reset();

nav2_costmap_2d/test/integration/test_collision_checker.cpp

@@ -35,9 +35,11 @@
 #pragma GCC diagnostic ignored "-Wpedantic"
 #include "tf2/utils.h"
 #pragma GCC diagnostic pop
+#include "nav2_util/geometry_utils.hpp"
 
 using namespace std::chrono_literals;
 using namespace std::placeholders;
+using nav2_util::geometry_utils::orientationAroundZAxis;
 
 class RclCppFixture
 {
@@ -198,9 +200,7 @@ class TestCollisionChecker : public nav2_util::LifecycleNode
     current_pose_.pose.position.x = x_;
     current_pose_.pose.position.y = y_;
     current_pose_.pose.position.z = 0;
-    tf2::Quaternion q;
-    q.setRPY(0, 0, yaw_);
-    current_pose_.pose.orientation = tf2::toMsg(q);
+    current_pose_.pose.orientation = orientationAroundZAxis(yaw_);
   }
 
   void publishFootprint()

nav2_dwb_controller/nav_2d_utils/CMakeLists.txt

@@ -10,6 +10,7 @@ find_package(rclcpp REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(nav2_msgs REQUIRED)
+find_package(nav2_util REQUIRED)
 
 nav2_package()
 
@@ -21,6 +22,7 @@ set(dependencies
   tf2
   tf2_geometry_msgs
   nav2_msgs
+  nav2_util
 )
 
 include_directories(

nav2_dwb_controller/nav_2d_utils/package.xml

@@ -16,6 +16,7 @@
   <depend>tf2</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>nav2_msgs</depend>
+  <depend>nav2_util</depend>
 
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>

nav2_dwb_controller/nav_2d_utils/src/conversions.cpp

@@ -39,9 +39,11 @@
 #pragma GCC diagnostic ignored "-Wpedantic"
 #include "tf2/utils.h"
 #pragma GCC diagnostic pop
+#include "nav2_util/geometry_utils.hpp"
 
 namespace nav_2d_utils
 {
+using nav2_util::geometry_utils::orientationAroundZAxis;
 
 geometry_msgs::msg::Twist twist2Dto3D(const nav_2d_msgs::msg::Twist2D & cmd_vel_2d)
 {
@@ -87,9 +89,7 @@ geometry_msgs::msg::Pose pose2DToPose(const geometry_msgs::msg::Pose2D & pose2d)
   geometry_msgs::msg::Pose pose;
   pose.position.x = pose2d.x;
   pose.position.y = pose2d.y;
-  tf2::Quaternion q;
-  q.setRPY(0, 0, pose2d.theta);
-  pose.orientation = tf2::toMsg(q);
+  pose.orientation = orientationAroundZAxis(pose2d.theta);
   return pose;
 }
 
@@ -111,9 +111,7 @@ geometry_msgs::msg::PoseStamped pose2DToPoseStamped(
   pose.header.stamp = stamp;
   pose.pose.position.x = pose2d.x;
   pose.pose.position.y = pose2d.y;
-  tf2::Quaternion q;
-  q.setRPY(0, 0, pose2d.theta);
-  pose.pose.orientation = tf2::toMsg(q);
+  pose.pose.orientation = orientationAroundZAxis(pose2d.theta);
   return pose;
 }
 

nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp

@@ -63,9 +63,6 @@ class LifecycleManagerClient
 
   // Also, for convenience, this client supports invoking the NavigateToPose action
   rclcpp_action::Client<nav2_msgs::action::NavigateToPose>::SharedPtr navigate_action_client_;
-
-  // A convenience function to convert from an algle to a Quaternion
-  geometry_msgs::msg::Quaternion orientationAroundZAxis(double angle);
 };
 
 }  // namespace nav2_lifecycle_manager

nav2_lifecycle_manager/src/lifecycle_manager_client.cpp

@@ -18,9 +18,11 @@
 #include <memory>
 
 #include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+#include "nav2_util/geometry_utils.hpp"
 
 namespace nav2_lifecycle_manager
 {
+using nav2_util::geometry_utils::orientationAroundZAxis;
 
 LifecycleManagerClient::LifecycleManagerClient()
 {
@@ -54,14 +56,6 @@ LifecycleManagerClient::shutdown()
   callService(shutdown_client_, "lifecycle_manager/shutdown");
 }
 
-geometry_msgs::msg::Quaternion
-LifecycleManagerClient::orientationAroundZAxis(double angle)
-{
-  tf2::Quaternion q;
-  q.setRPY(0, 0, angle);  // void returning function
-  return tf2::toMsg(q);
-}
-
 void
 LifecycleManagerClient::set_initial_pose(double x, double y, double theta)
 {

nav2_map_server/src/occ_grid_loader.cpp

@@ -40,11 +40,14 @@
 #include "Magick++.h"
 #include "tf2/LinearMath/Quaternion.h"
 #include "yaml-cpp/yaml.h"
+#include "nav2_util/geometry_utils.hpp"
 
 using namespace std::chrono_literals;
 
 namespace nav2_map_server
 {
+using nav2_util::geometry_utils::orientationAroundZAxis;
+
 OccGridLoader::OccGridLoader(
   rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::string & yaml_filename)
 : node_(node), yaml_filename_(yaml_filename)
@@ -191,6 +194,7 @@ nav2_util::CallbackReturn OccGridLoader::on_deactivate(const rclcpp_lifecycle::S
   RCLCPP_INFO(node_->get_logger(), "OccGridLoader: Deactivating");
 
   occ_pub_->on_deactivate();
+  timer_.reset();
 
   return nav2_util::CallbackReturn::SUCCESS;
 }
@@ -220,12 +224,7 @@ void OccGridLoader::loadMapFromFile(const LoadParameters & loadParameters)
   msg.info.origin.position.x = loadParameters.origin[0];
   msg.info.origin.position.y = loadParameters.origin[1];
   msg.info.origin.position.z = 0.0;
-  tf2::Quaternion q;
-  q.setRPY(0, 0, loadParameters.origin[2]);
-  msg.info.origin.orientation.x = q.x();
-  msg.info.origin.orientation.y = q.y();
-  msg.info.origin.orientation.z = q.z();
-  msg.info.origin.orientation.w = q.w();
+  msg.info.origin.orientation = orientationAroundZAxis(loadParameters.origin[2]);
 
   // Allocate space to hold the data
   msg.data.resize(msg.info.width * msg.info.height);

nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp

@@ -28,6 +28,7 @@
 #include "rclcpp_action/rclcpp_action.hpp"
 #include "rviz_common/panel.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+#include "nav2_util/geometry_utils.hpp"
 
 class QPushButton;
 
@@ -60,8 +61,6 @@ private Q_SLOTS:
   void onCancelButtonPressed();
   void timerEvent(QTimerEvent * event) override;
 
-  geometry_msgs::msg::Quaternion orientationAroundZAxis(double angle);
-
   // Call to send NavigateToPose action request for goal pose
   void startNavigation(geometry_msgs::msg::PoseStamped pose);
   using GoalHandle = rclcpp_action::ClientGoalHandle<nav2_msgs::action::NavigateToPose>;

nav2_rviz_plugins/src/nav2_panel.cpp

@@ -26,6 +26,7 @@ using namespace std::chrono_literals;
 
 namespace nav2_rviz_plugins
 {
+using nav2_util::geometry_utils::orientationAroundZAxis;
 
 // Define global GoalPoseUpdater so that the nav2 GoalTool plugin can access to update goal pose
 GoalPoseUpdater GoalUpdater;
@@ -180,14 +181,6 @@ Nav2Panel::onNewGoal(double x, double y, double theta, QString frame)
   startNavigation(pose);
 }
 
-geometry_msgs::msg::Quaternion
-Nav2Panel::orientationAroundZAxis(double angle)
-{
-  tf2::Quaternion q;
-  q.setRPY(0, 0, angle);  // void returning function
-  return tf2::toMsg(q);
-}
-
 void
 Nav2Panel::onCancelButtonPressed()
 {