Change micom.proto interface

- separate angular velocity and linear velocity info Bug fix in odometry calculation for Wheel TFs
lge-ros2 · Aug 25, 2020 · 95c1391 · 95c1391
1 parent 06ba0fb
commit 95c1391
Show file tree

Hide file tree

Showing 3 changed files with 150 additions and 151 deletions.
diff --git a/driver_sim/msgs/micom.proto b/driver_sim/msgs/micom.proto
@@ -29,8 +29,10 @@ message Micom
 
   message Odometry
   {
-    required double speed_left   = 1 [default = 0];
-    required double speed_right  = 2 [default = 0];
+    required double angular_velocity_left   = 1 [default = 0];
+    required double angular_velocity_right  = 2 [default = 0];
+    optional double linear_velocity_left    = 3 [default = 0];
+    optional double linear_velocity_right   = 4 [default = 0];
   }
 
   message AccGyro

diff --git a/micom_driver_sim/include/micom_driver_sim/MicomDriverSim.hpp b/micom_driver_sim/include/micom_driver_sim/MicomDriverSim.hpp
@@ -41,8 +41,7 @@ class MicomDriverSim : public DriverSim
 
   std::string MakeControlMessage(const geometry_msgs::msg::Twist::SharedPtr msg) const;
 
-  bool CalculateOdometry(const rclcpp::Duration duration,
-      const double _wheel_left, const double _wheel_right, const double _theta);
+  bool CalculateOdometry(const rclcpp::Duration duration, const double _wheel_angular_vel_left, const double _wheel_angular_vel_right, const double _theta);
 
   void UpdateOdom();
   void UpdateImu();
@@ -60,30 +59,30 @@ class MicomDriverSim : public DriverSim
   bool m_use_sub;
 
   // Micom msgs
-  gazebo::msgs::Micom m_pbBufMicom;
+  gazebo::msgs::Micom m_pbBufMicom_;
 
-  std::array<double, 3> odom_pose;
-  std::array<double, 3> odom_vel;
-  std::array<double, 2> last_rad;
+  std::array<double, 3> odom_pose_;
+  std::array<double, 3> odom_vel_;
+  std::array<double, 2> last_rad_;
 
   // IMU msgs
-  sensor_msgs::msg::Imu msg_imu;
+  sensor_msgs::msg::Imu msg_imu_;
 
   // Odometry msgs
-  geometry_msgs::msg::TransformStamped odom_tf;
-  geometry_msgs::msg::TransformStamped wheel_left_tf;
-  geometry_msgs::msg::TransformStamped wheel_right_tf;
-  nav_msgs::msg::Odometry msg_odom;
+  geometry_msgs::msg::TransformStamped odom_tf_;
+  geometry_msgs::msg::TransformStamped wheel_left_tf_;
+  geometry_msgs::msg::TransformStamped wheel_right_tf_;
+  nav_msgs::msg::Odometry msg_odom_;
 
   // Battery
-  sensor_msgs::msg::BatteryState msg_battery;
+  sensor_msgs::msg::BatteryState msg_battery_;
 
   // ROS2 micom publisher
-  rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pubImu;
-  rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pubOdometry;
-  rclcpp::Publisher<sensor_msgs::msg::BatteryState>::SharedPtr pubBatteryState;
+  rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pubImu_;
+  rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pubOdometry_;
+  rclcpp::Publisher<sensor_msgs::msg::BatteryState>::SharedPtr pubBatteryState_;
 
   // wheel command subscriber
-  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr subMicom;
+  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr subMicom_;
 };
 #endif