in robots/LoCoBot/locobot_control/src/locobot_controller.cpp [815:990]
void LoCoBotController::publishCallback(const ros::TimerEvent &) {
#ifdef DEBUG
static double priv_pub_secs =ros::Time::now().toSec();
#endif
joint_state_msg_.header.stamp = ros::Time::now();
joint_state_msg_.name.clear();
joint_state_msg_.position.clear();
joint_state_msg_.velocity.clear();
joint_state_msg_.effort.clear();
std::stringstream joint_name;
double position = 0.0;
double velocity = 0.0;
double effort = 0.0;
std::string arm_joint_names[5] = {"joint_1", "joint_2", "joint_3", "joint_4", "joint_5"};
// STEP 1 - Populate joint_state message with arm motor readings
if (use_group_["arm"]) {
for (int i = 0; i < 5; i++) {
std::string motor = arm_joint_names[i];
int motor_id = dynamixel_name_2ids_[motor];
int state_id = motor_id_2state_list_id_[motor_id];
if (motor_id == 3) {
// Shadow motor for joint 2 - ignore
continue;
} else {
joint_state_msg_.name.push_back(motor);
if (dxl_wb_->getProtocolVersion() == 2.0f) {
if (strcmp(dxl_wb_->getModelName((uint8_t) motor_id), "XL-320") == 0)
effort =
dxl_wb_->convertValue2Load((int16_t) dynamixel_state_list_.dynamixel_state[state_id].present_current);
else
effort = dxl_wb_
->convertValue2Current((int16_t) dynamixel_state_list_.dynamixel_state[state_id].present_current);
} else if (dxl_wb_->getProtocolVersion() == 1.0f)
effort = dxl_wb_
->convertValue2Load((int16_t) dynamixel_state_list_.dynamixel_state[state_id].present_current);
velocity = dxl_wb_->convertValue2Velocity((uint8_t) motor_id,
(int32_t) dynamixel_state_list_.dynamixel_state[state_id]
.present_velocity);
position = dxl_wb_->convertValue2Radian((uint8_t) motor_id,
(int32_t) dynamixel_state_list_.dynamixel_state[state_id]
.present_position);
if ((uint8_t) motor_id >= 4) {
velocity = -velocity;
position = -position;
}
joint_state_msg_.effort.push_back(effort);
joint_state_msg_.velocity.push_back(velocity);
joint_state_msg_.position.push_back(position);
}
}
} else {
for (int index = 1; index <= 5; index++) {
joint_name.str("");
joint_name << "joint_" << index;
joint_state_msg_.name.push_back(joint_name.str());
joint_state_msg_.effort.push_back(effort);
joint_state_msg_.velocity.push_back(velocity);
joint_state_msg_.position.push_back(position);
}
}
// STEP 2 - Populate joint_state message with gripper readings
position = 0.0;
velocity = 0.0;
effort = 0.0;
if (use_group_["gripper"]) {
int motor_id = dynamixel_name_2ids_[group_motors_["gripper"][0]];
int state_id = motor_id_2state_list_id_[motor_id];
if (dxl_wb_->getProtocolVersion() == 2.0f) {
if (strcmp(dxl_wb_->getModelName((uint8_t) motor_id), "XL-320") == 0)
effort =
dxl_wb_->convertValue2Load((int16_t) dynamixel_state_list_.dynamixel_state[state_id].present_current);
else
effort = dxl_wb_
->convertValue2Current((int16_t) dynamixel_state_list_.dynamixel_state[state_id].present_current);
} else if (dxl_wb_->getProtocolVersion() == 1.0f)
effort = dxl_wb_
->convertValue2Load((int16_t) dynamixel_state_list_.dynamixel_state[state_id].present_current);
velocity = dxl_wb_->convertValue2Velocity((uint8_t) motor_id,
(int32_t) dynamixel_state_list_.dynamixel_state[state_id]
.present_velocity);
position = dxl_wb_->convertValue2Radian((uint8_t) motor_id,
(int32_t) dynamixel_state_list_.dynamixel_state[state_id]
.present_position);
position = (-position - GRIPPER_OPEN_MOTOR_POS)/(GRIPPER_CLOSE_MOTOR_POS -
GRIPPER_OPEN_MOTOR_POS) * (GRIPPER_CLOSE_MOVEIT - GRIPPER_OPEN_MOVEIT) + GRIPPER_OPEN_MOVEIT;
} else {
position = GRIPPER_OPEN_MOVEIT;
}
joint_name.str("");
joint_name << "joint_6";
joint_state_msg_.name.push_back(joint_name.str());
joint_state_msg_.effort.push_back(effort);
joint_state_msg_.velocity.push_back(velocity);
joint_state_msg_.position.push_back(position);
joint_name.str("");
joint_name << "joint_7";
joint_state_msg_.name.push_back(joint_name.str());
joint_state_msg_.effort.push_back(effort);
joint_state_msg_.velocity.push_back(-velocity);
joint_state_msg_.position.push_back(-position);
// STEP 3 - Populate joint_state message with camera readings
position = 0.0;
velocity = 0.0;
effort = 0.0;
if (use_group_["camera"]) {
for (auto const &motor:group_motors_["camera"]) {
int motor_id = dynamixel_name_2ids_[motor];
int state_id = motor_id_2state_list_id_[motor_id];
joint_state_msg_.name.push_back(motor);
if (dxl_wb_->getProtocolVersion() == 2.0f) {
if (strcmp(dxl_wb_->getModelName((uint8_t) motor_id), "XL-320") == 0)
effort = dxl_wb_
->convertValue2Load((int16_t) dynamixel_state_list_.dynamixel_state[state_id].present_current);
else
effort = dxl_wb_
->convertValue2Current((int16_t) dynamixel_state_list_.dynamixel_state[state_id].present_current);
} else if (dxl_wb_->getProtocolVersion() == 1.0f)
effort = dxl_wb_
->convertValue2Load((int16_t) dynamixel_state_list_.dynamixel_state[state_id].present_current);
velocity = dxl_wb_->convertValue2Velocity((uint8_t) motor_id,
(int32_t) dynamixel_state_list_
.dynamixel_state[state_id].present_velocity);
position = dxl_wb_->convertValue2Radian((uint8_t) motor_id,
(int32_t) dynamixel_state_list_
.dynamixel_state[state_id].present_position);
joint_state_msg_.effort.push_back(effort);
joint_state_msg_.velocity.push_back(velocity);
joint_state_msg_.position.push_back(position);
}
} else {
joint_name.str("");
joint_name << "head_pan_joint";
joint_state_msg_.name.push_back(joint_name.str());
joint_state_msg_.effort.push_back(effort);
joint_state_msg_.velocity.push_back(velocity);
joint_state_msg_.position.push_back(position);
joint_name.str("");
joint_name << "head_tilt_joint";
joint_state_msg_.name.push_back(joint_name.str());
joint_state_msg_.effort.push_back(effort);
joint_state_msg_.velocity.push_back(velocity);
joint_state_msg_.position.push_back(position);
}
joint_states_pub_.publish(joint_state_msg_);
if (use_group_["camera"]) {
cameraStatePublish();
}
#ifdef DEBUG
ROS_WARN("[publishCallback] diff_secs : %f", ros::Time::now().toSec() - priv_pub_secs);
priv_pub_secs = ros::Time::now().toSec();
#endif
}