GripperRosControl(const std::vector<std::string>& actr_names, const std::vector<std::string>& jnt_names,
const std::vector<std::string>& controller_names, const std::vector<double>& reducers)
: actr_names_(actr_names)
, jnt_names_(jnt_names)
, controller_names_(controller_names)
{
for (int i = 0; i < jnt_names_.size(); i++)
{
std::string actr = actr_names_[i];
std::string jnt = jnt_names_[i];
// Get transmission
boost::shared_ptr<transmission_interface::SimpleTransmission> t_ptr(
new transmission_interface::SimpleTransmission(reducers[i]));
trans_.push_back(t_ptr);
// Initialize and wrap raw current data
actr_curr_data_[actr].position.push_back(&actr_curr_pos_[actr]);
actr_curr_data_[actr].velocity.push_back(&actr_curr_vel_[actr]);
actr_curr_data_[actr].effort.push_back(&actr_curr_eff_[actr]);
jnt_curr_data_[jnt].position.push_back(&jnt_curr_pos_[jnt]);
jnt_curr_data_[jnt].velocity.push_back(&jnt_curr_vel_[jnt]);
jnt_curr_data_[jnt].effort.push_back(&jnt_curr_eff_[jnt]);
// Initialize and wrap raw command data
actr_cmd_data_[actr].position.push_back(&actr_cmd_pos_[actr]);
jnt_cmd_data_[jnt].position.push_back(&jnt_cmd_pos_[jnt]);
// Register transmissions to each interface
actr_to_jnt_state_.registerHandle(transmission_interface::ActuatorToJointStateHandle(
actr + "_trans", trans_[i].get(), actr_curr_data_[actr], jnt_curr_data_[jnt]));
jnt_to_actr_pos_.registerHandle(transmission_interface::JointToActuatorPositionHandle(
jnt + "_trans", trans_[i].get(), actr_cmd_data_[actr], jnt_cmd_data_[jnt]));
// Connect and register the joint state handle
hardware_interface::JointStateHandle state_handle(jnt, &jnt_curr_pos_[jnt], &jnt_curr_vel_[jnt],
&jnt_curr_eff_[jnt]);
jnt_state_interface_.registerHandle(state_handle);
// Connect and register the joint position handle
hardware_interface::JointHandle pos_handle(jnt_state_interface_.getHandle(jnt), &jnt_cmd_pos_[jnt]);
pos_jnt_interface_.registerHandle(pos_handle);
// ROS publishers and subscribers
actr_cmd_pub_[actr] = nh_.advertise<std_msgs::Float64>("dxl/" + controller_names_[i] + "/command", 5);
actr_state_sub_[actr] =
nh_.subscribe("dxl/" + controller_names_[i] + "/state", 1, &GripperRosControl::actrStateCallback, this);
}
// Register interfaces
registerInterface(&jnt_state_interface_);
registerInterface(&pos_jnt_interface_);
// Start spinning
nh_.setCallbackQueue(&subscriber_queue_);
subscriber_spinner_.reset(new ros::AsyncSpinner(1, &subscriber_queue_));
subscriber_spinner_->start();
}
MyRobot()
{
// connect and register the joint state interface
hardware_interface::JointStateHandle state_handle_lwa1("lwa/joint_1", &pos[0], &vel[0], &eff[0]);
jnt_state_interface.registerHandle(state_handle_lwa1);
hardware_interface::JointStateHandle state_handle_lwa2("lwa/joint_2", &pos[1], &vel[1], &eff[1]);
jnt_state_interface.registerHandle(state_handle_lwa2);
hardware_interface::JointStateHandle state_handle_lwa3("lwa/joint_3", &pos[2], &vel[2], &eff[2]);
jnt_state_interface.registerHandle(state_handle_lwa3);
hardware_interface::JointStateHandle state_handle_lwa4("lwa/joint_4", &pos[3], &vel[3], &eff[3]);
jnt_state_interface.registerHandle(state_handle_lwa4);
hardware_interface::JointStateHandle state_handle_lwa5("lwa/joint_5", &pos[4], &vel[4], &eff[4]);
jnt_state_interface.registerHandle(state_handle_lwa5);
hardware_interface::JointStateHandle state_handle_lwa6("lwa/joint_6", &pos[5], &vel[5], &eff[5]);
jnt_state_interface.registerHandle(state_handle_lwa6);
hardware_interface::JointStateHandle state_handle_lwa7("lwa/joint_7", &pos[6], &vel[6], &eff[6]);
jnt_state_interface.registerHandle(state_handle_lwa7);
registerInterface(&jnt_state_interface);
// connect and register the joint position interface
hardware_interface::JointHandle pos_handle_lwa1(jnt_state_interface.getHandle("lwa/joint_1"), &cmd[0]);
jnt_pos_interface.registerHandle(pos_handle_lwa1);
hardware_interface::JointHandle pos_handle_lwa2(jnt_state_interface.getHandle("lwa/joint_2"), &cmd[1]);
jnt_pos_interface.registerHandle(pos_handle_lwa2);
hardware_interface::JointHandle pos_handle_lwa3(jnt_state_interface.getHandle("lwa/joint_3"), &cmd[2]);
jnt_pos_interface.registerHandle(pos_handle_lwa3);
hardware_interface::JointHandle pos_handle_lwa4(jnt_state_interface.getHandle("lwa/joint_4"), &cmd[3]);
jnt_pos_interface.registerHandle(pos_handle_lwa4);
hardware_interface::JointHandle pos_handle_lwa5(jnt_state_interface.getHandle("lwa/joint_5"), &cmd[4]);
jnt_pos_interface.registerHandle(pos_handle_lwa5);
hardware_interface::JointHandle pos_handle_lwa6(jnt_state_interface.getHandle("lwa/joint_6"), &cmd[5]);
jnt_pos_interface.registerHandle(pos_handle_lwa6);
hardware_interface::JointHandle pos_handle_lwa7(jnt_state_interface.getHandle("lwa/joint_7"), &cmd[6]);
jnt_pos_interface.registerHandle(pos_handle_lwa7);
registerInterface(&jnt_pos_interface);
}
bool MitsubishiArmInterface::init(hardware_interface::JointStateInterface & jnt_state_interface_,
hardware_interface::PositionJointInterface & jnt_pos_interface_)
{
// connect and register the joint state interface
hardware_interface::JointStateHandle state_handle_j1("j1", &pos[0], &vel[0], &eff[0]);
jnt_state_interface_.registerHandle(state_handle_j1);
hardware_interface::JointStateHandle state_handle_j2("j2", &pos[1], &vel[1], &eff[1]);
jnt_state_interface_.registerHandle(state_handle_j2);
hardware_interface::JointStateHandle state_handle_j3("j3", &pos[2], &vel[2], &eff[2]);
jnt_state_interface_.registerHandle(state_handle_j3);
hardware_interface::JointStateHandle state_handle_j4("j4", &pos[3], &vel[3], &eff[3]);
jnt_state_interface_.registerHandle(state_handle_j4);
hardware_interface::JointStateHandle state_handle_j5("j5", &pos[4], &vel[4], &eff[4]);
jnt_state_interface_.registerHandle(state_handle_j5);
hardware_interface::JointStateHandle state_handle_j6("j6", &pos[5], &vel[5], &eff[5]);
jnt_state_interface_.registerHandle(state_handle_j6);
registerInterface(&jnt_state_interface_);
// connect and register the joint position interface
hardware_interface::JointHandle pos_handle_j1(jnt_state_interface_.getHandle("j1"), &cmd[0]);
jnt_pos_interface_.registerHandle(pos_handle_j1);
hardware_interface::JointHandle pos_handle_j2(jnt_state_interface_.getHandle("j2"), &cmd[1]);
jnt_pos_interface_.registerHandle(pos_handle_j2);
hardware_interface::JointHandle pos_handle_j3(jnt_state_interface_.getHandle("j3"), &cmd[2]);
jnt_pos_interface_.registerHandle(pos_handle_j3);
hardware_interface::JointHandle pos_handle_j4(jnt_state_interface_.getHandle("j4"), &cmd[3]);
jnt_pos_interface_.registerHandle(pos_handle_j4);
hardware_interface::JointHandle pos_handle_j5(jnt_state_interface_.getHandle("j5"), &cmd[4]);
jnt_pos_interface_.registerHandle(pos_handle_j5);
hardware_interface::JointHandle pos_handle_j6(jnt_state_interface_.getHandle("j6"), &cmd[5]);
jnt_pos_interface_.registerHandle(pos_handle_j6);
registerInterface(&jnt_pos_interface_);
}