change testpose. add acc & stear test.not complete.

src/ros2/src/testpose/CMakeLists.txt

@@ -41,7 +41,8 @@ target_link_libraries(testpose Geographic)
 
 ament_target_dependencies(testpose rclcpp std_msgs geometry_msgs 
   tf2_geometry_msgs nav_msgs sensor_msgs autoware_auto_control_msgs autoware_auto_geometry_msgs
-  autoware_auto_mapping_msgs autoware_auto_planning_msgs autoware_auto_vehicle_msgs)
+  autoware_auto_mapping_msgs autoware_auto_planning_msgs autoware_auto_vehicle_msgs
+  tier4_autoware_utils)
 
 
 

src/ros2/src/testpose/include/testpose/testpose_core.hpp

@@ -62,6 +62,10 @@
 
 #include <string>
 
+#include "tier4_autoware_utils/geometry/geometry.hpp"
+#include "tier4_autoware_utils/ros/msg_covariance.hpp"
+#include "tier4_autoware_utils/ros/update_param.hpp"
+
 
 using autoware_auto_control_msgs::msg::AckermannControlCommand;
 using autoware_auto_geometry_msgs::msg::Complex32;
@@ -102,14 +106,22 @@ private:
 
   rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
 
-  rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_pub_;
-
-
+    rclcpp::Publisher<VelocityReport>::SharedPtr pub_velocity_;
   rclcpp::Publisher<Odometry>::SharedPtr pub_odom_;
-  rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr pub_tf_;
+  rclcpp::Publisher<SteeringReport>::SharedPtr pub_steer_;
   rclcpp::Publisher<AccelWithCovarianceStamped>::SharedPtr pub_acc_;
   rclcpp::Publisher<Imu>::SharedPtr pub_imu_;
-  rclcpp::Publisher<VelocityReport>::SharedPtr pub_velocity_;
+  rclcpp::Publisher<ControlModeReport>::SharedPtr pub_control_mode_report_;
+  rclcpp::Publisher<GearReport>::SharedPtr pub_gear_report_;
+  rclcpp::Publisher<TurnIndicatorsReport>::SharedPtr pub_turn_indicators_report_;
+  rclcpp::Publisher<HazardLightsReport>::SharedPtr pub_hazard_lights_report_;
+  rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr pub_tf_;
+  rclcpp::Publisher<PoseStamped>::SharedPtr pub_current_pose_;
+
+  rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_pub_;
+
+
+
 
   rclcpp::TimerBase::SharedPtr mptimer;
 
@@ -122,6 +134,14 @@ private:
    */
   void publish_velocity(const VelocityReport & velocity);
 
+    /**
+   * @brief publish steering
+   * @param [in] steer The steering to publish
+   */
+  void publish_steering(const SteeringReport & steer);
+
+  void publish_acceleration();
+
 
   void CalcXY(double flon,double flat,double & fx,double & fy);
 
@@ -130,6 +150,8 @@ private:
   std::string simulated_frame_id_;  //!< @brief simulated vehicle frame id
   std::string origin_frame_id_;     //!< @brief map frame_id
 
+  SteeringReport current_steer_;
+
 
 };
 

src/ros2/src/testpose/package.xml

@@ -16,6 +16,11 @@
   <depend>autoware_auto_tf2</depend>
   <depend>autoware_auto_vehicle_msgs</depend>
 
+    <depend>tier4_api_utils</depend>
+  <depend>tier4_autoware_utils</depend>
+  <depend>tier4_external_api_msgs</depend>
+  <depend>tier4_vehicle_msgs</depend>
+
   <depend>geometry_msgs</depend>
   <depend>nav_msgs</depend>
   <depend>sensor_msgs</depend>

src/ros2/src/testpose/src/testpose_core.cpp

@@ -57,6 +57,9 @@ pub_tf_ = create_publisher<tf2_msgs::msg::TFMessage>("/tf", durable_qos);
 
 pub_velocity_ = create_publisher<VelocityReport>("/vehicle/status/velocity_status", durable_qos);
 
+pub_steer_ = create_publisher<SteeringReport>("/vehicle/status/steering_status", durable_qos);
+pub_acc_ = create_publisher<AccelWithCovarianceStamped>("/localization/acceleration", durable_qos);
+
   mptimer= create_wall_timer(100ms,std::bind(&TestPose::callbackTimer, this));
 
 
@@ -237,6 +240,13 @@ void TestPose::callbackTimer()
   velocity.heading_rate = 0;
   publish_velocity(velocity);
 
+    current_steer_.steering_tire_angle = static_cast<double>(0);
+  current_steer_.stamp = get_clock()->now();
+
+  publish_steering(current_steer_);
+
+  publish_acceleration();
+
 
 }
 
@@ -302,3 +312,29 @@ void TestPose::CalcXY(double flon,double flat,double & fx,double & fy)
 }
 
 
+
+
+void TestPose::publish_steering(const SteeringReport & steer)
+{
+
+  pub_steer_->publish(steer);
+}
+
+void TestPose::publish_acceleration()
+{
+  AccelWithCovarianceStamped msg;
+  msg.header.frame_id = "/base_link";
+  msg.header.stamp = get_clock()->now();
+  msg.accel.accel.linear.x = 0;//vehicle_model_ptr_->getAx();
+
+  using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
+  constexpr auto COV = 0.001;
+  msg.accel.covariance.at(COV_IDX::X_X) = COV;          // linear x
+  msg.accel.covariance.at(COV_IDX::Y_Y) = COV;          // linear y
+  msg.accel.covariance.at(COV_IDX::Z_Z) = COV;          // linear z
+  msg.accel.covariance.at(COV_IDX::ROLL_ROLL) = COV;    // angular x
+  msg.accel.covariance.at(COV_IDX::PITCH_PITCH) = COV;  // angular y
+  msg.accel.covariance.at(COV_IDX::YAW_YAW) = COV;      // angular z
+  pub_acc_->publish(msg);
+}
+