control_ros_example.py

#!/usr/bin/env python3

import rospy
from control_msgs.msg import FollowJointTrajectoryGoal
from trajectory_msgs.msg import JointTrajectoryPoint
from std_srvs.srv import Trigger, TriggerRequest
import hello_helpers.hello_misc as hm
import time
from geometry_msgs.msg import Transform, TransformStamped, Pose, Twist
from control_msgs.msg import FollowJointTrajectoryGoal
from trajectory_msgs.msg import JointTrajectoryPoint


class control(hm.HelloNode):
    def __init__(self):
        hm.HelloNode.__init__(self)
        self.topic_name = '/stretch/cmd_vel'
        self.cmd_vel = Twist()
        self.switch_to_nav_service = rospy.ServiceProxy('/switch_to_navigation_mode', Trigger)
        self.switch_to_pos_Service = rospy.ServiceProxy('/switch_to_position_mode', Trigger)
        self.vel_publisher = rospy.Publisher('/stretch/cmd_vel', Twist, queue_size=10)
        hm.HelloNode.main(self, 'control', 'control', wait_for_first_pointcloud=False)

    def switch_position_mode(self):
        req = TriggerRequest()
        result = self.switch_to_pos_Service(req)
        print(result)

    def switch_navigation_mode(self):
        req = TriggerRequest()
        result = self.switch_to_nav_service(req)
        print(result)

    def publish_vel(self, linear_v, rot_v):
        t = Twist()
        t.angular.z = rot_v
        t.linear.x = linear_v
        self.vel_publisher.publish(t)
        print(f"Publishing vel: {t}")

    def issue_multipoint_command(self):
        point0 = JointTrajectoryPoint()
        point0.positions = [0.2, 0.0, 3.4]
        # point0.velocities = [0.2, 0.2, 2.5]
        # point0.accelerations = [1.0, 1.0, 3.5]

        point1 = JointTrajectoryPoint()
        point1.positions = [0.3, 0.1, 2.0]

        trajectory_goal = FollowJointTrajectoryGoal()
        trajectory_goal.trajectory.joint_names = ['joint_lift', 'wrist_extension', 'joint_wrist_yaw']
        trajectory_goal.trajectory.points = [point0, point1] #point2, point3]
        trajectory_goal.trajectory.header.stamp = rospy.Time(0)
        trajectory_goal.trajectory.header.frame_id = 'base_link'

        self.trajectory_client.send_goal(trajectory_goal)
        rospy.loginfo('Sent list of goals = {0}'.format(trajectory_goal))
        self.trajectory_client.wait_for_result()


if __name__ == "__main__":
    c = control()
    c.switch_position_mode()
    c.publish_vel(0.1, 0.1)
    c.switch_navigation_mode()