fix initial pose and goal buttons (#1510)

* fix initial pose and goal buttons

* use less ambiguous tf2::toMsg instead of tf2::convert

* clang format

Co-authored-by: doisyg <guillaume.?doisy@wyca.fr>

src/rviz/default_plugin/tools/goal_tool.cpp

@@ -71,7 +71,7 @@ void GoalTool::onPoseSet(double x, double y, double theta)
   tf2::Quaternion quat;
   quat.setRPY(0.0, 0.0, theta);
   geometry_msgs::PoseStamped goal;
-  tf2::convert(goal.pose.orientation, quat);
+  goal.pose.orientation = tf2::toMsg(quat);
   goal.pose.position.x = x;
   goal.pose.position.y = y;
   goal.header.frame_id = fixed_frame;

src/rviz/default_plugin/tools/initial_pose_tool.cpp

@@ -29,6 +29,7 @@
 
 #include <geometry_msgs/PoseWithCovarianceStamped.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2/LinearMath/Quaternion.h>
 
 #include <rviz/display_context.h>
 #include <rviz/properties/string_property.h>
@@ -85,9 +86,11 @@ void InitialPoseTool::onPoseSet(double x, double y, double theta)
   pose.pose.pose.position.x = x;
   pose.pose.pose.position.y = y;
 
+
+  geometry_msgs::Quaternion quat_msg;
   tf2::Quaternion quat;
   quat.setRPY(0.0, 0.0, theta);
-  tf2::convert(pose.pose.pose.orientation, quat);
+  pose.pose.pose.orientation = tf2::toMsg(quat);
   pose.pose.covariance[6 * 0 + 0] = std::pow(std_dev_x_->getFloat(), 2);
   pose.pose.covariance[6 * 1 + 1] = std::pow(std_dev_y_->getFloat(), 2);
   pose.pose.covariance[6 * 5 + 5] = std::pow(std_dev_theta_->getFloat(), 2);

src/test/marker_test.cpp

@@ -712,17 +712,17 @@ void frameCallback(const ros::TimerEvent& /*unused*/)
   static tf2_ros::TransformBroadcaster br;
   geometry_msgs::TransformStamped t;
 
-  tf2::convert(tf2::Vector3(0.0, 0.0, (counter % 1000) * 0.01), t.transform.translation);
-  tf2::convert(tf2::Quaternion(0.0, 0.0, 0.0, 1.0), t.transform.rotation);
+  t.transform.translation = tf2::toMsg(tf2::Vector3(0.0, 0.0, (counter % 1000) * 0.01));
+  t.transform.rotation = tf2::toMsg(tf2::Quaternion(0.0, 0.0, 0.0, 1.0));
   t.header.frame_id = "base_link";
   t.child_frame_id = "my_link";
   t.header.stamp = ros::Time::now();
   br.sendTransform(t);
 
-  tf2::convert(tf2::Vector3(0.0, 0.0, 0.0), t.transform.translation);
+  t.transform.translation = tf2::toMsg(tf2::Vector3(0.0, 0.0, 0.0));
   tf2::Quaternion q;
   q.setRPY(M_PI * 0.25, M_PI * 0.25, 0.0);
-  tf2::convert(q, t.transform.rotation);
+  t.transform.rotation = tf2::toMsg(q);
   t.header.frame_id = "base_link";
   t.child_frame_id = "rotate_base_link";
   t.header.stamp = ros::Time::now();

src/test/rviz_logo_marker.cpp

@@ -26,7 +26,7 @@ void makeMarker()
   visualization_msgs::Marker marker;
   marker.type = visualization_msgs::Marker::MESH_RESOURCE;
 
-  tf2::convert(tf2::Quaternion(tf2::Vector3(0, 0, 1), 0.2), marker.pose.orientation);
+  marker.pose.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), 0.2));
 
   marker.pose.position.x = 0;
   marker.pose.position.y = -0.22;
@@ -84,10 +84,10 @@ void publishCallback(const ros::TimerEvent& /*unused*/)
 {
   static tf2_ros::TransformBroadcaster br;
   geometry_msgs::TransformStamped transform;
-  tf2::convert(tf2::Vector3(3, 1, 0), transform.transform.translation);
+  transform.transform.translation = tf2::toMsg(tf2::Vector3(3, 1, 0));
   tf2::Quaternion q;
   q.setRPY(0, 0, M_PI * 0.9);
-  tf2::convert(q, transform.transform.rotation);
+  transform.transform.rotation = tf2::toMsg(q);
   transform.header.frame_id = "base_link";
   transform.header.stamp = ros::Time::now();
   transform.child_frame_id = "rviz_logo";