From a0206b0aa0d2d0128ee9128e8e3d138ce2d91515 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 14 Sep 2022 01:33:40 +0900 Subject: [PATCH] fix Signed-off-by: Takayuki Murooka --- .../src/tools/car_pose.cpp | 8 +++++ .../src/tools/interactive_object.cpp | 2 ++ .../src/tools/interactive_object.hpp | 4 +++ .../dummy_perception_publisher/msg/Object.msg | 2 ++ .../dummy_perception_publisher/src/node.cpp | 32 +++++++++++++++++-- 5 files changed, 46 insertions(+), 2 deletions(-) diff --git a/common/tier4_perception_rviz_plugin/src/tools/car_pose.cpp b/common/tier4_perception_rviz_plugin/src/tools/car_pose.cpp index 6ea14b4e5697d..654683ea925eb 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/car_pose.cpp +++ b/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); diff --git a/common/tier4_perception_rviz_plugin/src/tools/interactive_object.cpp b/common/tier4_perception_rviz_plugin/src/tools/interactive_object.cpp index 8df1f7bd5c429..c9626532cbecf 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/interactive_object.cpp +++ b/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); diff --git a/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp b/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp index 9f28b24c33049..6f0b382eb0b58 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp +++ b/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_; diff --git a/simulator/dummy_perception_publisher/msg/Object.msg b/simulator/dummy_perception_publisher/msg/Object.msg index 199a48b949bcb..30279299dbfb1 100644 --- a/simulator/dummy_perception_publisher/msg/Object.msg +++ b/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 diff --git a/simulator/dummy_perception_publisher/src/node.cpp b/simulator/dummy_perception_publisher/src/node.cpp index 07f8d5188b56b..13dc59b2a9add 100644 --- a/simulator/dummy_perception_publisher/src/node.cpp +++ b/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(object.min_velocity), + static_cast(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(object.min_velocity), + static_cast(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(object.max_velocity) - initial_vel, 0.0) / initial_acc; + move_distance += static_cast(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(object.min_velocity) - initial_vel, 0.0) / initial_acc; + move_distance += static_cast(object.min_velocity) * + std::max(elapsed_time - time_to_reach_min_vel, 0.0); + } } const auto current_pose =