Ask Your Question
1

Problem with joint position update with ros controller manager

asked 2019-04-30 08:39:19 -0600

xdaco gravatar image

updated 2019-04-30 08:39:50 -0600

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.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2019-06-12 07:40:04 -0600

xdaco gravatar image

This problem was solved implementing conditional variable and mutex.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

3 followers

Stats

Asked: 2019-04-30 08:39:19 -0600

Seen: 103 times

Last updated: Jun 12