3-3- برنامه‌نویسی turtlesim با پایتون

مثال رآس Hello World را در C++ و پایتون انجام دادیم. در این بخش، برنامه­ی جالب­تری به کار می­گیریم. برنامه کاربردی Turtlesim را در رآس مشاهده کردیم. حالا، به چگونگی برنامه­نویسی turtlesim با استفاده از rospy Py نگاهی می­اندازیم. از rospy برای دمو استفاده می‌کنیم، زیرا برای نمونه­سازی بسیار ساده است. در turtlesim، لاک‌پشتی وجود دارد که می‌توانیم آنرا در اطراف فضای‌کاری حرکت دهیم.

3-3-1- حرکت دادن turtlesim

این بخش در مورد چگونگی برنامه­نویسی turtlesim برای حرکت دادن آن در اطراف فضای‌کاری بحث می‌کند.

قبلا آموختید که چگونه می­توانید برنامه turtlesim را شروع کنید. فهرست دستورات در زیر آمده است.

شروع  roscore

$ roscore

اجرای گره turtlesim در ترمینالی دیگر

$ rosrun turtlesim turtlesim_node

فهرست کردن تاپیک­هایی که توسط گره turtlesim_node منتشر می شوند

$ rostopic list

/rosout

/rosout_agg

/turtle1/cmd_vel

/turtle1/color_sensor

/turtle1/pose

برای حرکت دادن لاک‌پشت داخل برنامه­کاربردی turtlesim باید سرعت خطی و زاویه‌ای را روی تاپیک /turtle1/cmd_vel منتشر کنید.

با استفاده از دستور زیر، نوع تاپیک /turtle1/cmd_vel را بررسی کنید.

$ rostopic type /turtle1/cmd_vel

پاسخ:

geometry_msgs/Twist

پس تاپیک /cmd_vel حاوی پیامی از نوع geometry_msgs/Twist است، بنابراین باید همان نوع پیام را بر روی این تاپیک منتشر کنیم تا ربات بتواند حرکت کند.

برای دیدن تعریف پیام geometry_msgs/Twist از دستور زیر استفاده کنید.

$ rosmsg show geometry_msgs/Twist

خروجی دستور فوق در شکل 5-17 نشان داده شده است.

شکل ‏5‑17: تعریف پیام geometry_msgs/Twist

پیام دو زیربخش دارد: سرعت خطی و سرعت زاویه ای.

اگر مولفه­ی سرعت خطی ربات را تنظیم کنیم، ربات به جلو یا عقب حرکت می‌کند. در turtlesim، تنها می‌توان مولفه­ی linear.x را تنظیم کرد زیرا ربات فقط در جهت x حرکت می­کند؛ و در جهت y و z حرکتی وجود ندارد. همچنین می‌توانیم مولفه­ی سرعت زاویه‌ای angular.z را برای چرخش ربات حول محور خود تنظیم کنیم. تنظیم مولفه­های دیگر هیچ تأثیری ندارد.

اطلاعات بیشتر در مورد این پیام را در اسناد رآس[28] بیابید.

چگونه می‌توانیم پیام را روی تاپیک از طریق خط‌فرمان منتشر کنیم؟ جواب استفاده از rostopic است. دستور زیر سرعت linear.x = 0.1 را روی گره turtlesim منتشر می‌کند.

M نکته: لازم نیست دستور را کامل وارد کنید. از کلید tab برای تکمیل خودکار دستور استفاده کنید. فقط کافی است تا /turtle1/cmd_vel را تایپ کنید و سپس کلید Tab را برای تکمیل خودکار ادامه دستورات فشار دهید. پس از ظاهر شدن دستورات کامل، مقدار x را تنظیم کنید.

$ rostopic pub /turtle1/cmd_vel geometry_msgs/Twist “linear:

x:0.1

y:0

z:0

angular:

x:0

y:0

z:0”

