def makePlanMessage(self, poses, poseTimes, info, fields):
from director import robotstate
# rescale poseTimes
poseTimes = np.array(poseTimes)*2.0
states = [robotstate.drakePoseToRobotState(pose) for pose in poses]
for i, state in enumerate(states):
state.utime = poseTimes[i]*1e6
msg = robot_plan_t()
msg.utime = fields.utime
msg.robot_name = 'robot'
msg.num_states = len(states)
msg.plan = states
msg.plan_info = [info]*len(states)
msg.num_bytes = 0
msg.matlab_data = []
msg.num_grasp_transitions = 0
msg.left_arm_control_type = msg.NONE
msg.right_arm_control_type = msg.NONE
msg.left_leg_control_type = msg.NONE
msg.right_leg_control_type = msg.NONE
return msg
def makePlanMessage(self, poses, poseTimes, info, fields):
from director import robotstate
# rescale poseTimes
poseTimes = np.array(poseTimes) * 2.0
states = [robotstate.drakePoseToRobotState(pose) for pose in poses]
for i, state in enumerate(states):
state.utime = poseTimes[i] * 1e6
msg = robot_plan_t()
msg.utime = fields.utime
msg.robot_name = 'robot'
msg.num_states = len(states)
msg.plan = states
msg.plan_info = [info] * len(states)
msg.num_bytes = 0
msg.matlab_data = []
msg.num_grasp_transitions = 0
msg.left_arm_control_type = msg.NONE
msg.right_arm_control_type = msg.NONE
msg.left_leg_control_type = msg.NONE
msg.right_leg_control_type = msg.NONE
return msg
def stopCurrentPoint(self, msg):
if msg.data:
rospy.loginfo("stopping robot")
lcm_msg = robot_plan_t()
lcm_msg.utime = 0
lc = lcm.LCM()
lc.publish("STOP", lcm_msg.encode())
def ROSJointTrajectoryToLCMRobotPlan(jointTrajectoryRos):
plan = robot_plan_t()
plan.utime = 0
plan.robot_name = ""
plan.num_states = len(jointTrajectoryRos.points)
plan.plan_info = [1] * plan.num_states
jointNames = jointTrajectoryRos.joint_names
for idx, jointTrajectoryPoint in enumerate(jointTrajectoryRos.points):
robotState = TrajectoryServer.ROSJointTrajectoryPointToLCMRobotState(
jointNames, jointTrajectoryPoint)
plan.plan.append(robotState)
return plan