feat(dummy_perception_publisher): publish object with acc

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

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);

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);

common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp

@@ -101,6 +101,7 @@ class InteractiveObject
   std::array<uint8_t, 16> 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:

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 <rclcpp/rclcpp.hpp>
 
@@ -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);

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/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

@@ -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,13 +108,18 @@ 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));
 }
 
 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;

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)
 {