def __init__(self): # Initialize ROS node rospy.init_node('turtlebot_pose_controller', anonymous=True) self.params = PoseControllerParams(verbose=True) # Current state self.x = 0. self.y = 0. self.theta = 0. # Controller from HW1 P2 self.controller = PoseController(self.params.k1, self.params.k2, self.params.k3, self.params.v_max, self.params.om_max) self.controller.load_goal(self.x, self.y, self.theta) self.pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10) # Time last pose command was received self.cmd_pose_time = rospy.get_rostime() # If using gazebo, we have access to perfect state if self.params.use_gazebo: rospy.Subscriber('/gazebo/model_states', ModelStates, self.gazebo_callback) self.trans_listener = tf.TransformListener() ########## Code starts here ########## # TODO: Create a subscriber to the '/cmd_pose' topic that receives # Pose2D messages and calls cmd_pose_callback. rospy.Subscriber('/cmd_pose', Pose2D, self.cmd_pose_callback)
class PoseControllerNode: def __init__(self): # Initialize ROS node rospy.init_node('turtlebot_pose_controller', anonymous=True) self.params = PoseControllerParams(verbose=True) # Current state self.x = 0. self.y = 0. self.theta = 0. # Controller from HW1 P2 self.controller = PoseController(self.params.k1, self.params.k2, self.params.k3, self.params.v_max, self.params.om_max) self.controller.load_goal(self.x, self.y, self.theta) self.pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10) # Time last pose command was received self.cmd_pose_time = rospy.get_rostime() # If using gazebo, we have access to perfect state if self.params.use_gazebo: rospy.Subscriber('/gazebo/model_states', ModelStates, self.gazebo_callback) self.trans_listener = tf.TransformListener() ########## Code starts here ########## # TODO: Create a subscriber to the '/cmd_pose' topic that receives # Pose2D messages and calls cmd_pose_callback. rospy.Subscriber('/cmd_pose', Pose2D, self.cmd_pose_callback) ########## Code ends here ########## def gazebo_callback(self, msg): if "turtlebot3_burger" not in msg.name: return pose = msg.pose[msg.name.index("turtlebot3_burger")] self.x = pose.position.x self.y = pose.position.y quaternion = (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w) euler = tf.transformations.euler_from_quaternion(quaternion) self.theta = euler[2] def cmd_pose_callback(self, msg): ########## Code starts here ########## # TODO: Update the goal pose in the pose controller. self.controller.load_goal(msg.x, msg.y, msg.theta) ########## Code ends here ########## # Record time of pose update self.cmd_pose_time = rospy.get_rostime() def compute_control(self): if rospy.get_rostime().to_sec() - self.cmd_pose_time.to_sec( ) > self.params.timeout: # haven't received a command in a while so stop rospy.loginfo("Pose controller TIMEOUT: commanding zero controls") cmd = Twist() cmd.linear.x = 0. cmd.angular.z = 0. return cmd # if you are not using gazebo, your need to use a TF look-up to find robot's states # relevant for hw 3+ if not self.params.use_gazebo: try: origin_frame = "/map" if self.params.mapping else "/odom" translation, rotation = self.trans_listener.lookupTransform( origin_frame, '/base_footprint', rospy.Time(0)) self.x, self.y = translation[0], translation[1] self.theta = tf.transformations.euler_from_quaternion( rotation)[2] except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): pass ######### YOUR CODE HERE ############ # TODO: Use your pose controller to compute controls (V, om) given the # robot's current state. V, om = self.controller.compute_control(self.x, self.y, self.theta, 0) ######### END OF YOUR CODE ########## cmd = Twist() cmd.linear.x = V cmd.angular.z = om return cmd def run(self): rate = rospy.Rate(10) # 10 Hz while not rospy.is_shutdown(): ctrl_output = self.compute_control() self.pub.publish(ctrl_output) rate.sleep()