Remove pose2d

include/slam_toolbox/slam_toolbox_common.hpp

@@ -128,7 +128,7 @@ class SlamToolbox : public rclcpp_lifecycle::LifecycleNode
     const sensor_msgs::msg::LaserScan::ConstSharedPtr & scan,
     karto::Pose2 & karto_pose);
   bool shouldStartWithPoseGraph(
-    std::string & filename, geometry_msgs::msg::Pose2D & pose,
+    std::string & filename, geometry_msgs::msg::Pose & pose,
     bool & start_at_dock);
   bool shouldProcessScan(
     const sensor_msgs::msg::LaserScan::ConstSharedPtr & scan,

rviz_plugin/slam_toolbox_rviz_plugin.cpp

@@ -21,6 +21,7 @@
 // ROS
 #include <tf2_ros/transform_listener.h>
 #include <tf2/convert.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 // QT
 #include <QPushButton>
 #include <QCheckBox>
@@ -307,9 +308,12 @@ void SlamToolboxPlugin::DeserializeMap()
     try
     {
       request->match_type = procType::START_AT_GIVEN_POSE;
-      request->initial_pose.x = std::stod(_line5->text().toStdString());
-      request->initial_pose.y = std::stod(_line6->text().toStdString());
-      request->initial_pose.theta = std::stod(_line7->text().toStdString());
+      request->initial_pose.position.x = std::stod(_line5->text().toStdString());
+      request->initial_pose.position.y = std::stod(_line6->text().toStdString());
+      request->initial_pose.position.z = 0.0;
+      tf2::Quaternion q;
+      q.setRPY(0.0, 0.0, std::stod(_line7->text().toStdString()));
+      request->initial_pose.orientation = tf2::toMsg(q);
     }
     catch (const std::invalid_argument& ia)
     {
@@ -320,9 +324,12 @@ void SlamToolboxPlugin::DeserializeMap()
     try
     {
       request->match_type = procType::LOCALIZE_AT_POSE;
-      request->initial_pose.x = std::stod(_line5->text().toStdString());
-      request->initial_pose.y = std::stod(_line6->text().toStdString());
-      request->initial_pose.theta = std::stod(_line7->text().toStdString());
+      request->initial_pose.position.x = std::stod(_line5->text().toStdString());
+      request->initial_pose.position.y = std::stod(_line6->text().toStdString());
+      request->initial_pose.position.z = 0.0;
+      tf2::Quaternion q;
+      q.setRPY(0.0, 0.0, std::stod(_line7->text().toStdString()));
+      request->initial_pose.orientation = tf2::toMsg(q);
     }
     catch (const std::invalid_argument& ia)
     {

src/slam_toolbox_common.cpp

@@ -544,7 +544,7 @@ void SlamToolbox::loadPoseGraphByParams()
 /*****************************************************************************/
 {
   std::string filename;
-  geometry_msgs::msg::Pose2D pose;
+  geometry_msgs::msg::Pose pose;
   bool dock = false;
   if (shouldStartWithPoseGraph(filename, pose, dock)) {
     std::shared_ptr<slam_toolbox::srv::DeserializePoseGraph::Request> req =
@@ -568,7 +568,7 @@ void SlamToolbox::loadPoseGraphByParams()
 /*****************************************************************************/
 bool SlamToolbox::shouldStartWithPoseGraph(
   std::string & filename,
-  geometry_msgs::msg::Pose2D & pose, bool & start_at_dock)
+  geometry_msgs::msg::Pose & pose, bool & start_at_dock)
 /*****************************************************************************/
 {
   // if given a map to load at run time, do it.
@@ -593,13 +593,14 @@ bool SlamToolbox::shouldStartWithPoseGraph(
         RCLCPP_ERROR(get_logger(), "LocalizationSlamToolbox: Incorrect "
           "number of arguments for map starting pose. Must be in format: "
           "[x, y, theta]. Starting at the origin");
-        pose.x = 0.;
-        pose.y = 0.;
-        pose.theta = 0.;
+        pose = geometry_msgs::msg::Pose();
       } else {
-        pose.x = read_pose[0];
-        pose.y = read_pose[1];
-        pose.theta = read_pose[2];
+        pose.position.x = read_pose[0];
+        pose.position.y = read_pose[1];
+        pose.position.z = 0.0;
+        tf2::Quaternion q;
+        q.setRPY(0.0, 0.0, read_pose[2]);
+        pose.orientation = tf2::toMsg(q);
       }
     } else if (map_start_at_dock.get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET) {
       start_at_dock = map_start_at_dock.get<bool>();
@@ -1072,13 +1073,13 @@ bool SlamToolbox::deserializePoseGraphCallback(
       break;
     case procType::START_AT_GIVEN_POSE:
       processor_type_ = PROCESS_NEAR_REGION;
-      process_near_pose_ = std::make_unique<Pose2>(req->initial_pose.x,
-          req->initial_pose.y, req->initial_pose.theta);
+      process_near_pose_ = std::make_unique<Pose2>(req->initial_pose.position.x,
+          req->initial_pose.position.y, tf2::getYaw(req->initial_pose.orientation));
       break;
     case procType::LOCALIZE_AT_POSE:
       processor_type_ = PROCESS_LOCALIZATION;
-      process_near_pose_ = std::make_unique<Pose2>(req->initial_pose.x,
-          req->initial_pose.y, req->initial_pose.theta);
+      process_near_pose_ = std::make_unique<Pose2>(req->initial_pose.position.x,
+          req->initial_pose.position.y, tf2::getYaw(req->initial_pose.orientation));
       break;
     default:
       RCLCPP_FATAL(get_logger(),

src/slam_toolbox_localization.cpp

@@ -35,7 +35,7 @@ void LocalizationSlamToolbox::loadPoseGraphByParams()
 /*****************************************************************************/
 {
   std::string filename;
-  geometry_msgs::msg::Pose2D pose;
+  geometry_msgs::msg::Pose pose;
   bool dock = false;
   if (shouldStartWithPoseGraph(filename, pose, dock)) {
     std::shared_ptr<slam_toolbox::srv::DeserializePoseGraph::Request> req =

srv/DeserializePoseGraph.srv

@@ -8,5 +8,5 @@ int8 LOCALIZE_AT_POSE = 3
 
 string filename
 int8 match_type
-geometry_msgs/Pose2D initial_pose
+geometry_msgs/Pose initial_pose
 ---