feat(dummy_perception_publisher): publish object with acc (autowarefo…

…undation#1853)

* feat(dummy_perception_publisher): publish object with acc

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

common/tier4_perception_rviz_plugin/src/tools/car_pose.cpp

@@ -90,6 +90,12 @@ CarInitialPoseTool::CarInitialPoseTool()
     "Z position", 0.0, "Z position for initial pose [m]", getPropertyContainer());
   velocity_ = new rviz_common::properties::FloatProperty(
     "Velocity", 0.0, "velocity [m/s]", getPropertyContainer());
+  accel_ = new rviz_common::properties::FloatProperty(
+    "Acceleration", 0.0, "acceleration [m/s^2]", getPropertyContainer());
+  max_velocity_ = new rviz_common::properties::FloatProperty(
+    "Max velocity", 33.3, "Max velocity [m/s]", getPropertyContainer());
+  min_velocity_ = new rviz_common::properties::FloatProperty(
+    "Min velocity", -33.3, "Min velocity [m/s]", getPropertyContainer());
   std_dev_x_->setMin(0);
   std_dev_y_->setMin(0);
   std_dev_z_->setMin(0);
@@ -175,6 +181,12 @@ BusInitialPoseTool::BusInitialPoseTool()
     "Z position", 0.0, "Z position for initial pose [m]", getPropertyContainer());
   velocity_ = new rviz_common::properties::FloatProperty(
     "Velocity", 0.0, "velocity [m/s]", getPropertyContainer());
+  accel_ = new rviz_common::properties::FloatProperty(
+    "Acceleration", 0.0, "acceleration [m/s^2]", getPropertyContainer());
+  max_velocity_ = new rviz_common::properties::FloatProperty(
+    "Max velocity", 33.3, "Max velocity [m/s]", getPropertyContainer());
+  min_velocity_ = new rviz_common::properties::FloatProperty(
+    "Min velocity", -33.3, "Min velocity [m/s]", getPropertyContainer());
   std_dev_x_->setMin(0);
   std_dev_y_->setMin(0);
   std_dev_z_->setMin(0);

common/tier4_perception_rviz_plugin/src/tools/interactive_object.cpp

@@ -263,6 +263,11 @@ void InteractiveObjectTool::onPoseSet(double x, double y, double theta)
   output_msg.initial_state.twist_covariance.twist.linear.x = velocity_->getFloat();
   output_msg.initial_state.twist_covariance.twist.linear.y = 0.0;
   output_msg.initial_state.twist_covariance.twist.linear.z = 0.0;
+  output_msg.initial_state.accel_covariance.accel.linear.x = accel_->getFloat();
+  output_msg.initial_state.accel_covariance.accel.linear.y = 0.0;
+  output_msg.initial_state.accel_covariance.accel.linear.z = 0.0;
+  output_msg.max_velocity = max_velocity_->getFloat();
+  output_msg.min_velocity = min_velocity_->getFloat();
   output_msg.action = Object::ADD;
 
   dummy_object_info_pub_->publish(output_msg);

common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp

@@ -101,6 +101,9 @@ class InteractiveObject
   std::array<uint8_t, 16> uuid_{};
   Ogre::Vector3 point_;
   Ogre::Vector3 velocity_;
+  Ogre::Vector3 accel_;
+  Ogre::Vector3 max_velocity_;
+  Ogre::Vector3 min_velocity_;
   double theta_{0.0};
 };
 
@@ -156,6 +159,9 @@ protected Q_SLOTS:
   rviz_common::properties::FloatProperty * std_dev_theta_;
   rviz_common::properties::FloatProperty * position_z_;
   rviz_common::properties::FloatProperty * velocity_;
+  rviz_common::properties::FloatProperty * max_velocity_;
+  rviz_common::properties::FloatProperty * min_velocity_;
+  rviz_common::properties::FloatProperty * accel_;
   rviz_common::properties::TfFrameProperty * property_frame_;
 
 private:

common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.cpp

