void ROSMaestroController::jointCallback(const sensor_msgs::JointState::ConstPtr& msg) { const sensor_msgs::JointState* write_joint = msg.get(); int length = write_joint->name.size(); for (int i = 0; i < length; ++i) { string name = write_joint->name[i]; //ROS_INFO("-> Servo: %s",name.c_str()); if (servo.find(name) != servo.end()) { int channel = servo[name]; //ROS_INFO("-> Servo: %s - Ch:%d",name.c_str(), channel); maestroSetAngle(channel, write_joint->position[i]); } } }
void jointsCallback(const sensor_msgs::JointState::ConstPtr& data) { // printf("Got some joint states.\n"); jointstates = *(data.get()); ready = true; }