Cleanup old code and uncommented tf odom publishing

Ensmasteel · Apr 26, 2024 · 5ffa0d4 · 5ffa0d4
1 parent abcbf40
commit 5ffa0d4
Show file tree

Hide file tree

Showing 4 changed files with 16 additions and 17 deletions.
diff --git a/src/ezbot_robot/config/omnidirectional_controller.yaml b/src/ezbot_robot/config/omnidirectional_controller.yaml
@@ -19,7 +19,6 @@ omnidirectional_controller:
 
     robot_radius: 0.25
     wheel_radius: 0.06
-    gamma: 30.0 # angle between body fixed y axis and the normal of wheel3
 
     publish_rate: 50.0
     odom_frame_id: odom

diff --git a/src/ezbot_simulation/launch/launch_sim.launch.py b/src/ezbot_simulation/launch/launch_sim.launch.py
@@ -80,6 +80,6 @@ def generate_launch_description():
         spawn_entity,
         rsp2,
         spawn_entity2,
-        gazebo,
-        joystick
+        gazebo
+        #joystick
     ])
diff --git a/...irectional_controllers/include/omnidirectional_controllers/omnidirectional_controller.hpp b/...irectional_controllers/include/omnidirectional_controllers/omnidirectional_controller.hpp
@@ -138,8 +138,6 @@ class OmnidirectionalController : public controller_interface::ControllerInterfa
   void velocityCommandStampedCallback(const geometry_msgs::msg::TwistStamped::SharedPtr cmd_vel);
   void velocityCommandUnstampedCallback(const geometry_msgs::msg::Twist::SharedPtr cmd_vel);
   geometry_msgs::msg::TwistStamped::SharedPtr cmd_vel_;
-  double cos_gamma{0};
-  double sin_gamma{0};
 
   std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_;
 };

diff --git a/src/omnidirectional_controllers/src/omnidirectional_controller.cpp b/src/omnidirectional_controllers/src/omnidirectional_controller.cpp
@@ -65,7 +65,6 @@ CallbackReturn OmnidirectionalController::on_init() {
 
     auto_declare<double>("robot_radius", robot_params_.robot_radius);
     auto_declare<double>("wheel_radius", robot_params_.wheel_radius);
-    auto_declare<double>("gamma", robot_params_.gamma);
 
     auto_declare<std::string>("odom_frame_id", odom_params_.odom_frame_id);
     auto_declare<std::string>("base_frame_id", odom_params_.base_frame_id);
@@ -130,8 +129,7 @@ CallbackReturn OmnidirectionalController::on_configure(
 
   robot_params_.robot_radius = node_->get_parameter("robot_radius").as_double();
   robot_params_.wheel_radius = node_->get_parameter("wheel_radius").as_double();
-  robot_params_.gamma = node_->get_parameter("gamma").as_double();
-  robot_params_.gamma = DEG2RAD(robot_params_.gamma);
+
 
   omni_robot_kinematics_.setRobotParams(robot_params_);
   odometry_.setRobotParams(robot_params_);
@@ -477,15 +475,19 @@ controller_interface::return_type OmnidirectionalController::update(
     odometry_publisher_->publish(odometry_message_);
   }
 
-  // if (odom_params_.enable_odom_tf) {
-  //   odometry_transform_message_.header.stamp = current_time;
-  //   odometry_transform_message_.transform.translation.x = odometry_.getPose().x;
-  //   odometry_transform_message_.transform.translation.y = odometry_.getPose().y;
-  //   odometry_transform_message_.transform.rotation.x = orientation.x();
-  //   odometry_transform_message_.transform.rotation.y = orientation.y();
-  //   odometry_transform_message_.transform.rotation.z = orientation.z();
-  //   odometry_transform_message_.transform.rotation.w = orientation.w();
-  // }
+  if (odom_params_.enable_odom_tf) {
+
+    auto & transform = odometry_transform_message_.transforms.front();
+    transform.header.stamp = current_time;
+    transform.transform.translation.x = odometry_.getPose().x;
+    transform.transform.translation.y = odometry_.getPose().y;
+    transform.transform.rotation.x = orientation.x();
+    transform.transform.rotation.y = orientation.y();
+    transform.transform.rotation.z = orientation.z();
+    transform.transform.rotation.w = orientation.w();
+
+    odometry_transform_publisher_->publish(odometry_transform_message_);
+  }
 
   auto & last_command = previous_commands_.back().twist;
   auto & second_to_last_command = previous_commands_.front().twist;