چگونه لاک‌پشت را با یک گره پایتون حرکت ‌دهیم؟

قصد داریم گره­ی جدیدی به‌نام move_turtle ایجاد کنیم و با استفاده از آن پیام Twist را به گره turtlesim بفرستیم. شکل 5-18 ارتباط بین دو گره را نشان می‌دهد.

شکل ‏5‑18: گراف محاسباتی گره‌های move_turtle و turtlesim

کد زیر برای گره move_turtle.py است. توضیحات کد را بخوانید تا دید بهتری در مورد هر خط کد بدست آورید.

#!/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]))

except rospy.ROSInterruptException:

pass

این کد سرعت خطی و سرعت زاویه‌ای را از طریق خط­فرمان می‌گیرد. از ماژول sys پایتون برای دریافت گزاره­های خط‌فرمان در داخل کد مان استفاده می­کنیم. هنگامی‌که آن سرعت خطی و سرعت زاویه‌ای دارد، تابع motion_turtle() را فرامی­خواند تا هر دو سرعت را در پیغام Twist وارد کرده و آن را منتشر می‌کند.

کد را با عنوان move_turtle.py در پوشه script بسته hello_world ذخیره کرده و مجوز دسترسی را تغییر دهید.

با دستورات زیر آن را اجرا کنید.

شروع  roscore

$ roscore

شروع گره turtlesim­_node

$ rosrun turtlesim turtlesim_node

گره move_turtle.py را با گزاره­های 0.2 و 0.1 در خط‌فرمان اجرا کنید. در واقع، سرعت خطی = 0.2 متر بر ثانیه و سرعت زاویه‌ای= 0.1 رادیان بر ثانیه است.

$ rosrun hello_world move_turtle.py 0.2 0.1

اگر دستور فوق را اجرا کنید، خروجی شکل 5-19 نمایش داده می‌شود. لاکپشت یک دایره را می پیماید.

شکل ‏5‑19: خروجی move_turtle.py

3-3-2- چاپ کردن موقعیت ربات

تا اینجا دیدید که چگونه سرعت لاکپشت را منتشر کنید. اکنون یاد ‌خواهید گرفت چگونه موقعیت جاری لاک‌پشت را از تاپیک /turtle1/pose بخوانید.

گره  turtlesim_nodeرا مجددا راه­اندازی کنید و گره move_turtle.py را ببندید. با استفاده از rostopic تاپیک /turtle1/pose را منعکس[29] کنید. موقعیت فعلی لاک‌پشت در شکل 5-20 نشان داده شده است.

$ rostopic echo /turtle1/pose

شکل ‏5‑20: گرفتن موقعیت لاکپشت از تاپیک /turtle1/pose

در شکل 5-20 موقعیت فعلی ربات (x، y، theta) و سرعت خطی و زاویه‌ای فعلی لاکپشت را می­بینید.

اگر بخواهید این موقعیت را در یک گره پایتون دریافت کنید، باید مشترک تاپیک /turtle1/pose شوید. برای انجام این کار و دریافت داده‌های پیام، باید نوع پیام رآس را بدانید. با دستور زیر نوع پیام را پیدا ‌کنید.

$ rostopic type /turtle1/pose

خروجی:

turtlsim/Pose

اگر بخواهید به تعریف پیام پی ببرید، از دستور زیر باید استفاده کنید.

$ rosmsg show turtlesim/Pose

همانطور که در شکل 5-21 نشان داده شده است، درون پیام پنج نوع پیام وجود دارد: x، y، theta، سرعت خطی و سرعت زاویه ای

شکل ‏5‑21: تعریف پیام turtlesim/Pose

برای کسب اطلاعات بیشتر در مورد این پیام، به اسناد رآس[30] مراجعه کنید.

بیایید در کد move_turtle.py موجود تغییراتی داده و گزینه‌ای برای اشتراک تاپیک /turtle1/pos به آن اضافه کنیم. کد جدید را با عنوان move_turtle_get_pose.py ذخیره کنید.

