#!/usr/bin/env python3import mathimport rclpyfrom rclpy.node import Nodefrom turtlesim.msg import Posefrom geometry_msgs.msg import Twistclass TurtleControllerNode(Node):def...

help on ubuntu assignment


#!/usr/bin/env python3 import math import rclpy from rclpy.node import Node from turtlesim.msg import Pose from geometry_msgs.msg import Twist class TurtleControllerNode(Node): def __init__(self): super().__init__("turtle_controller_node") self.current_pose = Pose() self.goal_pose = Pose() self.target_reached = True #-------------------------------------------------------------------------------------- #Add: # - A "turtle1/cmd_vel" topic publisher. # - A "turtle1/pose" topic subscriber which will call the "callback_turtle_pose" function # when the topic message is received. # - A timer which calls the "control_loop" function every 0.1 second. # #-------------------------------------------------------------------------------------- def callback_turtle_pose(self, msg): self.current_pose = msg def control_loop(self): #-------------------------------------------------------------------------------------- #Add: # - Code to read the turtle desired position ("self.goal_pose.x" and "self.goal_pose.x") # if "self.target_reached" flag is True. # - This code also reset the "self.target_reached" flag to False. # # - Code to compute the "distance" between "self.goal_pose" and "self.current_pose". #-------------------------------------------------------------------------------------- msg = Twist() if distance > 0.1: # position msg.linear.x = 2*distance # orientation goal_theta = math.atan2(dist_y, dist_x) diff = goal_theta - self.current_pose.theta if diff > math.pi: diff -= 2*math.pi elif diff < -math.pi:="" diff="" +="2*math.pi" msg.angular.z="6*diff" else:="" #="" target="" reached!="" msg.linear.x="0.0" msg.angular.z="0.0" self.target_reached="True" self.cmd_vel_publisher.publish(msg)="" def="" main(args="None):" rclpy.init(args="args)" node="TurtleControllerNode()" rclpy.spin(node)="" rclpy.shutdown()="" if="" __name__="=" "__main__":="" main()="" turtlesim="" exercise="" -------------------------------------="" reference:="" additional="" information="" on="" turtlesim="" can="" be="" found="" in="" ros="" robotics="" by="" example,="" chapter="" 1="" –="" turtlesim="" pages="" 19="" –="" 32.="" tip:="" remember="" to="" use="" tab="" completion="" to="" see="" the="" fields="" of="" the="" message!="" output="" from="" the="" following="" commands="" can="" be="" found="" in="" the="" previously="" referenced="" section="" on="" turtlesim.="" refer="" to="" this="" section="" in="" the="" book="" for="" descriptions="" of="" the="" commands="" or="" if="" there="" are="" any="" difficulties="" or="" errors.="" -------------------------------------="" directions:="" 1.="" for="" each="" command="" line,="" identify="" the="" parts="" of="" the="" command.="" example:="" $="" rosrun="" turtlesim="" turtlesim_node=""> 2. Capture the results of the commands in screenshots for both the terminal window and turtlesim’s window. You can combine multiple commands and movements in a single window. ------------------------------------- To start Turtlesim, first open a terminal window and start the ROS Master with the command: $ roscore Open a 2nd terminal window and start the Turtlesim node: $ rosrun turtlesim turtlesim_node Now it is time to examine the ROS components that were discussed in the lecture: nodes, topics, services and message. Open a 3rd terminal window and try the following commands to explore the ROS elements are the framework of Turtlesim: $ rosnode list $ rosnode info /turtlesim $ rostopic list $ rostopic type /turtle1/color_sensor $ rosmsg list | grep turtlesim $ rosmsg show turtlesim/Color $ rostopic echo /turtle1/color_sensor Use Ctrl+C to kill the process. The following commands will introduce how to move the turtle around in the window: $ rostopic type /turtle1/cmd_vel $ rosmsg show geometry_msgs/Twist $ rostopic pub /turtle1/cmd_vel geometry_msgs/Twist -r 1 -- '[2.0, 0.0, 0.0]' '[0.0, 0.0, 1.8]' Use Ctrl+C to kill the process. The following command moves the turtle using the keyboard: $ rosrun turtlesim turtle_teleop_key Use Ctrl+C to kill the process. The following commands will introduce Turtlesim parameters and ROS service: $ rosparam list $ rosparam get / $ rosparam set background_b 0 $ rosparam set background_g 0 $ rosparam set background_r 255 $ rosservice call /clear The following commands will find the pose of the turtle and introduce ROS service commands to move the turtle: $ rostopic type /turtle1/pose $ rosmsg show turtlesim/Pose $ rostopic echo /turtle1/pose Use Ctrl+C to kill the process. $ rosservice call /turtle1/teleport_absolute 1 1 0 $ rosservice call /turtle1/teleport_relative 1 0