def lower_head():
msg = HeadTrajectoryRosMessage()
point = SO3TrajectoryPointRosMessage()
point.time = 1.0
point.orientation.x = -0.2
point.orientation.y = 0.0
point.orientation.z = -0.0
point.orientation.w = 1.0
point.unique_id = time.time()
point.angular_velocity.x = 0.0
point.angular_velocity.y = 0.0
point.angular_velocity.z = 0.05
msg.taskspace_trajectory_points.append(point)
msg.unique_id = time.time()
#msg.unique_id = uid
#msg.previous_message_id = msg.unique_id+1
#msg.execution_mode = 0
print 'head trajectory ', msg
print time.time() + 1
print ''
rospy.loginfo('publishing head trajectory')
headTrajectoryPublisher.publish(msg)
def adjustHead(position, time):
#position = [South indian head, head nod,
msg = HeadTrajectoryRosMessage()
msg.unique_id = -1
msg = appendTrajectoryPoint(msg, time, position)
t.sleep(0.50)
headTrajectoryPublisher.publish(msg)
def process_head_command(self, binding, ch):
msg = HeadTrajectoryRosMessage()
msg.unique_id = -1
self._update_joint_values('head', binding, ch)
self._append_trajectory_point_so3(
msg, 1.0, self.joint_values['head'])
self.head_publisher.publish(msg)
def sendTrajectory(head_waypoints):
msg = HeadTrajectoryRosMessage()
msg.unique_id = -1
# for each set of joint states
for y in head_waypoints:
# first value is time duration
time = float(y[0])
# subsequent values are desired joint commands
commands = array([float(x) for x in y[1].split()])
msg = appendTrajectoryPoint(msg, time, commands)
rospy.loginfo('publishing %s trajectory' % "head")
headTrajectoryPublisher.publish(msg)