trajectory_tracker: support PathWithVelocity

README.md

@@ -14,13 +14,15 @@ ROS meta-package for autonomous vehicle navigation.
 
 ## Install
 
-**Note: neonavigation_msgs meta-package is required to build neonavigation meta-package.**
+- **Note 1: neonavigation_msgs meta-package is required to build neonavigation meta-package.**
+- **Note 2: neonavigation_rviz_plugins meta-package is required to visualize PathWithVelocity message used between planner_3d and trajectory_tracker.**
 
 ```shell
 # clone
 cd /path/to/your/catkin_ws/src
 git clone https://github.com/at-wat/neonavigation.git
 git clone https://github.com/at-wat/neonavigation_msgs.git
+git clone https://github.com/at-wat/neonavigation_rviz_plugins.git
 
 # build
 cd /path/to/your/catkin_ws

neonavigation_launch/config/visualization.rviz

@@ -6,7 +6,7 @@ Panels:
       Expanded:
         - /Global Options1
       Splitter Ratio: 0.7241380214691162
-    Tree Height: 744
+    Tree Height: 817
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -101,6 +101,29 @@ Visualization Manager:
       Topic: /path
       Unreliable: false
       Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: trajectory_tracker_rviz_plugins/PathWithVelocity
+      Color: 25; 255; 0
+      Enabled: true
+      Head Diameter: 0.30000001192092896
+      Head Length: 0.20000000298023224
+      Length: 0.30000001192092896
+      Line Style: Billboards
+      Line Width: 0.20000000298023224
+      Name: PathWithVelocity
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Radius: 0.029999999329447746
+      Shaft Diameter: 0.10000000149011612
+      Shaft Length: 0.10000000149011612
+      Topic: /path_velocity
+      Unreliable: false
+      Value: true
     - Alpha: 0.5
       Axes Length: 1
       Axes Radius: 0.10000000149011612
@@ -208,10 +231,10 @@ Visualization Manager:
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1016
+  Height: 1129
   Hide Left Dock: false
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd00000004000000000000017e00000387fc0200000009fb0000001200530065006c0065006300740069006f006e0000000040000001c70000006c01000007fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004800000387000000e101000007fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000303000000d00000000000000000000000010000011000000395fc0200000003fb0000000a00560069006500770073000000004400000395000000ca01000007fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000780000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003c000000046fc0100000002fb0000000800540069006d00650000000000000003c00000032b01000007fb0000000800540069006d00650100000000000004500000000000000000000006010000038700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000001a6000003defc0200000009fb0000001200530065006c0065006300740069006f006e0000000040000001c70000007001000007fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000058000003de000000ef01000007fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000303000000d00000000000000000000000010000011000000395fc0200000003fb0000000a00560069006500770073000000004400000395000000e201000007fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000780000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003c000000046fc0100000002fb0000000800540069006d00650000000000000003c00000043801000007fb0000000800540069006d00650100000000000004500000000000000000000005d9000003de00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -222,4 +245,4 @@ Window Geometry:
     collapsed: true
   Width: 1920
   X: 0
-  Y: 27
+  Y: 34

neonavigation_launch/launch/demo.launch

@@ -1,8 +1,11 @@
 <?xml version="1.0"?>
 <launch>
+  <arg name="use_path_with_velocity" default="true" />
+
   <include file="$(find neonavigation_launch)/launch/navigate.launch">
     <arg name="simulate" value="true" />
     <arg name="map_file" value="$(find neonavigation_launch)/map/demo_map.yaml" />
+    <arg name="use_path_with_velocity" value="true" />
   </include>
 
   <node pkg="map_server" type="map_server" name="map_server_local" args="$(find neonavigation_launch)/map/demo_map_local.yaml">

neonavigation_launch/launch/navigate.launch

@@ -3,6 +3,7 @@
   <arg name="map_file" />
   <arg name="use_map_server" default="true" />
   <arg name="use_safety_limiter" default="false" />
+  <arg name="use_path_with_velocity" default="true" />
   <arg name="simulate" default="false" />
   <arg name="sim_robot_x" default="0.0" />
   <arg name="sim_robot_y" default="0.0" />
@@ -28,7 +29,9 @@
   </node>
   <node pkg="map_server" type="map_server" name="map_server" args="$(arg map_file)" if="$(arg use_map_server)" />
 
-  <node pkg="planner_cspace" type="planner_3d" name="planner_3d" output="$(arg output_info)" />
+  <node pkg="planner_cspace" type="planner_3d" name="planner_3d" output="$(arg output_info)">
+    <param name="use_path_with_velocity" value="$(arg use_path_with_velocity)" />
+  </node>
 
   <node pkg="trajectory_tracker" type="trajectory_tracker" name="spur">
     <remap from="cmd_vel" to="$(arg cmd_vel_output)" unless="$(arg use_safety_limiter)" />

neonavigation_launch/package.xml

@@ -16,4 +16,5 @@
   <exec_depend>safety_limiter</exec_depend>
   <exec_depend>tf2_ros</exec_depend>
   <exec_depend>trajectory_tracker</exec_depend>
+  <!--exec_depend>trajectory_tracker_rviz_plugins</exec_depend-->  <!--TODO(at-wat): uncomment if trajectory_tracker_rviz_plugins is released. -->
 </package>

planner_cspace/CMakeLists.txt

@@ -21,6 +21,7 @@ find_package(catkin REQUIRED
     costmap_cspace_msgs
     neonavigation_common
     planner_cspace_msgs
+    trajectory_tracker_msgs
 )
 
 find_package(Boost REQUIRED COMPONENTS chrono)
@@ -46,6 +47,7 @@ catkin_package(
     costmap_cspace_msgs
     neonavigation_common
     planner_cspace_msgs
+    trajectory_tracker_msgs
 )
 
 include(CheckCXXCompilerFlag)

planner_cspace/package.xml

@@ -34,4 +34,5 @@
   <depend>costmap_cspace_msgs</depend>
   <depend>neonavigation_common</depend>
   <depend>planner_cspace_msgs</depend>
+  <depend>trajectory_tracker_msgs</depend>
 </package>

planner_cspace/src/planner_3d.cpp

@@ -27,6 +27,12 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
+#include <algorithm>
+#include <limits>
+#include <list>
+#include <string>
+#include <vector>
+
 #include <ros/ros.h>
 
 #include <costmap_cspace_msgs/CSpace3D.h>
@@ -37,6 +43,8 @@
 #include <planner_cspace_msgs/PlannerStatus.h>
 #include <sensor_msgs/PointCloud.h>
 #include <std_srvs/Empty.h>
+#include <trajectory_tracker_msgs/PathWithVelocity.h>
+#include <trajectory_tracker_msgs/converter.h>
 
 #include <ros/console.h>
 #include <tf2/utils.h>
@@ -46,11 +54,6 @@
 #include <actionlib/server/simple_action_server.h>
 #include <move_base_msgs/MoveBaseAction.h>
 
-#include <algorithm>
-#include <list>
-#include <string>
-#include <vector>
-
 #include <costmap_cspace/node_handle_float.h>
 #include <neonavigation_common/compatibility.h>
 
@@ -77,6 +80,7 @@ class Planner3dNode
   ros::Subscriber sub_map_update_;
   ros::Subscriber sub_goal_;
   ros::Publisher pub_path_;
+  ros::Publisher pub_path_velocity_;
   ros::Publisher pub_debug_;
   ros::Publisher pub_hist_;
   ros::Publisher pub_start_;
@@ -161,6 +165,7 @@ class Planner3dNode
   bool temporary_escape_;
   float remember_hit_odds_;
   float remember_miss_odds_;
+  bool use_path_with_velocity_;
 
   JumpDetector jump_;
   std::string robot_frame_;
@@ -1142,9 +1147,6 @@ class Planner3dNode
     sub_goal_ = neonavigation_common::compat::subscribe(
         nh_, "move_base_simple/goal",
         pnh_, "goal", 1, &Planner3dNode::cbGoal, this);
-    pub_path_ = neonavigation_common::compat::advertise<nav_msgs::Path>(
-        nh_, "path",
-        pnh_, "path", 1, true);
     pub_debug_ = pnh_.advertise<sensor_msgs::PointCloud>("debug", 1, true);
     pub_hist_ = pnh_.advertise<sensor_msgs::PointCloud>("remembered", 1, true);
     pub_start_ = pnh_.advertise<geometry_msgs::PoseStamped>("path_start", 1, true);
@@ -1159,6 +1161,19 @@ class Planner3dNode
     act_->registerGoalCallback(boost::bind(&Planner3dNode::cbAction, this));
     act_->registerPreemptCallback(boost::bind(&Planner3dNode::cbPreempt, this));
 
+    pnh_.param("use_path_with_velocity", use_path_with_velocity_, false);
+    if (use_path_with_velocity_)
+    {
+      pub_path_velocity_ = nh_.advertise<trajectory_tracker_msgs::PathWithVelocity>(
+          "path_velocity", 1, true);
+    }
+    else
+    {
+      pub_path_ = neonavigation_common::compat::advertise<nav_msgs::Path>(
+          nh_, "path",
+          pnh_, "path", 1, true);
+    }
+
     pnh_.param_cast("freq", freq_, 4.0f);
     pnh_.param_cast("freq_min", freq_min_, 2.0f);
     pnh_.param_cast("search_range", search_range_, 0.4f);
@@ -1372,7 +1387,16 @@ class Planner3dNode
           path.header = map_header_;
           path.header.stamp = now;
           makePlan(start_.pose, goal_.pose, path, true);
-          pub_path_.publish(path);
+          if (use_path_with_velocity_)
+          {
+            // NaN velocity means that don't care the velocity
+            pub_path_velocity_.publish(
+                trajectory_tracker_msgs::toPathWithVelocity(path, std::numeric_limits<double>::quiet_NaN()));
+          }
+          else
+          {
+            pub_path_.publish(path);
+          }
 
           if (switchDetect(path))
           {

trajectory_tracker/include/trajectory_tracker/path2d.h

@@ -49,15 +49,18 @@ class Pose2D
 public:
   Eigen::Vector2d pos_;
   float yaw_;
+  float velocity_;
 
-  inline Pose2D(const Eigen::Vector2d& p, float y)
+  inline Pose2D(const Eigen::Vector2d& p, float y, float velocity)
     : pos_(p)
     , yaw_(y)
+    , velocity_(velocity)
   {
   }
-  inline explicit Pose2D(const geometry_msgs::Pose& pose)
+  inline explicit Pose2D(const geometry_msgs::Pose& pose, float velocity)
     : pos_(Eigen::Vector2d(pose.position.x, pose.position.y))
     , yaw_(tf2::getYaw(pose.orientation))
+    , velocity_(velocity)
   {
   }
 };