them file geometry_msgs

geometry_msgs/include/geometry_msgs/Accel.h

@@ -0,0 +1,28 @@
+// # This expresses acceleration in free space broken into its linear and angular parts.
+// Vector3  linear
+// Vector3  angular
+
+#ifndef ACCEL_H
+#define ACCEL_H
+
+#include <geometry_msgs/Vector3.h>
+
+namespace geometry_msgs
+{
+
+struct Accel
+{
+    Vector3 linear;
+    Vector3 angular;
+
+    // Constructor mặc định
+    Accel() : linear(), angular() {}
+
+    // Constructor khởi tạo nhanh
+    Accel(const Vector3& linear_, const Vector3& angular_)
+        : linear(linear_), angular(angular_) {}
+};
+
+} // namespace geometry_msgs
+
+#endif // ACCEL_H

geometry_msgs/include/geometry_msgs/AccelStamped.h

@@ -0,0 +1,24 @@
+// # An accel with reference coordinate frame and timestamp
+// Header header
+// Accel accel
+
+#ifndef ACCEL_STAMPED_H
+#define ACCEL_STAMPED_H
+
+#include <std_msgs/Header.h>
+#include <geometry_msgs/Accel.h>
+
+namespace geometry_msgs
+{
+
+struct AccelStamped
+{
+    std_msgs::Header header;
+    Accel accel;
+    // Constructor mặc định
+    AccelStamped() = default;
+};
+
+} // namespace geometry_msgs
+
+#endif // ACCEL_STAMPED_H

geometry_msgs/include/geometry_msgs/AccelWithCovariance.h

@@ -0,0 +1,31 @@
+// # This expresses acceleration in free space with uncertainty.
+
+// Accel accel
+
+// # Row-major representation of the 6x6 covariance matrix
+// # The orientation parameters use a fixed-axis representation.
+// # In order, the parameters are:
+// # (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)
+// float64[36] covariance
+
+#ifndef ACCEL_WITH_COVARIANCE_H
+#define ACCEL_WITH_COVARIANCE_H
+
+#include <array>
+#include <geometry_msgs/Accel.h>
+
+namespace geometry_msgs
+{
+
+struct AccelWithCovariance
+{
+    Accel accel;
+    std::array<double, 36> covariance;
+    // Constructor mặc định
+    AccelWithCovariance() = default;
+};
+
+} // namespace geometry_msgs
+
+#endif // ACCEL_WITH_COVARIANCE_H
+

geometry_msgs/include/geometry_msgs/AccelWithCovarianceStamped.h

@@ -0,0 +1,24 @@
+// # This represents an estimated accel with reference coordinate frame and timestamp.
+// Header header
+// AccelWithCovariance accel
+
+#ifndef ACCEL_WITH_COVARIANCE_STAMPED_H
+#define ACCEL_WITH_COVARIANCE_STAMPED_H
+
+#include <std_msgs/Header.h>
+#include <geometry_msgs/AccelWithCovariance.h>
+
+namespace geometry_msgs
+{
+
+struct AccelWithCovarianceStamped
+{
+    std_msgs::Header header;
+    AccelWithCovariance accel;
+    // Constructor mặc định
+    AccelWithCovarianceStamped() = default;
+};
+
+} // namespace geometry_msgs
+
+#endif // ACCEL_WITH_COVARIANCE_STAMPED_H

geometry_msgs/include/geometry_msgs/Inertia.h

@@ -0,0 +1,40 @@
+// # Mass [kg]
+// float64 m
+
+// # Center of mass [m]
+// geometry_msgs/Vector3 com
+
+// # Inertia Tensor [kg-m^2]
+// #     | ixx ixy ixz |
+// # I = | ixy iyy iyz |
+// #     | ixz iyz izz |
+// float64 ixx
+// float64 ixy
+// float64 ixz
+// float64 iyy
+// float64 iyz
+// float64 izz
+
+#ifndef INERTIA_H
+#define INERTIA_H
+
+#include <geometry_msgs/Vector3.h>
+
+namespace geometry_msgs
+{
+
+struct Inertia
+{
+    double m;
+    Vector3 com;
+    double ixx;
+    double ixy;
+    double ixz;
+    double iyy;
+    double iyz;
+    double izz;
+};
+
+} // namespace geometry_msgs
+
+#endif // INERTIA_H

