void PlatformCtrlNode::receiveCmd(const geometry_msgs::Twist& twist) { mutex.lock(); trajectory_msgs::JointTrajectory traj; kin->execInvKin(twist,traj); topicPub_DriveCommands.publish(traj); mutex.unlock(); }