@@ -84,6 +84,12 @@ PedestrianInitialPoseTool::PedestrianInitialPoseTool()
     "Z position", 0.0, "Z position for initial pose [m]", getPropertyContainer());
   velocity_ = new rviz_common::properties::FloatProperty(
     "Velocity", 0.0, "velocity [m/s]", getPropertyContainer());
+  accel_ = new rviz_common::properties::FloatProperty(
+    "Acceleration", 0.0, "acceleration [m/s^2]", getPropertyContainer());
+  max_velocity_ = new rviz_common::properties::FloatProperty(
+    "Max velocity", 33.3, "Max velocity [m/s]", getPropertyContainer());
+  min_velocity_ = new rviz_common::properties::FloatProperty(
+    "Min velocity", -33.3, "Min velocity [m/s]", getPropertyContainer());
   std_dev_x_->setMin(0);
   std_dev_y_->setMin(0);
   std_dev_z_->setMin(0);

common/tier4_perception_rviz_plugin/src/tools/unknown_pose.cpp

@@ -79,6 +79,12 @@ UnknownInitialPoseTool::UnknownInitialPoseTool()
     "Z position", 0.0, "Z position for initial pose [m]", getPropertyContainer());
   velocity_ = new rviz_common::properties::FloatProperty(
     "Velocity", 0.0, "velocity [m/s]", getPropertyContainer());
+  accel_ = new rviz_common::properties::FloatProperty(
+    "Acceleration", 0.0, "acceleration [m/s^2]", getPropertyContainer());
+  max_velocity_ = new rviz_common::properties::FloatProperty(
+    "Max velocity", 33.3, "Max velocity [m/s]", getPropertyContainer());
+  min_velocity_ = new rviz_common::properties::FloatProperty(
+    "Min velocity", -33.3, "Min velocity [m/s]", getPropertyContainer());
   std_dev_x_->setMin(0);
   std_dev_y_->setMin(0);
   std_dev_z_->setMin(0);

simulator/dummy_perception_publisher/msg/InitialState.msg

@@ -1,2 +1,3 @@
 geometry_msgs/PoseWithCovariance pose_covariance
 geometry_msgs/TwistWithCovariance twist_covariance
+geometry_msgs/AccelWithCovariance accel_covariance

simulator/dummy_perception_publisher/msg/Object.msg

@@ -3,6 +3,8 @@ unique_identifier_msgs/UUID id
 dummy_perception_publisher/InitialState initial_state
 autoware_auto_perception_msgs/ObjectClassification classification
 autoware_auto_perception_msgs/Shape shape
+float32 max_velocity
+float32 min_velocity
 
 uint8 ADD=0
 uint8 MODIFY=1

simulator/dummy_perception_publisher/package.xml

@@ -28,6 +28,7 @@
   <depend>tf2</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
+  <depend>tier4_autoware_utils</depend>
   <depend>tier4_perception_msgs</depend>
   <depend>unique_identifier_msgs</depend>
 

simulator/dummy_perception_publisher/src/node.cpp

@@ -14,6 +14,8 @@
 
 #include "dummy_perception_publisher/node.hpp"
 
+#include "tier4_autoware_utils/tier4_autoware_utils.hpp"
+
 #include <pcl/filters/voxel_grid_occlusion_estimation.h>
 #include <tf2/LinearMath/Quaternion.h>
 #include <tf2/LinearMath/Transform.h>
