From c50b25e86ad4f425a7466eef1c11f9f243508cfd Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 14 Sep 2022 10:05:01 +0900 Subject: [PATCH] feat(dummy_perception_publisher): publish object with acc (#1853) * feat(dummy_perception_publisher): publish object with acc Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka Signed-off-by: Takayuki Murooka --- .../src/tools/car_pose.cpp | 12 ++++ .../src/tools/interactive_object.cpp | 5 ++ .../src/tools/interactive_object.hpp | 6 ++ .../src/tools/pedestrian_pose.cpp | 6 ++ .../src/tools/unknown_pose.cpp | 6 ++ .../msg/InitialState.msg | 1 + .../dummy_perception_publisher/msg/Object.msg | 2 + .../dummy_perception_publisher/package.xml | 1 + .../dummy_perception_publisher/src/node.cpp | 71 +++++++++++++++---- 9 files changed, 95 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 aa2c198b35c8b..654683ea925eb 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,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); 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..c9626532cbecf 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,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); 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..6f0b382eb0b58 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,9 @@ class InteractiveObject std::array 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: 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..f4eb3064855ec 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,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); 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..93d14396b3561 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,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); 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/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/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..13dc59b2a9add 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 @@ -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(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(); + + 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(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); + } } - 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;