- added enable offset service

- added load offset service
2018-05-29 17:57:11 +09:00
parent b846058fdc
commit 037b96de6a
2 changed files with 87 additions and 11 deletions
--- a/robotis_controller/include/robotis_controller/robotis_controller.h
+++ b/robotis_controller/include/robotis_controller/robotis_controller.h
@ -64,6 +64,7 @@ private:

  bool            init_pose_loaded_;
  bool            is_timer_running_;
+  bool            is_offset_enabled_;
  bool            stop_timer_;
  pthread_t       timer_thread_;
  ControllerMode  controller_mode_;
@ -142,6 +143,8 @@ public:
  bool    getJointCtrlModuleService(robotis_controller_msgs::GetJointModule::Request &req, robotis_controller_msgs::GetJointModule::Response &res);
  bool    setJointCtrlModuleService(robotis_controller_msgs::SetJointModule::Request &req, robotis_controller_msgs::SetJointModule::Response &res);
  bool    setCtrlModuleService(robotis_controller_msgs::SetModule::Request &req, robotis_controller_msgs::SetModule::Response &res);
+  bool    enableOffsetService(robotis_controller_msgs::EnableOffset::Request &req, robotis_controller_msgs::EnableOffset::Response &res);
+  bool    loadOffsetService(robotis_controller_msgs::LoadOffset::Request &req, robotis_controller_msgs::LoadOffset::Response &res);

  void    gazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg);

--- a/robotis_controller/src/robotis_controller/robotis_controller.cpp
+++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp
@ -30,6 +30,7 @@ using namespace robotis_framework;

 RobotisController::RobotisController()
  : is_timer_running_(false),
+    is_offset_enabled_(true),
    stop_timer_(false),
    init_pose_loaded_(false),
    timer_thread_(0),
@ -142,7 +143,10 @@ void RobotisController::initializeSyncWrite()
        if ((dxl->present_position_item_ != 0) &&
            (dxl->bulk_read_items_[i]->item_name_ == dxl->present_position_item_->item_name_))
        {
-          dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(read_data) - dxl->dxl_state_->position_offset_;   // remove offset
+          if(is_offset_enabled_)
+            dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(read_data) - dxl->dxl_state_->position_offset_;   // remove offset
+          else
+            dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(read_data);
          dxl->dxl_state_->goal_position_ = dxl->dxl_state_->present_position_;

          port_to_sync_write_position_[dxl->port_name_]->addParam(dxl->id_, sync_write_data);
@ -664,6 +668,10 @@ void RobotisController::msgQueueThread()
                                                        &RobotisController::setJointCtrlModuleService, this);
  ros::ServiceServer set_module_server = ros_node.advertiseService("/robotis/set_present_ctrl_modules",
                                                        &RobotisController::setCtrlModuleService, this);
+  ros::ServiceServer enable_offset_server = ros_node.advertiseService("/robotis/enable_offset",
+                                                        &RobotisController::enableOffsetService, this);
+  ros::ServiceServer load_offset_server = ros_node.advertiseService("/robotis/load_offset",
+                                                        &RobotisController::loadOffsetService, this);

  ros::WallDuration duration(robot_->getControlCycle() / 1000.0);
  while(ros_node.ok())
@ -926,13 +934,23 @@ void RobotisController::process()

                // change dxl_state
                if (dxl->present_position_item_ != 0 && item->item_name_ == dxl->present_position_item_->item_name_)
-                  dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset
+                {
+                  if(is_offset_enabled_)
+                    dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset
+                  else
+                    dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(data);
+                }
                else if (dxl->present_velocity_item_ != 0 && item->item_name_ == dxl->present_velocity_item_->item_name_)
                  dxl->dxl_state_->present_velocity_ = dxl->convertValue2Velocity(data);
                else if (dxl->present_current_item_ != 0 && item->item_name_ == dxl->present_current_item_->item_name_)
                  dxl->dxl_state_->present_torque_ = dxl->convertValue2Torque(data);
                else if (dxl->goal_position_item_ != 0 && item->item_name_ == dxl->goal_position_item_->item_name_)
-                  dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset
+                {
+                  if(is_offset_enabled_)
+                    dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset
+                  else
+                    dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data);
+                }
                else if (dxl->goal_velocity_item_ != 0 && item->item_name_ == dxl->goal_velocity_item_->item_name_)
                  dxl->dxl_state_->goal_velocity_ = dxl->convertValue2Velocity(data);
                else if (dxl->goal_current_item_ != 0 && item->item_name_ == dxl->goal_current_item_->item_name_)
@ -1146,13 +1164,23 @@ void RobotisController::process()

                // change dxl_state
                if (dxl->present_position_item_ != 0 && item->item_name_ == dxl->present_position_item_->item_name_)
