Lab E: Sample Solution for the TrafficBot Exercise


These are sample solutions for the Lab E TrafficBot Exercise.


Traffic.msg

int8 direction

traffic.py

#!/usr/bin/env python3

import rospy
from class_meeting_08_kinematics.msg import Traffic
import numpy as np

class TrafficNode(object):
    def __init__(self):
        # Set up traffic status publisher
        self.traffic_status_pub = rospy.Publisher("/traffic_status", Traffic)

        # Counter to loop publishing direction with
        self.direction_counter = 0
        rospy.sleep(1)

    def run(self):
        # Once every 10 seconds
        rate = rospy.Rate(0.1)
        while (not rospy.is_shutdown()):
            trafficMsg = Traffic()
            trafficMsg.direction = self.direction_counter % 3
            self.direction_counter += 1
            self.traffic_status_pub.publish(trafficMsg)
            rate.sleep()

if __name__ == '__main__':
    rospy.init_node('traffic_controller')
    traffic_node = TrafficNode()
    traffic_node.run()

move.py

#!/usr/bin/env python3

import rospy
# import the moveit_commander, which allows us to control the arms
import moveit_commander
import math
# import the custom message
from class_meeting_08_kinematics.msg import Traffic

class Robot(object):

    def __init__(self):

        # initialize this node
        rospy.init_node('turtlebot3_dance')

        # Traffic status subscriber
        rospy.Subscriber("/traffic_status", Traffic, self.traffic_dir_received)

        # the interface to the group of joints making up the turtlebot3
        # openmanipulator arm
        self.move_group_arm = moveit_commander.MoveGroupCommander("arm")

        # the interface to the group of joints making up the turtlebot3
        # openmanipulator gripper
        self.move_group_gripper = moveit_commander.MoveGroupCommander("gripper")

        # Reset arm position
        self.move_group_arm.go([0,0,0,0], wait=True)
        print("ready")

    def traffic_dir_received(self, data: Traffic):
        # array of arm joint locations for joint 0
        arm_joint_0 = [math.pi/2, 0, -1 * math.pi/2]

        # select location based on data direction
        arm_joint_0_goal = arm_joint_0[data.direction]

        gripper_joint_close = [-0.01, -0.01]
        gripper_joint_open = [0, 0]

        self.move_group_gripper.go(gripper_joint_close)
        self.move_group_gripper.stop()

        for i in range(3):
            # wait=True ensures that the movement is synchronous
            self.move_group_arm.go([arm_joint_0_goal, 0, 0, 0], wait=True)
            # Calling ``stop()`` ensures that there is no residual movement
            self.move_group_arm.stop()

            self.move_group_arm.go([arm_joint_0_goal, 0, 0, -1 * math.pi/4], wait=True)
            self.move_group_arm.stop()

        self.move_group_gripper.go(gripper_joint_open, wait=True)
        self.move_group_gripper.stop()

    def run(self):
        rospy.spin()

if __name__ == "__main__":
    robot = Robot()
    robot.run()

© Sarah Sebo