@@ -41,22 +43,60 @@ ObjectInfo::ObjectInfo(
   std_dev_z(std::sqrt(object.initial_state.pose_covariance.covariance[14])),
   std_dev_yaw(std::sqrt(object.initial_state.pose_covariance.covariance[35]))
 {
-  const double move_distance =
-    object.initial_state.twist_covariance.twist.linear.x *
-    (current_time.seconds() - rclcpp::Time(object.header.stamp).seconds());
-  tf2::Transform tf_object_origin2moved_object;
-  tf2::Transform tf_map2object_origin;
-  {
-    geometry_msgs::msg::Transform ros_object_origin2moved_object;
-    ros_object_origin2moved_object.translation.x = move_distance;
-    ros_object_origin2moved_object.rotation.x = 0;
-    ros_object_origin2moved_object.rotation.y = 0;
-    ros_object_origin2moved_object.rotation.z = 0;
-    ros_object_origin2moved_object.rotation.w = 1;
-    tf2::fromMsg(ros_object_origin2moved_object, tf_object_origin2moved_object);
+  // calculate current pose
+  const auto & initial_pose = object.initial_state.pose_covariance.pose;
+  const double initial_vel = std::clamp(
+    object.initial_state.twist_covariance.twist.linear.x, static_cast<double>(object.min_velocity),
+    static_cast<double>(object.max_velocity));
+  const double initial_acc = object.initial_state.accel_covariance.accel.linear.x;
+
+  const double elapsed_time = current_time.seconds() - rclcpp::Time(object.header.stamp).seconds();
+
+  double move_distance;
+  if (initial_acc == 0.0) {
+    move_distance = initial_vel * elapsed_time;
+  } else {
+    double current_vel = initial_vel + initial_acc * elapsed_time;
+    if (initial_acc < 0 && 0 < initial_vel) {
+      current_vel = std::max(current_vel, 0.0);
+    }
+    if (0 < initial_acc && initial_vel < 0) {
+      current_vel = std::min(current_vel, 0.0);
+    }
+
+    // add distance on acceleration or deceleration
+    current_vel = std::clamp(
+      current_vel, static_cast<double>(object.min_velocity),
+      static_cast<double>(object.max_velocity));
+    move_distance = (std::pow(current_vel, 2) - std::pow(initial_vel, 2)) * 0.5 / initial_acc;
+
+    // add distance after reaching max_velocity
+    if (0 < initial_acc) {
+      const double time_to_reach_max_vel =
+        std::max(static_cast<double>(object.max_velocity) - initial_vel, 0.0) / initial_acc;
+      move_distance += static_cast<double>(object.max_velocity) *
+                       std::max(elapsed_time - time_to_reach_max_vel, 0.0);
+    }
+
+    // add distance after reaching min_velocity
+    if (initial_acc < 0) {
+      const double time_to_reach_min_vel =
+        std::min(static_cast<double>(object.min_velocity) - initial_vel, 0.0) / initial_acc;
+      move_distance += static_cast<double>(object.min_velocity) *
+                       std::max(elapsed_time - time_to_reach_min_vel, 0.0);
+    }
   }
-  tf2::fromMsg(object.initial_state.pose_covariance.pose, tf_map2object_origin);
-  this->tf_map2moved_object = tf_map2object_origin * tf_object_origin2moved_object;
+
+  const auto current_pose =
+    tier4_autoware_utils::calcOffsetPose(initial_pose, move_distance, 0.0, 0.0);
+
+  // calculate tf from map to moved_object
+  geometry_msgs::msg::Transform ros_map2moved_object;
+  ros_map2moved_object.translation.x = current_pose.position.x;
+  ros_map2moved_object.translation.y = current_pose.position.y;
+  ros_map2moved_object.translation.z = current_pose.position.z;
+  ros_map2moved_object.rotation = current_pose.orientation;
+  tf2::fromMsg(ros_map2moved_object, tf_map2moved_object);
 }
 
 DummyPerceptionPublisherNode::DummyPerceptionPublisherNode()
@@ -182,6 +222,7 @@ void DummyPerceptionPublisherNode::timerCallback()
       tf2::Transform tf_base_link2noised_moved_object;
       tf_base_link2noised_moved_object =
         tf_base_link2map * object_info.tf_map2moved_object * tf_moved_object2noised_moved_object;
+
       tier4_perception_msgs::msg::DetectedObjectWithFeature feature_object;
       feature_object.object.classification.push_back(object.classification);
       feature_object.object.kinematics.pose_with_covariance = object.initial_state.pose_covariance;