ROS----rospy編寫turtlesim程式

一、移動turtlesim

roscore
rosrun turtlesim turtlesim_node
rostopic pub /turtle1/cmd_vel geometry_msgs/Twist "linear:
x:0.1
y:0
z:0
angular:
x:0
y:0
z:0"
           

python檔案：

#!/usr/bin/env python
import rospy
#Importing Twist message: Used to send velocity to Turtlesim
from geometry_msgs.msg import Twist
#Handling command line arguments
import sys
#Function to move turtle: Linear and angular velocities are arguments
def move_turtle(lin_vel,ang_vel):
	rospy.init_node('move_turtle', anonymous=False)
	#The /turtle1/cmd_vel is the topic in which we have to send Twist messages
	pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
	rate = rospy.Rate(10) # 10hz
	#Creating Twist message instance
	vel = Twist()
	while not rospy.is_shutdown():
		#Adding linear and angular velocity to the message
		vel.linear.x = lin_vel
		vel.linear.y = 0
		vel.linear.z = 0
		vel.angular.x = 0
		vel.angular.y = 0
		vel.angular.z = ang_vel
		rospy.loginfo("Linear Vel = %f: Angular Vel =%f",lin_vel,ang_vel)
		#Publishing Twist message
		pub.publish(vel)
		rate.sleep()
if __name__ == '__main__':
	try:
		#Providing linear and angular velocity through command line
		move_turtle(float(sys.argv[1]),float(sys.argv[2]))
		#move_turtle(0.2, 0.1)
	except rospy.ROSInterruptException:
		pass
           

終端輸入：

roscore
rosrun turtlesim turtlesim_node
rosrun hello_world move_turtle.py 0.2 0.1
#烏龜畫圈！！！
           

擷取烏龜位置：

#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose
import sys
#/turtle1/Pose topic callback
def pose_callback(pose):
	rospy.loginfo("Robot X = %f : Y=%f :Z=%f\n",pose.x,pose.y,pose.theta)
def move_turtle(lin_vel,ang_vel):
	rospy.init_node('move_turtle', anonymous=True)
	pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
	#Creating new subscriber: Topic name= /turtle1/pose:Callback name: pose_callback
	rospy.Subscriber('/turtle1/pose',Pose, pose_callback)
	rate = rospy.Rate(10) # 10hz
	vel = Twist()
	while not rospy.is_shutdown():
		vel.linear.x = lin_vel
		vel.linear.y = 0
		vel.linear.z = 0
		vel.angular.x = 0
		vel.angular.y = 0
		vel.angular.z = ang_vel
		rospy.loginfo("Linear Vel = %f: Angular Vel = %f",lin_vel,ang_vel)
		pub.publish(vel)
		rate.sleep()
if __name__ == '__main__':
		try:
			move_turtle(float(sys.argv[1]),float(sys.argv[2]))
		except rospy.ROSInterruptException:
			pass
           

利用位置回報移動烏龜：

#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose
import sys
robot_x = 0
def pose_callback(pose):
	global robot_x
	rospy.loginfo("Robot X = %f\n",pose.x)
	robot_x = pose.x
def move_turtle(lin_vel,ang_vel,distance):
	global robot_x
	rospy.init_node('move_turtle', anonymous=True)
	pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
	rospy.Subscriber('/turtle1/pose',Pose, pose_callback)
	rate = rospy.Rate(10) # 10hz
	vel = Twist()
	while not rospy.is_shutdown():
		vel.linear.x = lin_vel
		vel.linear.y = 0
		vel.linear.z = 0
		vel.angular.x = 0
		vel.angular.y = 0
		vel.angular.z = ang_vel
		#rospy.loginfo("Linear Vel = %f: Angular Vel = %f",lin_vel,ang_vel)
		#Checking the robot distance is greater than the commanded distance
		# If it is greater, stop the node
		if(robot_x >= distance):
			rospy.loginfo("Robot Reached destination")
			rospy.logwarn("Stopping robot")
			break
		pub.publish(vel)
		rate.sleep()
if __name__ == '__main__':
		try:
			move_turtle(float(sys.argv[1]),float(sys.argv[2]),float(sys.argv[3]))
		except rospy.ROSInterruptException:
			pass