Problem with joint position update with ros controller manager
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.