一、移动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