change testpose, add vel.

src/ros2/src/testpose/CMakeLists.txt

@@ -1,6 +1,10 @@
 cmake_minimum_required(VERSION 3.14)
 project(testpose)
 
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+
 #Default to C++14
 if(NOT CMAKE_CXX_STANDARD)
   set(CMAKE_CXX_STANDARD 14)
@@ -18,6 +22,7 @@ find_package(geometry_msgs REQUIRED)
 find_package(nav_msgs REQUIRED) 
 find_package(sensor_msgs REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
+find_package(autoware_auto_control_msgs REQUIRED)
 
 include_directories(
 ##  INCLUDE_DIRS include
@@ -31,7 +36,9 @@ add_executable(testpose
   src/testpose_core.cpp
 )
 
-ament_target_dependencies(testpose rclcpp std_msgs geometry_msgs tf2_geometry_msgs nav_msgs sensor_msgs)
+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)
 
 
 

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

@@ -20,6 +20,33 @@
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/twist_stamped.hpp>
 
+#include "autoware_auto_control_msgs/msg/ackermann_control_command.hpp"
+#include "autoware_auto_geometry_msgs/msg/complex32.hpp"
+#include "autoware_auto_mapping_msgs/msg/had_map_bin.hpp"
+#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
+#include "autoware_auto_vehicle_msgs/msg/control_mode_command.hpp"
+#include "autoware_auto_vehicle_msgs/msg/control_mode_report.hpp"
+#include "autoware_auto_vehicle_msgs/msg/engage.hpp"
+#include "autoware_auto_vehicle_msgs/msg/gear_command.hpp"
+#include "autoware_auto_vehicle_msgs/msg/gear_report.hpp"
+#include "autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp"
+#include "autoware_auto_vehicle_msgs/msg/hazard_lights_report.hpp"
+#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp"
+#include "autoware_auto_vehicle_msgs/msg/turn_indicators_command.hpp"
+#include "autoware_auto_vehicle_msgs/msg/turn_indicators_report.hpp"
+#include "autoware_auto_vehicle_msgs/msg/vehicle_control_command.hpp"
+#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp"
+#include "autoware_auto_vehicle_msgs/srv/control_mode_command.hpp"
+#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp"
+#include "geometry_msgs/msg/pose.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
+#include "geometry_msgs/msg/transform_stamped.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+#include "geometry_msgs/msg/twist_stamped.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "sensor_msgs/msg/imu.hpp"
+
 #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp"
 #include "geometry_msgs/msg/pose.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
@@ -36,6 +63,22 @@
 #include <string>
 
 
+using autoware_auto_control_msgs::msg::AckermannControlCommand;
+using autoware_auto_geometry_msgs::msg::Complex32;
+using autoware_auto_mapping_msgs::msg::HADMapBin;
+using autoware_auto_planning_msgs::msg::Trajectory;
+using autoware_auto_vehicle_msgs::msg::ControlModeReport;
+using autoware_auto_vehicle_msgs::msg::Engage;
+using autoware_auto_vehicle_msgs::msg::GearCommand;
+using autoware_auto_vehicle_msgs::msg::GearReport;
+using autoware_auto_vehicle_msgs::msg::HazardLightsCommand;
+using autoware_auto_vehicle_msgs::msg::HazardLightsReport;
+using autoware_auto_vehicle_msgs::msg::SteeringReport;
+using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand;
+using autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport;
+using autoware_auto_vehicle_msgs::msg::VehicleControlCommand;
+using autoware_auto_vehicle_msgs::msg::VelocityReport;
+using autoware_auto_vehicle_msgs::srv::ControlModeCommand;
 
 using geometry_msgs::msg::AccelWithCovarianceStamped;
 using geometry_msgs::msg::Pose;
@@ -66,12 +109,20 @@ private:
   rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr pub_tf_;
   rclcpp::Publisher<AccelWithCovarianceStamped>::SharedPtr pub_acc_;
   rclcpp::Publisher<Imu>::SharedPtr pub_imu_;
+  rclcpp::Publisher<VelocityReport>::SharedPtr pub_velocity_;
 
   rclcpp::TimerBase::SharedPtr mptimer;
 
   void publish_odometry(const Odometry & odometry);
   void publish_tf(const Odometry & odometry);
 
+    /**
+   * @brief publish velocity
+   * @param [in] velocity The velocity report to publish
+   */
+  void publish_velocity(const VelocityReport & velocity);
+
+
 private:
   std::string simulated_frame_id_;  //!< @brief simulated vehicle frame id
   std::string origin_frame_id_;     //!< @brief map frame_id

src/ros2/src/testpose/package.xml

@@ -10,6 +10,12 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+    <depend>autoware_auto_control_msgs</depend>
+  <depend>autoware_auto_mapping_msgs</depend>
+  <depend>autoware_auto_planning_msgs</depend>
+  <depend>autoware_auto_tf2</depend>
+  <depend>autoware_auto_vehicle_msgs</depend>
+
   <depend>geometry_msgs</depend>
   <depend>nav_msgs</depend>
   <depend>sensor_msgs</depend>

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

@@ -38,7 +38,7 @@ TestPose::TestPose() : Node("testpose_core")
   using std::placeholders::_1;
 
   simulated_frame_id_ = declare_parameter("simulated_frame_id", "base_link");
-  origin_frame_id_ = declare_parameter("origin_frame_id", "odom");
+  origin_frame_id_ = declare_parameter("origin_frame_id", "map");
 
   static constexpr std::size_t queue_size = 1;
   rclcpp::QoS durable_qos(queue_size);
@@ -52,6 +52,8 @@ TestPose::TestPose() : Node("testpose_core")
 pub_odom_ = create_publisher<Odometry>("/localization/kinematic_state", durable_qos);
 pub_tf_ = create_publisher<tf2_msgs::msg::TFMessage>("/tf", durable_qos);
 
+pub_velocity_ = create_publisher<VelocityReport>("/vehicle/status/velocity_status", durable_qos);
+
   mptimer= create_wall_timer(100ms,std::bind(&TestPose::callbackTimer, this));
 
 
@@ -177,6 +179,7 @@ Quaternion ToQuaternion(double yaw, double pitch, double roll) // yaw (Z), pitch
 
 void TestPose::callbackPose(geometry_msgs::msg::PoseStamped::SharedPtr pose_msg_ptr)
 {
+  (void)pose_msg_ptr;
   // TODO(YamatoAndo) check time stamp diff
   // TODO(YamatoAndo) check suddenly move
   // TODO(YamatoAndo) apply low pass filter
@@ -204,6 +207,7 @@ void TestPose::callbackTimer()
     static double rel_z = 0;
     static double vn = 0;
     static double ve = 0;
+   // rel_x+=0.01;
 
     msg.pose.pose.position.x = rel_x;
     msg.pose.pose.position.y = rel_y;
@@ -219,6 +223,12 @@ void TestPose::callbackTimer()
     publish_odometry(msg);
     publish_tf(msg);
 
+      autoware_auto_vehicle_msgs::msg::VelocityReport velocity;
+  velocity.longitudinal_velocity = 0;
+  velocity.lateral_velocity = 0.0F;
+  velocity.heading_rate = 0;
+  publish_velocity(velocity);
+
 
 }
 
@@ -253,3 +263,12 @@ void TestPose::publish_tf(const Odometry & odometry)
   pub_tf_->publish(tf_msg);
 }
 
+void TestPose::publish_velocity(const VelocityReport & velocity)
+{
+  VelocityReport msg = velocity;
+  msg.header.stamp = get_clock()->now();
+  msg.header.frame_id = simulated_frame_id_;
+  pub_velocity_->publish(msg);
+}
+
+