#!/us
in/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):
XXXXXXXXXXsuper().__init__("turtle_controller_node")
XXXXXXXXXXself.cu
ent_pose = Pose()
XXXXXXXXXXself.goal_pose = Pose()
XXXXXXXXXXself.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):
XXXXXXXXXXself.cu
ent_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.cu
ent_pose".
#--------------------------------------------------------------------------------------
XXXXXXXXXXmsg = Twist()
XXXXXXXXXXif distance > 0.1:
# position
XXXXXXXXXXmsg.linear.x = 2*distance
# orientation
XXXXXXXXXXgoal_theta = math.atan2(dist_y, dist_x)
XXXXXXXXXXdiff = goal_theta - self.cu
ent_pose.theta
XXXXXXXXXXif diff > math.pi:
XXXXXXXXXXdiff -= 2*math.pi
XXXXXXXXXXelif diff < -math.pi:
XXXXXXXXXXdiff += 2*math.pi
XXXXXXXXXXmsg.angular.z = 6*diff
XXXXXXXXXXelse:
# target reached!
XXXXXXXXXXmsg.linear.x = 0.0
XXXXXXXXXXmsg.angular.z = 0.0
XXXXXXXXXXself.target_reached = True
XXXXXXXXXXself.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 e
ors.
-------------------------------------
Directions:
1. For each command line, identify the parts of the command. Example:
$ rosrun turtlesim turtlesim_node
command>
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_senso
$ rosmsg list | grep turtlesim
$ rosmsg show turtlesim/Colo
$ rostopic echo /turtle1/color_senso
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 /clea
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