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)