def MoveToPointCallback(msg): x, y, z, pitch = ParseMsg(msg) roboticArm = RoboticArm() availJointState, goalJointState = roboticArm.InversProblem(x, y, z, pitch) if (not availJointState): # rospy.loginfo('Point cannot be reached') return point_cmdResponse(False) else: #rospy.loginfo('Wait...') goalJointState = [str(el) for el in goalJointState] strName = ' '.join(nameList) strJS = ' '.join(goalJointState) strCmd = strName + ' ' + strJS result = jointStateSrv(strCmd) #rospy.loginfo('Well Done!!!') return point_cmdResponse(True)
def GripperCmdCallback(msg): global gripperPose gripperPose = msg.point strName = ' '.join(nameList) strJS = ' '.join(lastGoalJointState) + ' ' + gripperPose strCmd = strName + ' ' + strJS jointStateSrv(strCmd) #rospy.loginfo('Well Done!!!') return point_cmdResponse(True)
def MoveToPointCallback(msg): global lastGoalJointState x, y, z, pitch, roll = ParseMsg(msg) roboticArm = RoboticArm() availJointState, goalJointState = roboticArm.InversProblem( x, y, z, pitch, roll) if (not availJointState): rospy.loginfo('Point cannot be reached') return point_cmdResponse(False) else: rospy.loginfo('Wait...') goalJointState = [str(el) for el in goalJointState] lastGoalJointState = goalJointState strName = ' '.join(nameList) strJS = ' '.join(goalJointState) + ' ' + gripperPose strCmd = strName + ' ' + strJS jointStateSrv(strCmd) rospy.loginfo('Well Done!!!') return point_cmdResponse(True)