geometry_msgs/include/geometry_msgs/InertiaStamped.h

@@ -0,0 +1,23 @@
+// Header header
+// Inertia inertia
+
+#ifndef INERTIA_STAMPED_H
+#define INERTIA_STAMPED_H
+
+#include <std_msgs/Header.h>
+#include <geometry_msgs/Inertia.h>
+
+namespace geometry_msgs
+{
+
+struct InertiaStamped
+{
+    std_msgs::Header header;
+    Inertia inertia;
+    // Constructor mặc định
+    InertiaStamped() = default;
+};
+
+} // namespace geometry_msgs
+
+#endif // INERTIA_STAMPED_H

geometry_msgs/include/geometry_msgs/Pose2D.h

@@ -0,0 +1,31 @@
+// # Deprecated
+// # Please use the full 3D pose.
+
+// # In general our recommendation is to use a full 3D representation of everything and for 2D specific applications make the appropriate projections into the plane for their calculations but optimally will preserve the 3D information during processing.
+
+// # If we have parallel copies of 2D datatypes every UI and other pipeline will end up needing to have dual interfaces to plot everything. And you will end up with not being able to use 3D tools for 2D use cases even if they're completely valid, as you'd have to reimplement it with different inputs and outputs. It's not particularly hard to plot the 2D pose or compute the yaw error for the Pose message and there are already tools and libraries that can do this for you.
+
+
+// # This expresses a position and orientation on a 2D manifold.
+
+// float64 x
+// float64 y
+// float64 theta
+
+#ifndef POSE2D_H
+#define POSE2D_H
+
+namespace geometry_msgs
+{
+
+struct Pose2D
+{
+    double x;
+    double y;
+    double theta;
+};
+
+} // namespace geometry_msgs
+
+#endif // POSE2D_H
+

geometry_msgs/include/geometry_msgs/PoseArray.h

@@ -0,0 +1,28 @@
+// # An array of poses with a header for global reference.
+
+// Header header
+
+// Pose[] poses
+#ifndef POSE_ARRAY_H
+#define POSE_ARRAY_H
+
+#include <vector>
+#include <std_msgs/Header.h>
+#include <geometry_msgs/Pose.h>
+
+
+
+namespace geometry_msgs
+{
+
+struct PoseArray
+{
+    std_msgs::Header header;
+    std::vector<Pose> poses;
+
+    PoseArray() = default;
+};
+
+} // namespace geometry_msgs
+
+#endif // POSE_ARRAY_H

geometry_msgs/include/geometry_msgs/PoseStamped.h

@@ -0,0 +1,25 @@
+// # A Pose with reference coordinate frame and timestamp
+// Header header
+// Pose pose
+#ifndef POSE_STAMPED_H
+#define POSE_STAMPED_H
+
+#include <std_msgs/Header.h>
+#include <geometry_msgs/Pose.h>
+
+
+
+namespace geometry_msgs
+{
+
+struct PoseStamped
+{
+    std_msgs::Header header;
+    Pose pose;
+
+    PoseStamped() = default;
+};
+
+} // namespace geometry_msgs
+
+#endif // POSE_STAMPED_H

geometry_msgs/include/geometry_msgs/PoseWithCovariance.h

@@ -0,0 +1,31 @@
+// # This represents a pose in free space with uncertainty.
+
+// Pose pose
+
+// # Row-major representation of the 6x6 covariance matrix
+// # The orientation parameters use a fixed-axis representation.
+// # In order, the parameters are:
+// # (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)
+// float64[36] covariance
+
+#ifndef POSE_WITH_COVARIANCE_H
+#define POSE_WITH_COVARIANCE_H
+
+#include <array>
+#include <geometry_msgs/Pose.h>
+
+
+namespace geometry_msgs
+{
+
+struct PoseWithCovariance
+{
+    Pose pose;
+    std::array<double, 36> covariance;
+
+    PoseWithCovariance() = default;
+};
+
+} // namespace geometry_msgs
+
+#endif // POSE_WITH_COVARIANCE_H

geometry_msgs/include/geometry_msgs/PoseWithCovarianceStamped.h

