yaml_to_command

#!/usr/bin/env python3

import rospy
from std_srvs.srv import Trigger, TriggerRequest, TriggerResponse
from control_msgs.msg import FollowJointTrajectoryGoal
from trajectory_msgs.msg import JointTrajectoryPoint

import hello_helpers.hello_misc as hm
import argparse as ap
import time
import yaml

instream = """
goal:
  trajectory:
    header:
      seq: 0
      stamp:
        secs: 1663809518
        nsecs: 932443380
      frame_id: ''
    joint_names:
      - joint_lift
      - joint_arm_l0
      - joint_arm_l1
      - joint_arm_l2
      - joint_arm_l3
      - joint_gripper_finger_left
      - joint_wrist_roll
      - joint_wrist_pitch
      - joint_wrist_yaw
      - joint_head_pan
      - joint_head_tilt
    points:
      -
        positions: [0.5137053394519911, 0.0, 0.0, 0.0, 0.0, -0.038557933294775415, -0.7439806821245359, -0.7761942786701345, -0.7676295526044397, -0.8244375296703885, -0.0010067811717669514]
        velocities: []
        accelerations: []
        effort: []
        time_from_start:
          secs: 0
          nsecs:         0
  path_tolerance: []
  goal_tolerance: []
  goal_time_tolerance:
    secs: 1
    nsecs: 0
"""


class YamlToCommand(hm.HelloNode):

    def __init__(self):
        hm.HelloNode.__init__(self)
        self.rate = 10.0

    def issue_stow_command(self):
        stow_point = JointTrajectoryPoint()
        stow_point.time_from_start = rospy.Duration(0.000)
        stow_point.positions = [0.2, 0.0, 3.0, 0.0, 0.0]

        trajectory_goal = FollowJointTrajectoryGoal()
        trajectory_goal.trajectory.joint_names = ['joint_lift', 'wrist_extension', 'joint_wrist_yaw', 'joint_wrist_pitch', 'joint_wrist_roll']
        trajectory_goal.trajectory.points = [stow_point]
        trajectory_goal.trajectory.header.stamp = rospy.Time(0.0)
        trajectory_goal.trajectory.header.frame_id = 'base_link'

        self.trajectory_client.send_goal(trajectory_goal)
        rospy.loginfo('Sent stow goal = {0}'.format(trajectory_goal))

    def issue_yaml_command(self, valid_joints=['joint_gripper_finger_left', 'joint_lift', 'joint_wrist_yaw', 'joint_wrist_pitch', 'joint_wrist_roll', 'joint_head_pan', 'joint_head_tilt']):
        data = yaml.load(instream, Loader=yaml.FullLoader)
        valid_joints_indices = sorted([data['goal']['trajectory']['joint_names'].index(j) for j in valid_joints])

        points = []
        for point in data['goal']['trajectory']['points']:
            p = JointTrajectoryPoint()
            p.time_from_start = rospy.Duration(float(point['time_from_start']['secs']) + float(1e-9 * point['time_from_start']['nsecs']))
            p.positions = [point['positions'][i] for i in valid_joints_indices] if len(point['positions']) >= len(valid_joints_indices) else []
            p.velocities = [point['velocities'][i] for i in valid_joints_indices] if len(point['velocities']) >= len(valid_joints_indices) else []
            p.accelerations = [point['accelerations'][i] for i in valid_joints_indices] if len(point['accelerations']) >= len(valid_joints_indices) else []
            points.append(p)

        trajectory_goal = FollowJointTrajectoryGoal()
        trajectory_goal.trajectory.joint_names = [data['goal']['trajectory']['joint_names'][i] for i in valid_joints_indices]
        trajectory_goal.trajectory.points = points
        trajectory_goal.trajectory.header.stamp = rospy.Time(0.0)
        trajectory_goal.trajectory.header.frame_id = 'base_link'

        self.trajectory_client.send_goal(trajectory_goal)
        rospy.loginfo('Sent yaml goal = {0}'.format(trajectory_goal))

    def main(self):
        hm.HelloNode.main(self, 'yaml_to_command', 'yaml_to_command', wait_for_first_pointcloud=False)

        rospy.loginfo('stowing...')
        node.issue_stow_command()
        time.sleep(6)
        rospy.loginfo('issuing first command...')
        node.issue_yaml_command()
        time.sleep(6)
        rospy.loginfo('issuing second command...')
        node.issue_yaml_command()
        time.sleep(6)
        rospy.loginfo('done!')


if __name__ == '__main__':
    try:
        parser = ap.ArgumentParser(description='')
        args, unknown = parser.parse_known_args()
        node = YamlToCommand()
        node.main()
    except KeyboardInterrupt:
        rospy.loginfo('interrupt received, so shutting down')