Problem with joint position update with ros controller manager

edit

Hi All,

I am working on a robot with 9 joints and I am controlling them using ros control framework. Initially, I used to update all velocity commands in a for loop in the write () method and update all joint position in another for loop inside read() method. This was having slow loop rate.

So, I decided to spawn separate threads for each joint for updating the command and joint position for each joint. This has improved the loop rate a lot. But after the new implementation, I am getting zero value while calling getPosition() occasionally, even though the actual position is not zero inside read() method. It seems like the controlle_manager.update() is failing to update the position.

Here is the code snippets.

Node :

while (ros::ok()) {
    // Update time variables
    if (!clock_gettime(CLOCK_REALTIME, &ts)) {
      now.sec = static_cast<uint32_t>(ts.tv_sec);
      now.nsec = static_cast<uint32_t>(ts.tv_nsec);
      period = now - last;
      last = now;
    } else {
      ROS_FATAL("Failed to poll realtime clock!");
      break;
    }
    mercury_hw.read(now, period);
    manager.update(now, period);
    mercury_hw.write(now, period);
    updater.update();
    rate.sleep();
  }
  spinner.stop();
  ros::shutdown();
  return 0;

********Here is the read () and write () methods********

void MercuryHW::read(const ros::Time& time, const ros::Duration& period) {
  std::vector<std::thread> read_vector;
  for (auto& node : dual_channel_map_) {
    read_vector.emplace_back(
        std::thread(&MercuryHW::feedback, this, std::ref(node.second)));
  }
  for (auto it = begin(read_vector); it != end(read_vector); ++it) {
    if (it->joinable()) it->join();
  }

}

void MercuryHW::write(const ros::Time& time, const ros::Duration& period) {

  std::vector<std::thread> write_vector;
  for (auto& node : dual_channel_map_) {
    read_vector.emplace_back(
        std::thread(&MercuryHW::command, this, std::ref(node.second)));
  }
  for (auto it = begin(read_vector); it != end(read_vector); ++it) {
    if (it->joinable()) it->join();
  }

}

The feedback() method read the joints position from the motor controller. And command() method writes the velocity to the motor controller.