@@ -0,0 +1,27 @@
+// # This expresses an estimated pose with a reference coordinate frame and timestamp
+
+// Header header
+// PoseWithCovariance pose
+
+#ifndef POSE_WITH_COVARIANCE_STAMPED_H
+#define POSE_WITH_COVARIANCE_STAMPED_H
+
+#include <std_msgs/Header.h>
+#include <geometry_msgs/PoseWithCovariance.h>
+
+
+
+namespace geometry_msgs
+{
+
+struct PoseWithCovarianceStamped
+{
+    std_msgs::Header header;
+    PoseWithCovariance pose;
+
+    PoseWithCovarianceStamped() = default;
+};
+
+} // namespace geometry_msgs
+
+#endif // POSE_WITH_COVARIANCE_STAMPED_H

geometry_msgs/include/geometry_msgs/QuaternionStamped.h

@@ -0,0 +1,25 @@
+// # This represents an orientation with reference coordinate frame and timestamp.
+
+// Header header
+// Quaternion quaternion
+
+#ifndef QUATERNION_STAMPED_H
+#define QUATERNION_STAMPED_H
+
+#include <std_msgs/Header.h>
+#include <geometry_msgs/Quaternion.h>
+
+namespace geometry_msgs
+{
+
+struct QuaternionStamped
+{
+    std_msgs::Header header;
+    Quaternion quaternion;
+    // Constructor mặc định
+    QuaternionStamped() = default;
+};
+
+} // namespace geometry_msgs
+
+#endif // QUATERNION_STAMPED_H

geometry_msgs/include/geometry_msgs/Twist.h

@@ -0,0 +1,28 @@
+// # This expresses velocity in free space broken into its linear and angular parts.
+// Vector3 linear
+// Vector3 angular
+
+#ifndef TWIST_H
+#define TWIST_H
+
+#include <geometry_msgs/Vector3.h>
+
+namespace geometry_msgs
+{
+
+struct Twist
+{
+    Vector3 linear;
+    Vector3 angular;
+
+    // Constructor mặc định
+    Twist() : linear(), angular() {}
+
+    // Constructor khởi tạo nhanh
+    Twist(const Vector3& linear_, const Vector3& angular_)
+        : linear(linear_), angular(angular_) {}
+};
+
+} // namespace geometry_msgs
+
+#endif // ACCEL_H

geometry_msgs/include/geometry_msgs/TwistStamped.h

@@ -0,0 +1,24 @@
+// # A twist with reference coordinate frame and timestamp
+// Header header
+// Twist twist
+
+#ifndef TWIST_STAMPED_H
+#define TWIST_STAMPED_H
+
+#include <std_msgs/Header.h>
+#include <geometry_msgs/Twist.h>
+
+namespace geometry_msgs
+{
+
+struct TwistStamped
+{
+    std_msgs::Header header;
+    Twist twist;
+    // Constructor mặc định
+    TwistStamped() = default;
+};
+
+} // namespace geometry_msgs
+
+#endif // TWIST_STAMPED_H

geometry_msgs/include/geometry_msgs/TwistWithCovariance.h

@@ -0,0 +1,31 @@
+// # This expresses velocity in free space with uncertainty.
+
+// Twist twist
+
+// # Row-major representation of the 6x6 covariance matrix
+// # The orientation parameters use a fixed-axis representation.
+// # In order, the parameters are:
+// # (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)
+// float64[36] covariance
+
+#ifndef TWIST_WITH_COVARIANCE_H
+#define TWIST_WITH_COVARIANCE_H
+
+#include <array>
+#include <geometry_msgs/Twist.h>
+
+
+namespace geometry_msgs
+{
+
+struct TwistWithCovariance
+{
+    Twist twist;
+    std::array<double, 36> covariance;
+
+    TwistWithCovariance() = default;
+};
+
+} // namespace geometry_msgs
+
+#endif // TWIST_WITH_COVARIANCE_H

geometry_msgs/include/geometry_msgs/TwistWithCovarianceStamped.h