شکل 5-22 نشان می‌دهد که چگونه برنامه کار می‌کند. سرعت در حال تغذیه است و در همان زمان اشتراک موقعیت گره turtlesim صورت می­گیرد.

شکل ‏5‑22: کد move_turtle_get_pose.py

#!/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

این کدی خود مفسر است. می‌توانید توضیحات جاهائیکه مربوط به اضافه کردن اشتراک تاپیک /turtle1/pose است را در کد ببینید.

کد را با استفاده از دستورات زیر اجرا کنید. شکل 5-23 نشان می‌دهد که کد موقعیت و سرعت ربات را چاپ می­کند.

شروع  roscore

$ roscore

اجرای گره­ی turtlesim

$ rosrun turtlesim turtlesim_node

اجرای کدmove_turtle_get_pose.py

$ rosrun hello_world move_turtle_get_pose.py 0.2 0.1

شکل ‏5‑23: خروجی کد move_turtle_get_pose.py

خوب، اگر موقعیت و سرعت ربات را دریافت ‌کنیم، می‌توانیم به سادگی به ربات دستور دهیم یک مسافت خاص را بپیماید، درست است؟ مثال بعدی حرکت ربات با گرفتن بازخورد[31] فاصله است که با اصلاح کد move_turtle_get_pose.py صورت می­گیرد.

3-3-3- حرکت دادن ربات با بازخورد موقعیت

کد جدید را با عنوان move_distance.py ذخیره کنید. ارتباط بین این گره و لاک‌پشت در شکل 5-24 نشان داده شده است.

شکل ‏5‑24: ارتباط بین گره های move_distance.py و turtlesim

کارکرد گره ساده است. سرعت خطی، سرعت زاویه‌ای و فاصله (فاصله جهانی) به عنوان گزاره­های خط‌فرمان به گره داده می­شوند.

در حین انتشار سرعت به لاک‌پشت، فاصله توسط وی کنترل شده و هنگامی‌که به مقصد خود برسد، لاک‌پشت/ ربات متوقف می‌شود. توضیحات درون کد را بخوانید تا آنچه در داخل کد اتفاق می‌افتد را بهتر درک کنید.

#!/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

کد فوف را می توان با استفاده از دستورات زیر اجرا کرد. خروجی کد در شکل 5-25 نشان داده شده است.

شروع  roscore

$ roscore

شروع گره  turtlesim

$ rosrun turtlesim turtlesim_node

اجرای کد  move_distance.py. با دادن سرعت خطی، سرعت زاویه­ای و مسافتی که ربات باید طی کند

$ rosrun hello_world move_distance.py 0.2 0.0 8.0

شکل ‏5‑25: خروجی move_distance.py

با استفاده از تاپیک رآس با بسیاری از چیزها در turtlesim بازی کردیم. اکنون می­خواهیم با سرویس رآس و یک پارامتر رآس کار ‌کنیم. مثال بعدی به سادگی فضای‌کاری turtlesim را بازنشانی[32] می‌کند و رنگ پس زمینه را به صورت تصادفی تغییر می‌دهد. بازنشانی فضای‌کاری با استفاده از سرویس‌های رآس انجام می‌شود و تغییر رنگ با استفاده از پارامتر رآس صورت می­گیرد. هنگامی‌که فضای‌کاری بازنشانی می‌شود، موقعیت ربات به وضعیت خانه[33] بازنشانی شده و مدل لاک‌پشت تغییر می‌کند.

3-3-4- بازنشانی و تغییر رنگ پس زمینه

این کد نشان می‌دهد که چگونه یک سرویس و یک پارامتر را از کد پایتون فراخوانی کنید.

فهرستی از سرویس­ها در گره turtlesim (شکل 5-26) را در ادامه مشاهده می­کنید.

$ rosservice list

