From 20a8c7256654c292bc8fe3b89decb6cb1bd2630c Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 13 Sep 2022 16:24:37 +0900 Subject: [PATCH 1/3] feat(dummy_perception_publisher): publish object with acc Signed-off-by: Takayuki Murooka --- .../src/tools/car_pose.cpp | 2 + .../src/tools/interactive_object.cpp | 3 ++ .../src/tools/interactive_object.hpp | 2 + .../dummy_perception_publisher/node.hpp | 3 ++ .../msg/InitialState.msg | 1 + .../dummy_perception_publisher/package.xml | 1 + .../dummy_perception_publisher/src/node.cpp | 52 +++++++++++++------ .../src/test_signed_distance_function.cpp | 2 + 8 files changed, 50 insertions(+), 16 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 aa2c198b35c8b..be88772b70369 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/car_pose.cpp +++ b/common/tier4_perception_rviz_plugin/src/tools/car_pose.cpp @@ -90,6 +90,8 @@ 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()); 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 5f0233221963e..8df1f7bd5c429 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/interactive_object.cpp +++ b/common/tier4_perception_rviz_plugin/src/tools/interactive_object.cpp @@ -263,6 +263,9 @@ 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.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 5513e93600466..9f28b24c33049 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp +++ b/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp @@ -101,6 +101,7 @@ class InteractiveObject std::array uuid_{}; Ogre::Vector3 point_; Ogre::Vector3 velocity_; + Ogre::Vector3 accel_; double theta_{0.0}; }; @@ -156,6 +157,7 @@ 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 * accel_; rviz_common::properties::TfFrameProperty * property_frame_; private: diff --git a/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp b/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp index ea314207dc203..158a2c6ba6d5b 100644 --- a/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp +++ b/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp @@ -16,6 +16,7 @@ #define DUMMY_PERCEPTION_PUBLISHER__NODE_HPP_ #include "dummy_perception_publisher/msg/object.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include @@ -120,6 +121,8 @@ class DummyPerceptionPublisherNode : public rclcpp::Node double angle_increment_; + tier4_autoware_utils::SelfPoseListener self_pose_listener_; + std::mt19937 random_generator_; void timerCallback(); void objectCallback(const dummy_perception_publisher::msg::Object::ConstSharedPtr msg); diff --git a/simulator/dummy_perception_publisher/msg/InitialState.msg b/simulator/dummy_perception_publisher/msg/InitialState.msg index 1b73ce57e8a88..39948a7f731b8 100644 --- a/simulator/dummy_perception_publisher/msg/InitialState.msg +++ b/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 diff --git a/simulator/dummy_perception_publisher/package.xml b/simulator/dummy_perception_publisher/package.xml index 1635deda40a4b..b24a015d21ddd 100644 --- a/simulator/dummy_perception_publisher/package.xml +++ b/simulator/dummy_perception_publisher/package.xml @@ -28,6 +28,7 @@ tf2 tf2_geometry_msgs tf2_ros + tier4_autoware_utils tier4_perception_msgs unique_identifier_msgs diff --git a/simulator/dummy_perception_publisher/src/node.cpp b/simulator/dummy_perception_publisher/src/node.cpp index a24479a2ac0eb..920929eb7916f 100644 --- a/simulator/dummy_perception_publisher/src/node.cpp +++ b/simulator/dummy_perception_publisher/src/node.cpp @@ -41,26 +41,40 @@ 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 auto & initial_vel = object.initial_state.twist_covariance.twist.linear.x; + const double initial_acc = object.initial_state.accel_covariance.accel.linear.x; + // const double initial_acc = -1.0; + + 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 { + const double current_vel = std::max(0.0, initial_vel + initial_acc * elapsed_time); + + move_distance = (std::pow(current_vel, 2) - std::pow(initial_vel, 2)) * 0.5 / initial_acc; } - 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() -: Node("dummy_perception_publisher"), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) +: Node("dummy_perception_publisher"), + tf_buffer_(this->get_clock()), + tf_listener_(tf_buffer_), + self_pose_listener_(this) { visible_range_ = this->declare_parameter("visible_range", 100.0); detection_successful_rate_ = this->declare_parameter("detection_successful_rate", 0.8); @@ -94,6 +108,9 @@ DummyPerceptionPublisherNode::DummyPerceptionPublisherNode() "input/object", 100, std::bind(&DummyPerceptionPublisherNode::objectCallback, this, std::placeholders::_1)); + // wait for first self pose + self_pose_listener_.waitForFirstPose(); + using std::chrono_literals::operator""ms; timer_ = rclcpp::create_timer( this, get_clock(), 100ms, std::bind(&DummyPerceptionPublisherNode::timerCallback, this)); @@ -101,6 +118,8 @@ DummyPerceptionPublisherNode::DummyPerceptionPublisherNode() void DummyPerceptionPublisherNode::timerCallback() { + const auto ego_pose = self_pose_listener_.getCurrentPose()->pose; + // output msgs tier4_perception_msgs::msg::DetectedObjectsWithFeature output_dynamic_object_msg; geometry_msgs::msg::PoseStamped output_moved_object_pose; @@ -182,6 +201,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; diff --git a/simulator/dummy_perception_publisher/test/src/test_signed_distance_function.cpp b/simulator/dummy_perception_publisher/test/src/test_signed_distance_function.cpp index 5b8dcee549ea7..cf90094fdf5d8 100644 --- a/simulator/dummy_perception_publisher/test/src/test_signed_distance_function.cpp +++ b/simulator/dummy_perception_publisher/test/src/test_signed_distance_function.cpp @@ -25,6 +25,7 @@ namespace sdf = signed_distance_function; +/* TEST(SignedDistanceFunctionTest, BoxSDF) { const double eps = 1e-5; @@ -76,6 +77,7 @@ TEST(SignedDistanceFunctionTest, CompositeSDF) // ASSERT_NEAR(func.getSphereTracingDist(0.0, 1.0, M_PI * 0.5, eps), 0.5, eps); // ASSERT_NEAR(func.getSphereTracingDist(0.0, 1.0, M_PI * -0.5, eps), 0.5, eps); } +*/ int main(int argc, char ** argv) { From 965f2928129bea57fa8f69faae9d36b5e89f082d Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 13 Sep 2022 16:33:39 +0900 Subject: [PATCH 2/3] fix Signed-off-by: Takayuki Murooka --- .../src/tools/car_pose.cpp | 2 ++ .../src/tools/pedestrian_pose.cpp | 2 ++ .../src/tools/unknown_pose.cpp | 2 ++ .../include/dummy_perception_publisher/node.hpp | 3 --- simulator/dummy_perception_publisher/src/node.cpp | 13 +++---------- .../test/src/test_signed_distance_function.cpp | 2 -- 6 files changed, 9 insertions(+), 15 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 be88772b70369..6ea14b4e5697d 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/car_pose.cpp +++ b/common/tier4_perception_rviz_plugin/src/tools/car_pose.cpp @@ -177,6 +177,8 @@ 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()); 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/pedestrian_pose.cpp b/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.cpp index caf85cdae1358..a0bf04ae2efe8 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.cpp +++ b/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.cpp @@ -84,6 +84,8 @@ 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()); 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/unknown_pose.cpp b/common/tier4_perception_rviz_plugin/src/tools/unknown_pose.cpp index 81885f8931928..4b7188e9584e0 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/unknown_pose.cpp +++ b/common/tier4_perception_rviz_plugin/src/tools/unknown_pose.cpp @@ -79,6 +79,8 @@ 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()); std_dev_x_->setMin(0); std_dev_y_->setMin(0); std_dev_z_->setMin(0); diff --git a/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp b/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp index 158a2c6ba6d5b..ea314207dc203 100644 --- a/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp +++ b/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp @@ -16,7 +16,6 @@ #define DUMMY_PERCEPTION_PUBLISHER__NODE_HPP_ #include "dummy_perception_publisher/msg/object.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include @@ -121,8 +120,6 @@ class DummyPerceptionPublisherNode : public rclcpp::Node double angle_increment_; - tier4_autoware_utils::SelfPoseListener self_pose_listener_; - std::mt19937 random_generator_; void timerCallback(); void objectCallback(const dummy_perception_publisher::msg::Object::ConstSharedPtr msg); diff --git a/simulator/dummy_perception_publisher/src/node.cpp b/simulator/dummy_perception_publisher/src/node.cpp index 920929eb7916f..07f8d5188b56b 100644 --- a/simulator/dummy_perception_publisher/src/node.cpp +++ b/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 #include #include @@ -45,7 +47,6 @@ ObjectInfo::ObjectInfo( 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_acc = object.initial_state.accel_covariance.accel.linear.x; - // const double initial_acc = -1.0; const double elapsed_time = current_time.seconds() - rclcpp::Time(object.header.stamp).seconds(); @@ -71,10 +72,7 @@ ObjectInfo::ObjectInfo( } DummyPerceptionPublisherNode::DummyPerceptionPublisherNode() -: Node("dummy_perception_publisher"), - tf_buffer_(this->get_clock()), - tf_listener_(tf_buffer_), - self_pose_listener_(this) +: Node("dummy_perception_publisher"), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) { visible_range_ = this->declare_parameter("visible_range", 100.0); detection_successful_rate_ = this->declare_parameter("detection_successful_rate", 0.8); @@ -108,9 +106,6 @@ DummyPerceptionPublisherNode::DummyPerceptionPublisherNode() "input/object", 100, std::bind(&DummyPerceptionPublisherNode::objectCallback, this, std::placeholders::_1)); - // wait for first self pose - self_pose_listener_.waitForFirstPose(); - using std::chrono_literals::operator""ms; timer_ = rclcpp::create_timer( this, get_clock(), 100ms, std::bind(&DummyPerceptionPublisherNode::timerCallback, this)); @@ -118,8 +113,6 @@ DummyPerceptionPublisherNode::DummyPerceptionPublisherNode() void DummyPerceptionPublisherNode::timerCallback() { - const auto ego_pose = self_pose_listener_.getCurrentPose()->pose; - // output msgs tier4_perception_msgs::msg::DetectedObjectsWithFeature output_dynamic_object_msg; geometry_msgs::msg::PoseStamped output_moved_object_pose; diff --git a/simulator/dummy_perception_publisher/test/src/test_signed_distance_function.cpp b/simulator/dummy_perception_publisher/test/src/test_signed_distance_function.cpp index cf90094fdf5d8..5b8dcee549ea7 100644 --- a/simulator/dummy_perception_publisher/test/src/test_signed_distance_function.cpp +++ b/simulator/dummy_perception_publisher/test/src/test_signed_distance_function.cpp @@ -25,7 +25,6 @@ namespace sdf = signed_distance_function; -/* TEST(SignedDistanceFunctionTest, BoxSDF) { const double eps = 1e-5; @@ -77,7 +76,6 @@ TEST(SignedDistanceFunctionTest, CompositeSDF) // ASSERT_NEAR(func.getSphereTracingDist(0.0, 1.0, M_PI * 0.5, eps), 0.5, eps); // ASSERT_NEAR(func.getSphereTracingDist(0.0, 1.0, M_PI * -0.5, eps), 0.5, eps); } -*/ int main(int argc, char ** argv) { From c9f0d18355b8a28535235d2e23e52dde05c23e3a Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 14 Sep 2022 01:40:48 +0900 Subject: [PATCH 3/3] fix Signed-off-by: Takayuki Murooka --- .../src/tools/car_pose.cpp | 8 +++++ .../src/tools/interactive_object.cpp | 2 ++ .../src/tools/interactive_object.hpp | 4 +++ .../src/tools/pedestrian_pose.cpp | 4 +++ .../src/tools/unknown_pose.cpp | 4 +++ .../dummy_perception_publisher/msg/Object.msg | 2 ++ .../dummy_perception_publisher/src/node.cpp | 32 +++++++++++++++++-- 7 files changed, 54 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/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.cpp b/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.cpp index a0bf04ae2efe8..f4eb3064855ec 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.cpp +++ b/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); diff --git a/common/tier4_perception_rviz_plugin/src/tools/unknown_pose.cpp b/common/tier4_perception_rviz_plugin/src/tools/unknown_pose.cpp index 4b7188e9584e0..93d14396b3561 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/unknown_pose.cpp +++ b/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); 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 =