@@ -0,0 +1,26 @@
+// # This represents an estimated twist with reference coordinate frame and timestamp.
+// Header header
+// TwistWithCovariance twist
+
+#ifndef TWIST_WITH_COVARIANCE_STAMPED_H
+#define TWIST_WITH_COVARIANCE_STAMPED_H
+
+#include <std_msgs/Header.h>
+#include <geometry_msgs/PoseWithCovariance.h>
+
+
+
+namespace geometry_msgs
+{
+
+struct TwistWithCovarianceStamped
+{
+    std_msgs::Header header;
+    TwistWithCovariance twist;
+
+    TwistWithCovarianceStamped() = default;
+};
+
+} // namespace geometry_msgs
+
+#endif // TWIST_WITH_COVARIANCE_STAMPED_H

geometry_msgs/include/geometry_msgs/Vector3Stamped.h

@@ -0,0 +1,24 @@
+// # This represents a Vector3 with reference coordinate frame and timestamp
+// Header header
+// Vector3 vector
+
+#ifndef VECTOR_3_STAMPED_H
+#define VECTOR_3_STAMPED_H
+
+#include <std_msgs/Header.h>
+#include <geometry_msgs/Vector3.h>
+
+namespace geometry_msgs
+{
+
+struct Vector3Stamped
+{
+    std_msgs::Header header;
+    Vector3 vector;
+    // Constructor mặc định
+    Vector3Stamped() = default;
+};
+
+} // namespace geometry_msgs
+
+#endif // VECTOR_3_STAMPED_H

geometry_msgs/include/geometry_msgs/Vector3Stamped.msg

@@ -1,3 +0,0 @@
-# This represents a Vector3 with reference coordinate frame and timestamp
-Header header
-Vector3 vector

geometry_msgs/include/geometry_msgs/Wrench.h

@@ -0,0 +1,24 @@
+// # This represents force in free space, separated into
+// # its linear and angular parts.
+// Vector3  force
+// Vector3  torque
+
+#ifndef WRENCH_H
+#define WRENCH_H
+
+#include <geometry_msgs/Vector3.h>
+
+namespace geometry_msgs
+{
+
+struct Wrench
+{
+    Vector3  force;
+    Vector3  torque;
+    // Constructor mặc định
+    Wrench() = default;
+};
+
+} // namespace geometry_msgs
+
+#endif // WRENCH_H

geometry_msgs/include/geometry_msgs/WrenchStamped.h

@@ -0,0 +1,24 @@
+// # A wrench with reference coordinate frame and timestamp
+// Header header
+// Wrench wrench
+
+#ifndef WRENCH_STAMPED_H
+#define WRENCH_STAMPED_H
+
+#include <std_msgs/Header.h>
+#include <geometry_msgs/Wrench.h>
+
+namespace geometry_msgs
+{
+
+struct WrenchStamped
+{
+    std_msgs::Header header;
+    Wrench wrench;
+    // Constructor mặc định
+    WrenchStamped() = default;
+};
+
+} // namespace geometry_msgs
+
+#endif // WRENCH_STAMPED_H

sensor_msgs/include/sensor_msgs/CameraInfo.h

@@ -1,33 +1,8 @@
-
-
-# Time of image acquisition, camera coordinate frame ID
-Header header    # Header timestamp should be acquisition time of image
-
-
-uint32 height
-uint32 width
-
-string distortion_model
-
-float64[] D
-
-float64[9]  K # 3x3 row-major matrix
-
-
-float64[9]  R # 3x3 row-major matrix
-
-float64[12] P # 3x4 row-major matrix
-
-uint32 binning_x
-uint32 binning_y
-
-RegionOfInterest roi
-
 #pragma once
 #include <string>
 #include <vector>
