fix

Signed-off-by: Takayuki Murooka <[email protected]>

common/tier4_perception_rviz_plugin/src/tools/car_pose.cpp

@@ -92,6 +92,10 @@ CarInitialPoseTool::CarInitialPoseTool()
     "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);
@@ -179,6 +183,10 @@ BusInitialPoseTool::BusInitialPoseTool()
     "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

@@ -266,6 +266,8 @@ void InteractiveObjectTool::onPoseSet(double x, double y, double theta)
   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

@@ -102,6 +102,8 @@ class InteractiveObject
   Ogre::Vector3 point_;
   Ogre::Vector3 velocity_;
   Ogre::Vector3 accel_;
+  Ogre::Vector3 max_velocity_;
+  Ogre::Vector3 min_velocity_;
   double theta_{0.0};
 };
 
@@ -157,6 +159,8 @@ 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_;
 

common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.cpp

@@ -86,6 +86,10 @@ PedestrianInitialPoseTool::PedestrianInitialPoseTool()
     "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

@@ -81,6 +81,10 @@ UnknownInitialPoseTool::UnknownInitialPoseTool()
     "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/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/src/node.cpp

@@ -45,7 +45,9 @@ ObjectInfo::ObjectInfo(
 {
   // calculate current pose
   const auto & initial_pose = object.initial_state.pose_covariance.pose;
-  const auto & initial_vel = object.initial_state.twist_covariance.twist.linear.x;
+  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();
@@ -54,9 +56,35 @@ ObjectInfo::ObjectInfo(
   if (initial_acc == 0.0) {
     move_distance = initial_vel * elapsed_time;
   } else {
-    const double current_vel = std::max(0.0, initial_vel + initial_acc * elapsed_time);
+    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);
+    }
   }
 
   const auto current_pose =