change testpose. not complete.

src/ros2/src/testpose/CMakeLists.txt

@@ -15,6 +15,8 @@ find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED) 
+find_package(sensor_msgs REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 
 include_directories(
@@ -29,7 +31,9 @@ add_executable(testpose
   src/testpose_core.cpp
 )
 
-ament_target_dependencies(testpose rclcpp std_msgs geometry_msgs tf2_geometry_msgs)
+ament_target_dependencies(testpose rclcpp std_msgs geometry_msgs tf2_geometry_msgs nav_msgs sensor_msgs)
+
+
 
 install(TARGETS
 testpose

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 "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 <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <string>
+
+
+
+using geometry_msgs::msg::AccelWithCovarianceStamped;
+using geometry_msgs::msg::Pose;
+using geometry_msgs::msg::PoseStamped;
+using geometry_msgs::msg::PoseWithCovarianceStamped;
+using geometry_msgs::msg::TransformStamped;
+using geometry_msgs::msg::Twist;
+using geometry_msgs::msg::TwistStamped;
+using nav_msgs::msg::Odometry;
+using sensor_msgs::msg::Imu;
+
 class TestPose : public rclcpp::Node
 {
 public:
@@ -28,11 +55,28 @@ public:
 
 private:
   void callbackPose(geometry_msgs::msg::PoseStamped::SharedPtr pose_msg_ptr);
+  void callbackTimer();
 
   rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
 
   rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_pub_;
 
+
+  rclcpp::Publisher<Odometry>::SharedPtr pub_odom_;
+  rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr pub_tf_;
+  rclcpp::Publisher<AccelWithCovarianceStamped>::SharedPtr pub_acc_;
+  rclcpp::Publisher<Imu>::SharedPtr pub_imu_;
+
+  rclcpp::TimerBase::SharedPtr mptimer;
+
+  void publish_odometry(const Odometry & odometry);
+  void publish_tf(const Odometry & odometry);
+
+private:
+  std::string simulated_frame_id_;  //!< @brief simulated vehicle frame id
+  std::string origin_frame_id_;     //!< @brief map frame_id
+
+
 };
 
 #endif  // TESTPOSE__TESTPOSE_CORE_HPP_

src/ros2/src/testpose/package.xml

@@ -11,7 +11,12 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>sensor_msgs</depend>
   <depend>rclcpp</depend>
 
 
+ <export>
+   <build_type>ament_cmake</build_type>
+ </export>
 </package>

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

@@ -24,10 +24,22 @@
 #include <cstddef>
 #include <functional>
 
+#include <time.h>
+#include <sstream>
+
+#include <iostream>
+
+using namespace std;
+
+
+
 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");
+
   static constexpr std::size_t queue_size = 1;
   rclcpp::QoS durable_qos(queue_size);
   durable_qos.transient_local();
@@ -36,6 +48,13 @@ TestPose::TestPose() : Node("testpose_core")
   // Note: this callback publishes topics above
   pose_sub_ = create_subscription<geometry_msgs::msg::PoseStamped>(
     "pose", queue_size, std::bind(&TestPose::callbackPose, this, _1));
+
+pub_odom_ = create_publisher<Odometry>("/localization/kinematic_state", durable_qos);
+pub_tf_ = create_publisher<tf2_msgs::msg::TFMessage>("/tf", durable_qos);
+
+  mptimer= create_wall_timer(100ms,std::bind(&TestPose::callbackTimer, this));
+
+
 }
 
 double calcDiffForRadian(const double lhs_rad, const double rhs_rad)
@@ -103,6 +122,59 @@ geometry_msgs::msg::TwistStamped calcTwist(
   return twist;
 }
 
+struct Quaternion {
+    double w, x, y, z;
+};
+
+struct EulerAngles {
+    double roll, pitch, yaw;
+};
+
+EulerAngles ToEulerAngles(Quaternion q) {
+    EulerAngles angles;
+
+    // roll (x-axis rotation)
+    double sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
+    double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
+    angles.roll = std::atan2(sinr_cosp, cosr_cosp);
+
+    // pitch (y-axis rotation)
+    double sinp = 2 * (q.w * q.y - q.z * q.x);
+    if (std::abs(sinp) >= 1)
+        angles.pitch = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range
+    else
+        angles.pitch = std::asin(sinp);
+
+    // yaw (z-axis rotation)
+    double siny_cosp = 2 * (q.w * q.z + q.x * q.y);
+    double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
+    angles.yaw = std::atan2(siny_cosp, cosy_cosp);
+
+    return angles;
+}
+
+
+Quaternion ToQuaternion(double yaw, double pitch, double roll) // yaw (Z), pitch (Y), roll (X)
+{
+    // Abbreviations for the various angular functions
+    double cy = cos(yaw * 0.5);
+    double sy = sin(yaw * 0.5);
+    double cp = cos(pitch * 0.5);
+    double sp = sin(pitch * 0.5);
+    double cr = cos(roll * 0.5);
+    double sr = sin(roll * 0.5);
+
+    Quaternion q;
+    q.w = cy * cp * cr + sy * sp * sr;
+    q.x = cy * cp * sr - sy * sp * cr;
+    q.y = sy * cp * sr + cy * sp * cr;
+    q.z = sy * cp * cr - cy * sp * sr;
+
+    return q;
+}
+
+//原文链接：https://blog.csdn.net/xiaoma_bk/article/details/79082629
+
 void TestPose::callbackPose(geometry_msgs::msg::PoseStamped::SharedPtr pose_msg_ptr)
 {
   // TODO(YamatoAndo) check time stamp diff
@@ -111,3 +183,73 @@ void TestPose::callbackPose(geometry_msgs::msg::PoseStamped::SharedPtr pose_msg_
 
 
 }
+
+void TestPose::callbackTimer()
+{
+  std::cout<<" testpose. "<<std::endl;
+
+   double pitch,roll,yaw;
+    pitch = 0;
+    roll = 0;
+
+    yaw = 0;
+    Quaternion quat = ToQuaternion(yaw,pitch,roll);
+
+    nav_msgs::msg::Odometry msg;
+
+
+
+    static double rel_x = 3705;
+    static double rel_y = 73765;
+    static double rel_z = 0;
+    static double vn = 0;
+    static double ve = 0;
+
+    msg.pose.pose.position.x = rel_x;
+    msg.pose.pose.position.y = rel_y;
+    msg.pose.pose.position.z = rel_z;
+
+    msg.pose.pose.orientation.x = quat.x;
+    msg.pose.pose.orientation.y = quat.y;
+    msg.pose.pose.orientation.z = quat.z;
+    msg.pose.pose.orientation.w = quat.w;
+
+    msg.twist.twist.linear.x = sqrt(pow(vn,2)+pow(ve,2));
+
+    publish_odometry(msg);
+    publish_tf(msg);
+
+
+}
+
+void TestPose::publish_odometry(const Odometry & odometry)
+{
+  static int frame_id = 0;
+  frame_id++;
+  Odometry msg = odometry;
+  msg.header.frame_id = origin_frame_id_;
+  msg.header.stamp = get_clock()->now();
+  msg.child_frame_id = simulated_frame_id_;
+  pub_odom_->publish(msg);
+
+
+}
+
+void TestPose::publish_tf(const Odometry & odometry)
+{
+    static int frame_id = 0;
+  frame_id++;
+  TransformStamped tf;
+  tf.header.stamp = get_clock()->now();
+  tf.header.frame_id = origin_frame_id_;
+  tf.child_frame_id = simulated_frame_id_;
+  tf.transform.translation.x = odometry.pose.pose.position.x;
+  tf.transform.translation.y = odometry.pose.pose.position.y;
+  tf.transform.translation.z = odometry.pose.pose.position.z;
+  tf.transform.rotation = odometry.pose.pose.orientation;
+
+  tf2_msgs::msg::TFMessage tf_msg{};
+  tf_msg.transforms.emplace_back(std::move(tf));
+  pub_tf_->publish(tf_msg);
+}
+