Skip to content

change read mmethod with bulk method and model abstraction #26

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Closed
raess1 wants to merge 1 commit into from
Closed

change read mmethod with bulk method and model abstraction #26

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
78 changes: 42 additions & 36 deletions include/dynamixel_control_hw/hardware_interface.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -274,50 +274,56 @@ namespace dynamixel {
void DynamixelHardwareInterface<Protocol>::read(const ros::Time& time,
const ros::Duration& elapsed_time)
{
for (unsigned i = 0; i < _servos.size(); i++) {
int n = 0;
try {

std::vector<id_t> _ids_vector;
for (unsigned i = 0; i < _servos.size(); i++) {
_ids_vector.push_back(_servos[i]->id());
}
_dynamixel_controller.send(_servos[0]->bulk_read_position_angle(_ids_vector));

dynamixel::StatusPacket<Protocol> status;
try {
// current position
_dynamixel_controller.send(_servos[i]->get_present_position_angle());
for (unsigned i = 0; i < _servos.size(); i++) {
_dynamixel_controller.recv(status);
if (status.valid()) {
try {
_joint_angles[i] = _servos[i]->parse_present_position_angle(status);
n = n + 1;
}
catch (dynamixel::errors::Error& e) {
ROS_ERROR_STREAM("Unpack exception while getting "
<< _dynamixel_map[_servos[n]->id()] << "'s position\n"
<< e.msg());
}
}
}
catch (dynamixel::errors::Error& e) {
ROS_ERROR_STREAM("Caught a Dynamixel exception while getting "
<< _dynamixel_map[_servos[i]->id()] << "'s position\n"
<< e.msg());
}
catch (dynamixel::errors::Error& e) {
ROS_ERROR_STREAM("Caught a Dynamixel exception while getting "
<< _dynamixel_map[_servos[n]->id()] << "'s position\n"
<< e.msg());
}
for (unsigned i = 0; i < _servos.size(); i++) {
// Invert the orientation, if configured
typename std::unordered_map<id_t, bool>::iterator invert_iterator = _invert.find(_servos[i]->id());
if (invert_iterator != _invert.end()) {
_joint_angles[i] = 2 * M_PI - _joint_angles[i];
}
if (status.valid()) {
try {
_joint_angles[i] = _servos[i]->parse_present_position_angle(status);
}
catch (dynamixel::errors::Error& e) {
ROS_ERROR_STREAM("Unpack exception while getting "
<< _dynamixel_map[_servos[i]->id()] << "'s position\n"
<< e.msg());
}

// Invert the orientation, if configured
typename std::unordered_map<id_t, bool>::iterator
invert_iterator
= _invert.find(_servos[i]->id());
if (invert_iterator != _invert.end()) {
_joint_angles[i] = 2 * M_PI - _joint_angles[i];
}

// Apply angle correction to joint, if any
typename std::unordered_map<id_t, double>::iterator
dynamixel_corrections_iterator
= _dynamixel_corrections.find(_servos[i]->id());
if (dynamixel_corrections_iterator != _dynamixel_corrections.end()) {
_joint_angles[i] -= dynamixel_corrections_iterator->second;
}
// Apply angle correction to joint, if any
typename std::unordered_map<id_t, double>::iterator dynamixel_corrections_iterator = _dynamixel_corrections.find(_servos[i]->id());
if (dynamixel_corrections_iterator != _dynamixel_corrections.end()) {
_joint_angles[i] -= dynamixel_corrections_iterator->second;
}

else {
ROS_WARN_STREAM("Did not receive any data when reading "
<< _dynamixel_map[_servos[i]->id()] << "'s position");
}

dynamixel::StatusPacket<Protocol> status_speed;
}
dynamixel::StatusPacket<Protocol> status_speed;
for (unsigned i = 0; i < _servos.size(); i++) {
try {
// current speed
_dynamixel_controller.send(_servos[i]->get_present_speed());
Expand Down Expand Up @@ -350,8 +356,8 @@ namespace dynamixel {
ROS_WARN_STREAM("Did not receive any data when reading "
<< _dynamixel_map[_servos[i]->id()] << "'s velocity");
}
}
}
} // namespace dynamixel
} // namespace dynamixel

template <class Protocol>
void DynamixelHardwareInterface<Protocol>::write(const ros::Time& time,
Expand Down
4 changes: 2 additions & 2 deletions package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,8 +4,8 @@
<version>0.1.0</version>
<description>An interface to the Dynamixel actuators for ROS control</description>

<maintainer email="dorian.goepp@inria.fr">Dorian Goepp</maintainer>

<maintainer email="pierre.desreumaux@inria.fr">Pierre Desreumaux</maintainer>
<author>Dorian Goepp</author>
<license>CeCILL</license>

<buildtool_depend>catkin</buildtool_depend>
Expand Down