-#include "msg/Header.h"
+#include "std_msgs/Header.h"
-#include "msg/RegionOfInterest.h"
+#include "sensor_msgs/RegionOfInterest.h"
 
 namespace sensor_msgs
 {

sensor_msgs/include/sensor_msgs/CompressedImage.h

@@ -1,7 +1,7 @@
 #pragma once
 #include <string>
 #include <vector>
-#include "msg/Header.h"
+#include "std_msgs/Header.h"
 
 namespace sensor_msgs
 {

sensor_msgs/include/sensor_msgs/FluidPressure.h

@@ -1,5 +1,5 @@
 #pragma once
-#include "msg/Header.h"
+#include "std_msgs/Header.h"
 
 namespace sensor_msgs
 {

sensor_msgs/include/sensor_msgs/Illuminance.h

@@ -1,5 +1,5 @@
 #pragma once
-#include "msg/Header.h"
+#include "std_msgs/Header.h"
 
 namespace sensor_msgs
 {

sensor_msgs/include/sensor_msgs/Image.h

@@ -2,7 +2,7 @@
 #include <string>
 #include <vector>
 #include <cstdint>
-#include "msg/Header.h"
+#include "std_msgs/Header.h"
 
 namespace sensor_msgs
 {

sensor_msgs/include/sensor_msgs/Imu.h

@@ -1,6 +1,6 @@
 #pragma once
 #include <array>
-#include "msg/Header.h"
+#include "std_msgs/Header.h"
 #include "geometry_msgs/Quaternion.h"
 #include "geometry_msgs/Vector3.h"
 

sensor_msgs/include/sensor_msgs/JointState.h

@@ -1,7 +1,7 @@
 #pragma once
 #include <string>
 #include <vector>
-#include "msg/Header.h"
+#include "std_msgs/Header.h"
 
 namespace sensor_msgs
 {

sensor_msgs/include/sensor_msgs/Joy.h

@@ -1,6 +1,6 @@
 #pragma once
 #include <vector>
-#include "msg/Header.h"
+#include "std_msgs/Header.h"
 
 namespace sensor_msgs
 {

sensor_msgs/include/sensor_msgs/MultiEchoLaserScan.h

@@ -2,8 +2,8 @@
 #define MULTI_ECHO_LASER_SCAN_H
 
 #include <vector>
-#include "msg/Header.h"
+#include "std_msgs/Header.h"
-#include "msg/LaserEcho.h"   // Định nghĩa struct LaserEcho (float32[] echoes)
+#include "sensor_msgs/LaserEcho.h"   // Định nghĩa struct LaserEcho (float32[] echoes)
 
 namespace sensor_msgs
 {

sensor_msgs/include/sensor_msgs/Range.h

@@ -2,7 +2,7 @@
 #define RANGE_H
 
 #include <cstdint>
-#include "Header.h"  // Header tương tự std_msgs/Header
+#include "std_msgs/Header.h"  // Header tương tự std_msgs/Header
 
 namespace sensor_msgs
 {

sensor_msgs/include/sensor_msgs/RelativeHumidity.h

@@ -2,7 +2,7 @@
 #define RELATIVE_HUMIDITY_H
 
 #include <cstdint>
-#include "msg/Header.h"  // Giả định bạn đã có struct Header tương tự ROS std_msgs/Header
+#include "std_msgs/Header.h"  // Giả định bạn đã có struct Header tương tự ROS std_msgs/Header
 namespace sensor_msgs
 {
 

sensor_msgs/include/sensor_msgs/Temperature.h

@@ -2,7 +2,7 @@
 #define TEMPERATURE_H
 
 #include <cstdint>
-#include "msg/Header.h"  // Định nghĩa struct Header tương tự std_msgs/Header
+#include "std_msgs/Header.h"  // Định nghĩa struct Header tương tự std_msgs/Header
 namespace sensor_msgs
 {
 

sensor_msgs/include/sensor_msgs/TimeReference.h

@@ -2,7 +2,7 @@
 #define TIMEREFERENCE_H
 
 #include <string>
-#include "msg/Header.h"   // Định nghĩa struct Header tương tự std_msgs/Header
+#include "std_msgs/Header.h"   // Định nghĩa struct Header tương tự std_msgs/Header
 namespace sensor_msgs
 {
 

sensor_msgs/test/main.cpp

@@ -40,20 +40,20 @@ int main()
   getParams();
   sensor_msgs::BatteryState battery;
 
-  battery.header = std_msgs::Header::now("battery_frame");
+//   battery.header = std_msgs::Header::now("battery_frame");
-  battery.voltage = 12.5f;
+//   battery.voltage = 12.5f;
-  battery.current = -1.2f;
+//   battery.current = -1.2f;
-  battery.percentage = 0.85f;
+//   battery.percentage = 0.85f;
-  battery.power_supply_status = sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_DISCHARGING;
+//   battery.power_supply_status = sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_DISCHARGING;
 
-  battery.cell_voltage = {4.15f, 4.18f, 4.12f};
+//   battery.cell_voltage = {4.15f, 4.18f, 4.12f};
-  battery.location = "slot_1";
+//   battery.location = "slot_1";
-  battery.serial_number = "SN12345";
+//   battery.serial_number = "SN12345";
 
-  std::cout << "Battery header timestamp: " << battery.header.stamp << "\n";
+//   std::cout << "Battery header timestamp: " << battery.header.stamp << "\n";
-  std::cout << "Battery voltage: " << battery.voltage << " V\n";
+//   std::cout << "Battery voltage: " << battery.voltage << " V\n";
-  std::cout << "Status: " << static_cast<int>(battery.power_supply_status) << "\n";
+//   std::cout << "Status: " << static_cast<int>(battery.power_supply_status) << "\n";
-  std::cout << "Serial Number: " << battery.serial_number << "\n";
+//   std::cout << "Serial Number: " << battery.serial_number << "\n";
 
   sensor_msgs::JoyFeedbackArray feedback_array;
   feedback_array.array.push_back(sensor_msgs::JoyFeedback{sensor_msgs::JoyFeedback::TYPE_BUZZER, 0, 5000.0f});
@@ -62,14 +62,14 @@ int main()
   std::cout << "First feedback intensity: " << feedback_array.array[0].intensity << "\n";
 
   sensor_msgs::PointCloud2 cloud;
-  cloud.header = std_msgs::Header::now("pointcloud_frame");
+//   cloud.header = std_msgs::Header::now("pointcloud_frame");
-  cloud.width = 100;
+//   cloud.width = 100;
-  cloud.height = 1;
+//   cloud.height = 1;
 
-  std::cout << "PointCloud2 header frame_id: " << cloud.header.frame_id << "\n";
+//   std::cout << "PointCloud2 header frame_id: " << cloud.header.frame_id << "\n";
-  std::cout << "PointCloud2 header timestamp: " << cloud.header.stamp << "\n";
+//   std::cout << "PointCloud2 header timestamp: " << cloud.header.stamp << "\n";
-  std::cout << "PointCloud2 width: " << cloud.width << "\n";
+//   std::cout << "PointCloud2 width: " << cloud.width << "\n";
-  std::cout << "PointCloud2 height: " << cloud.height << "\n";
+//   std::cout << "PointCloud2 height: " << cloud.height << "\n";
 
   return 0;
 }

std_msgs/CMakeLists.txt

@@ -3,5 +3,11 @@ project(std_msgs)
 
 set(CMAKE_CXX_STANDARD 17)
 
+if (NOT TARGET robot_time)
+    add_subdirectory(${CMAKE_SOURCE_DIR}/../robot_time ${CMAKE_BINARY_DIR}/robot_time_build)
+endif()
+
 add_library(std_msgs INTERFACE)
-target_include_directories(std_msgs INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}/include)
+target_include_directories(std_msgs INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}/include 
+                        ${CMAKE_SOURCE_DIR}/../robot_time/include)
+target_link_libraries(std_msgs INTERFACE robot_time)

std_msgs/include/std_msgs/Header.h

@@ -2,15 +2,15 @@
 #define STD_MSGS_HEADER_H
 
 #include <string>
-#include <chrono>
 #include <cstdint>
+#include <robot/time.h>
 
 namespace std_msgs {
 
 struct Header
 {
   uint32_t seq = 0;  // số thứ tự message
-  double stamp = 0.0; // thời gian theo giây (Unix time)
+  robot::Time stamp = robot::Time(0); // thời gian theo giây (Unix time)
   std::string frame_id;
 
   Header() = default;
@@ -18,8 +18,7 @@ struct Header
   // Hàm tạo nhanh header với timestamp hiện tại
   static Header now(const std::string& frame = "")
   {
-    using namespace std::chrono;
+    robot::Time time_now = robot::Time::now();
-    double time_now = duration_cast<duration<double>>(system_clock::now().time_since_epoch()).count();
     Header h;
     h.seq = 0;
     h.stamp = time_now;