شکل ‏5‑26: فهرستی از سرویس‌های گره turtlesim

چند سرویس موجود است، ما می‌خواهیم از سرویس /reset استفاده کنیم. وقتی این سرویس را فراخوانی می‌کنیم، فضای‌کاری بازنشانی می‌شود.

نوع سرویس را می توان با دستور زیر بازیابی کرد.

$ rosservice type /reset

خروجی:

std_srvs/Empty

std_srvs/Empty یک سرویس پیش­ساخته­ی رآس است. این سرویس هیچ زمینه­ای ندارد

دستور زیر زمینه­ی تاپیک مربوطه را نشان می‌دهد.

$ rossrv show std_srvs/Empty

پارامترهای رآس را نیز می‌توانیم فهرست کنیم. رنگ پس­زمینه­ی لاک‌پشت را می‌توانید در سه پارامتر ببینید. اگر مقدار این پارامترها را عوض کنیم، رنگ پس­زمینه را می توانیم تغییر ‌دهیم. پس از تنظیم رنگ، باید فضای‌کاری را بازنشانی کنیم تا رنگ جدید نشان داده شود (به شکل 5-27 نگاه کنید).

$ rosparam list

شکل ‏5‑27: فهرستی از پارامترهای گره turtlesim

دستور زیر مقدار هر پارامتر را می‌دهد.

$ rosparam get /background_b

شکل ‏5‑28: تاپیک انتشار رنگ

تاپیک زیر رنگ پس­زمینه را چاپ می‌کند (به شکل 5-28 نگاه کنید).

$ rostopic echo /turtle1/color_sensor

کد زیر پارامترهای رنگ پس زمینه را تنظیم می‌کند و با استفاده از سرویس /reset فضای‌کاری را بازنشانی می کند.

#!/usr/bin/env python

import rospy

import random

from std_srvs.srv import Empty

def change_color():

rospy.init_node(‘change_color’, anonymous=True)

#Setting random values from 0-255 in the color parameters

rospy.set_param(‘/background_b’,random.randint(0,255))

rospy.set_param(‘/background_g’,random.randint(0,255))

rospy.set_param(‘/background_r’,random.randint(0,255))

#Waiting for service /reset

rospy.wait_for_service(‘/reset’)

#Calling /reset service

try:

serv = rospy.ServiceProxy(‘/reset’,Empty)

resp = serv()

rospy.loginfo(“Executed service”)

except rospy.ServiceException, e:

rospy.loginfo(“Service call failed: %s” %e)

rospy.spin()

if __name__ == ‘__main__’:

try:

change_color()

except rospy.ROSInterruptException:

pass

کد را با عنوان turtle_service_param.py ذخیره کنید. دستورات زیر گره رآس را شروع می‌کند (به شکل 5-29 نگاه کنید).

شروع  roscore

$ roscore

شروع گره  turtlesim_node

$ rosrun turtlesim turtlesim_node

اجرای کد turtle_service_param.py

$ rosrun hello_world turtle_service_param.py

شکل ‏5‑29: راه­اندازی مجدد فضای­کاری و تغییردادن رنگ­ها

این تمرین turtlesim را با موفقیت انجام دادید. لاک‌پشت در واقع یک ربات است. تمام عملیاتی که روی لاک‌پشت انجام داده اید را می‌توانید روی یک ربات فیزیکی انجام دهید. بخش بعدی توضیح می‌دهد چگونه این عملیات را با یک ربات واقعی انجام دهید. بخش بعد فقط شبیه‌سازی است اما از فرآیندی شبیه به سخت افزار واقعی استفاده می‌کند.

[28] http://docs.ros.org/api/geometry_msgs/html/msg/Twist.html

[29] Echo

[30] http://docs.ros.org/api/turtlesim/html/msg/Pose.html

[31] feedback

[32] reset

[33] home

[34] www.turtlebot.com/turtlebot2