- gazebo bug fixed (when ctrl_module_name_ == "none")

ROBOTIS-GIT · Dec 20, 2018 · edde9d2 · edde9d2
1 parent 7b7250c
commit edde9d2
Showing 1 changed file with 15 additions and 2 deletions.
diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp
@@ -1097,13 +1097,26 @@ void RobotisController::process()
     }
     else if (gazebo_mode_ == true)
     {
+      std_msgs::Float64 joint_msg;
+
+      for (auto& dxl_it : robot_->dxls_)
+      {
+        std::string     joint_name  = dxl_it.first;
+        Dynamixel      *dxl         = dxl_it.second;
+        DynamixelState *dxl_state   = dxl_it.second->dxl_state_;
+
+        if (dxl->ctrl_module_name_ == "none")
+        {
+          joint_msg.data = dxl_state->goal_position_;
+          gazebo_joint_position_pub_[joint_name].publish(joint_msg);
+        }
+      }
+
       for (auto module_it = motion_modules_.begin(); module_it != motion_modules_.end(); module_it++)
       {
         if ((*module_it)->getModuleEnable() == false)
           continue;
 
-        std_msgs::Float64 joint_msg;
-
         for (auto& dxl_it : robot_->dxls_)
         {
           std::string     joint_name  = dxl_it.first;