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;
	}