Change quaternion calculation method (#11)

* Update CMakeLists.txt

* Add param

* Change quaternion calculation method

* Update package.xml

CMakeLists.txt

@@ -8,6 +8,7 @@ find_package(catkin REQUIRED COMPONENTS
   nav_msgs
   roscpp
   tf2
+  tf2_geometry_msgs
 )
 
 ###################################

include/local_goal_creator/local_goal_creator.h

@@ -13,6 +13,7 @@
 #include <optional>
 #include <ros/ros.h>
 #include <tf2/LinearMath/Quaternion.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 
 /**
  * @class LocalGoalCreator

launch/test.launch

@@ -2,9 +2,10 @@
 <launch>
     <arg name="rviz_path" default="$(find local_goal_creator_ros)/launch/rviz.rviz"/>
     <arg name="map_path" default="$(find a_star_ros)/maps/map.yaml"/>
+    <arg name="use_direction_in_path" default="true"/>
 
     <include file="$(find local_goal_creator_ros)/launch/local_goal_creator.launch">
-        <arg name="use_direction_in_path" value="true"/>
+        <arg name="use_direction_in_path" value="$(arg use_direction_in_path)"/>
         <arg name="robot_pose_topic" value="/initialpose"/>
     </include>
 

package.xml

@@ -12,6 +12,7 @@
   <depend>nav_msgs</depend>
   <depend>roscpp</depend>
   <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
 
   <test_depend>rostest</test_depend>
   <test_depend>roslint</test_depend>

src/local_goal_creator.cpp

@@ -158,16 +158,10 @@ float LocalGoalCreator::calc_dist_between_points(const geometry_msgs::Point &poi
 geometry_msgs::Quaternion
 LocalGoalCreator::calc_direction(const geometry_msgs::Point &point1, const geometry_msgs::Point &point2)
 {
-  const float yaw = atan2(point2.y - point1.y, point2.x - point1.x);
   tf2::Quaternion q;
-  q.setRPY(0, 0, yaw);
-
+  q.setRPY(0, 0, atan2(point2.y - point1.y, point2.x - point1.x));
   geometry_msgs::Quaternion q_msg;
-  q_msg.x = q.x();
-  q_msg.y = q.y();
-  q_msg.z = q.z();
-  q_msg.w = q.w();
-
+  tf2::convert(q, q_msg);
   return q_msg;
 }