-                  dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset
+                {
+                  if(is_offset_enabled_)
+                    dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset
+                  else
+                    dxl->dxl_state_->present_position_ = dxl->convertValue2Radian(data);
+                }
                else if (dxl->present_velocity_item_ != 0 && item->item_name_ == dxl->present_velocity_item_->item_name_)
                  dxl->dxl_state_->present_velocity_ = dxl->convertValue2Velocity(data);
                else if (dxl->present_current_item_ != 0 && item->item_name_ == dxl->present_current_item_->item_name_)
                  dxl->dxl_state_->present_torque_ = dxl->convertValue2Torque(data);
                else if (dxl->goal_position_item_ != 0 && item->item_name_ == dxl->goal_position_item_->item_name_)
-                  dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset
+                {
+                  if(is_offset_enabled_)
+                    dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data) - dxl->dxl_state_->position_offset_; // remove offset
+                  else
+                    dxl->dxl_state_->goal_position_ = dxl->convertValue2Radian(data);
+                }
                else if (dxl->goal_velocity_item_ != 0 && item->item_name_ == dxl->goal_velocity_item_->item_name_)
                  dxl->dxl_state_->goal_velocity_ = dxl->convertValue2Velocity(data);
                else if (dxl->goal_current_item_ != 0 && item->item_name_ == dxl->goal_current_item_->item_name_)
@ -1255,7 +1283,12 @@ void RobotisController::process()
              if (gazebo_mode_ == false)
              {
                // add offset
-                uint32_t pos_data = dxl->convertRadian2Value(dxl_state->goal_position_ + dxl_state->position_offset_);
+                uint32_t pos_data;
+                if(is_offset_enabled_)
+                  pos_data= dxl->convertRadian2Value(dxl_state->goal_position_ + dxl_state->position_offset_);
+                else
+                  pos_data= dxl->convertRadian2Value(dxl_state->goal_position_);
+
                uint8_t sync_write_data[4] = { 0 };
                sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data));
                sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data));
@ -1653,7 +1686,12 @@ void RobotisController::setJointStatesCallback(const sensor_msgs::JointState::Co
      if (gazebo_mode_ == false)
      {
        // add offset
-        uint32_t pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_);
+        uint32_t pos_data;
+        if(is_offset_enabled_)
+          pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_);
+        else
+          pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_);
+
        uint8_t sync_write_data[4] = { 0 };
        sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data));
        sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data));
@ -1748,6 +1786,21 @@ bool RobotisController::setCtrlModuleService(robotis_controller_msgs::SetModule:

  set_module_thread_.join();

+  res.result = true;
+  return true;
+}
+
+bool RobotisController::enableOffsetService(robotis_controller_msgs::EnableOffset::Request &req, robotis_controller_msgs::EnableOffset::Response &res)
+{
+  is_offset_enabled_ = (bool)req.enable;
+  res.result = true;
+  return true;
+}
+
+bool RobotisController::loadOffsetService(robotis_controller_msgs::LoadOffset::Request &req, robotis_controller_msgs::LoadOffset::Response &res)
+{
+  loadOffset((std::string)req.file_path);
+  res.result = true;
  return true;
 }

@ -1820,7 +1873,12 @@ void RobotisController::setJointCtrlModuleThread(const robotis_controller_msgs::
      if(gazebo_mode_ == true)
        continue;

-      uint32_t _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_ + _dxl->dxl_state_->position_offset_);
+      uint32_t _pos_data;
+      if(is_offset_enabled_)
+        _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_ + _dxl->dxl_state_->position_offset_);
+      else
+        _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_);
+
      uint8_t _sync_write_data[4];
      _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data));
      _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data));
@ -1858,7 +1916,12 @@ void RobotisController::setJointCtrlModuleThread(const robotis_controller_msgs::

          if(_mode == PositionControl)
          {
-            uint32_t _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_ + _dxl->dxl_state_->position_offset_);
+            uint32_t _pos_data;
+            if(is_offset_enabled_)
+              _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_ + _dxl->dxl_state_->position_offset_);
+            else
+              _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_);
+
            uint8_t _sync_write_data[4];
            _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data));
            _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data));
@ -2024,7 +2087,12 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module)
      if (gazebo_mode_ == true)
        continue;

-      uint32_t pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_);
+      uint32_t pos_data;
+      if(is_offset_enabled_)
+        pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_);
+      else
+        pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_);
+
      uint8_t sync_write_data[4] = { 0 };
      sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data));
      sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data));
@ -2062,7 +2130,12 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module)

            if (mode == PositionControl)
            {
-              uint32_t pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_);
+              uint32_t pos_data;
+              if(is_offset_enabled_)
+                pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_ + dxl->dxl_state_->position_offset_);
+              else
+                pos_data = dxl->convertRadian2Value(dxl->dxl_state_->goal_position_);
+
              uint8_t sync_write_data[4] = { 0 };
              sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(pos_data));
              sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(pos_data));