ROS动作编程之控制小乌龟运动到目标位置

<think>我们正在Ubuntu20.04上使用ROS（ROSNoetic）来运行小乌龟模拟器，并绘制轨迹组成数字"228702229"。这个任务需要控制小乌龟按照数字的形状移动。由于小乌龟只能通过直线和旋转来移动，我们需要将每个数字分解为一系列连续的线段（或路径点），然后通过发布运动指令让小乌龟沿着这些路径点移动。###步骤概述1.**安装ROSNoetic**：确保在Ubuntu20.04上已安装ROSNoetic桌面完整版。2.**创建ROS工作空间和功能包**：创建一个工作空间和一个功能包来编写我们的控制节点。3.**编写控制节点**：编写一个ROS节点，该节点发布速度指令（`geometry_msgs/Twist`）来控制小乌龟的运动，使其轨迹形成数字"228702229"。4.**分解数字轨迹**：将每个数字的轨迹分解为一系列坐标点（或线段），并转换为小乌龟的运动指令（线速度和角速度）。5.**运行模拟器并执行节点**：启动`roscore`，启动小乌龟模拟器节点（`turtlesim_node`），然后运行我们的控制节点。###详细步骤####1.安装ROSNoetic如果尚未安装ROS，请按照以下步骤安装：```bashsudosh-c'echo"debhttp://packages.ros.org/ros/ubuntufocalmain">/etc/apt/sources.list.d/ros-latest.list'sudoapt-keyadv--keyserver'hkp://keyserver.ubuntu.com:80'--recv-keyC1CF6E31E6BADE8868B172B4F42ED6FBAB17C654sudoaptupdatesudoaptinstallros-noetic-desktop-fullecho"source/opt/ros/noetic/setup.bash">>~/.bashrcsource~/.bashrcsudoaptinstallpython3-rosdeppython3-rosinstallpython3-rosinstall-generatorpython3-wstoolbuild-essentialsudorosdepinitrosdepupdate```####2.创建ROS工作空间和功能包```bashmkdir-p~/catkin_ws/srccd~/catkin_ws/catkin_makesourcedevel/setup.bashcdsrccatkin_create_pkgdraw_numbersrospygeometry_msgscd~/catkin_wscatkin_make```####3.编写控制节点在`draw_numbers`功能包的`src`文件夹中创建Python脚本`draw_numbers.py`。我们需要将数字"228702229"的轨迹分解为一系列路径点。每个数字可以看作是由连续的线段组成。我们可以为每个数字定义一系列相对坐标点（相对于起始点），然后连接这些点。由于数字的书写是连续的，我们可以将整个数字串看作一个连续的轨迹。但是，为了避免在移动过程中绘制不需要的线段（如抬笔动作），我们可以在移动到下一个数字的起始位置时将笔抬起（设置`turtlesim`中的笔颜色为与背景相同），移动到位后再落笔。然而，在`turtlesim`中，我们可以通过服务调用控制笔的抬起和落下：-服务`/turtle1/set_pen`可以设置笔的颜色和宽度，当设置宽度为0时，相当于抬笔（不绘制轨迹），设置非零宽度时落笔。因此，我们的节点需要：-发布速度指令到`/turtle1/cmd_vel`。-使用服务`/turtle1/set_pen`来控制抬笔和落笔。-使用服务`/turtle1/teleport_absolute`来快速移动到某个起始位置（可选，用于调整初始位置）。**注意**：由于数字轨迹比较复杂，我们需要预先设计每个数字的路径。每个数字可以写在一个固定大小的格子中，然后按顺序排列。#####数字轨迹设计我们可以将每个数字设计在1x1的格子内，然后根据数字的顺序调整起始位置。例如，第一个数字'2'从(1,1)开始，第二个数字'2'从(2,1)开始，以此类推。每个数字的轨迹可以用一系列连续的线段（点）来表示。我们可以为每个数字编写一个函数，该函数接收起始位置，然后生成从起始位置开始的一系列点（相对于起始点的偏移量）。例如，数字'2'的轨迹可以分解为：1.从(0,0)到(1,0)2.到(1,1)3.到(0,1)4.到(0,0)#这个点不需要，因为下一个数字可能不是从这里开始，所以到(0,0)后，实际上我们只画了上面的部分，然后需要抬笔移动到下一个数字的起始位置。但是，为了形成完整的'2'，我们通常这样画：-从左下角(0,0)开始，向右到(1,0)-然后向右上到(1,1)-然后向左到(0,1)-然后向左下到(0,0)（但这样会形成一个矩形，所以实际上我们只画三边：下边、右边、上边，然后直接画斜线到左下角？）实际上，我们常见的'2'的写法是：-从左上角(0,1)开始，向右到(1,1)-然后向右下到(1,0.5)（或者直接到(1,0)？）-然后向左下到(0,0)-然后向右到(1,0)但这样可能不标准。我们可以参考七段数码管的显示方式，但这里我们自由手写风格。为了简化，我们可以预先用图形软件画出数字，然后获取关键点坐标。这里为了编程，我们采用简单的线段连接。**设计每个数字的路径点（以相对坐标表示，每个数字占据一个1x1的格子）**：数字'0'：一个矩形-(0,0)->(1,0)->(1,1)->(0,1)->(0,0)数字'1'：一条竖线-(0.5,0)->(0.5,1)数字'2'：-(0,1)->(1,1)->(1,0.5)->(0,0.5)->(0,0)->(1,0)数字'8'：两个矩形重叠-可以看作0的中间加一横，但我们可以画：从(0,0.5)开始->(1,0.5)->(1,1)->(0,1)->(0,0.5)->(0,0)->(1,0)->(1,0.5)但是这样会重复画中间线，我们可以简化：-(0,0.5)->(1,0.5)->(1,1)->(0,1)->(0,0)->(1,0)->(1,0.5)这样画完。数字'7'：-(0,1)->(1,1)->(0.5,0)数字'9'：-从(1,0)开始->(1,1)->(0,1)->(0,0.5)->(1,0.5)数字'2'：我们上面设计了一个，但可能和另一个2不一致，所以统一一个写法。由于数字串"228702229"包含多个重复数字，我们为每个数字定义相同的路径函数。我们定义以下数字的路径（相对坐标，从每个数字的左下角(0,0)到右上角(1,1)）：-**0**:[(0,0),(1,0),(1,1),(0,1),(0,0)]-**1**:[(0.5,0),(0.5,1)]-**2**:[(0,1),(1,1),(1,0.5),(0,0.5),(0,0),(1,0)]-**8**:[(0,0.5),(1,0.5),(1,1),(0,1),(0,0.5),(0,0),(1,0),(1,0.5)]-**7**:[(0,1),(1,1),(0.5,0)]-**9**:[(1,0),(1,1),(0,1),(0,0.5),(1,0.5)]注意：数字'2'的路径中，从(0,0.5)到(0,0)再到(1,0)可能会形成一条折线，但这样画出来就是2。####4.编写控制节点代码在`draw_numbers.py`中，我们将：-初始化ROS节点。-创建发布速度指令的发布器。-创建服务客户端用于设置笔。-定义每个数字的路径函数。-定义主函数：首先将小乌龟移动到初始位置，然后按顺序绘制每个数字。由于数字串较长，我们按顺序绘制每个数字，每个数字绘制前先抬笔移动到该数字的起始位置（左下角），然后落笔，按照路径点移动，绘制完一个数字后抬笔移动到下一个数字的起始位置。每个数字占据一个1x1的格子，我们假设数字之间间隔0.2。整个轨迹从(1,1)开始，然后每个数字的起始x坐标递增1.2（1的宽度+0.2的间隔）。具体步骤：1.初始化节点。2.等待`/turtle1/set_pen`服务可用。3.创建速度发布器。4.定义移动函数：从当前位置移动到目标位置（通过计算线速度和角速度，使用比例控制）。5.定义设置笔的函数（抬笔和落笔）。6.定义每个数字的路径（相对坐标）。7.循环数字串中的每个字符：-计算该字符的起始绝对位置（x=起始x+索引*1.2,y=1）。-抬笔移动到该起始位置（即该数字左下角）。-落笔。-根据数字字符获取路径点列表（相对坐标），转换为绝对坐标（起始位置+相对坐标）。-按顺序移动到每个点。8.完成。**注意**：由于小乌龟的初始位置在(5.5,5.5)，我们的数字轨迹如果从(1,1)开始，可能超出窗口（窗口大小11x11）。因此，我们可以将整个轨迹的起始点设置在(1,1)，然后向右排列。或者为了居中，我们可以计算总宽度（9个数字，每个1.2宽，总宽10.8，起始x=(11-10.8)/2≈0.1，然后每个数字从0.1开始，x递增1.2）。但为了方便，我们直接使用(1,1)作为第一个数字的起始位置。####代码实现以下是`draw_numbers.py`的代码示例。由于实际运动控制需要连续发布速度指令直到到达目标点，我们将实现一个`move_to`函数。```python#!/usr/bin/envpython3importrospyfromgeometry_msgs.msgimportTwist,Pointfromturtlesim.srvimportSetPen,SetPenRequestfromturtlesim.srvimportTeleportAbsoluteimportmathclassDrawNumbers:def__init__(self):rospy.init_node('draw_numbers_node',anonymous=True)self.vel_pub=rospy.Publisher('/turtle1/cmd_vel',Twist,queue_size=10)rospy.wait_for_service('/turtle1/set_pen')self.set_pen=rospy.ServiceProxy('/turtle1/set_pen',SetPen)rospy.wait_for_service('/turtle1/teleport_absolute')self.teleport=rospy.ServiceProxy('/turtle1/teleport_absolute',TeleportAbsolute)#设置初始位置，抬笔移动到(1,1)self.teleport(1.0,1.0,0.0)self.set_pen(0,0,0,0,1)#先抬笔（宽度为0）self.rate=rospy.Rate(10)#10Hzdefset_pen_status(self,width):#设置笔，颜色不变（默认黑色），宽度为0表示抬笔，非0落笔self.set_pen(0,0,0,width,0)defmove_to(self,x,y,linear_speed=1.0,angular_speed=1.0,tolerance=0.1):#计算当前小乌龟位置？我们没有订阅位置，所以需要记录位置#但为了简化，我们每次使用teleport_absolute后记录位置，或者我们不用记录，因为每次移动都是相对目标点#这里我们采用连续发布速度指令直到到达目标点#但是，我们不知道当前位置，因此我们需要先获取当前位置。但这里为了简化，我们使用全局位置记录。#由于我们每次移动都是绝对位置，我们可以记录当前位置。#但我们没有初始位置，所以我们在初始化时记录初始位置(1,1,0)#我们创建一个变量来记录当前位置ifnothasattr(self,'current_x'):#如果没有，则初始化为(1,1,0)self.current_x=1.0self.current_y=1.0self.current_theta=0.0#计算目标点与当前位置的距离和角度差dx=x-self.current_xdy=y-self.current_ydistance=math.sqrt(dx**2+dy**2)target_theta=math.atan2(dy,dx)#先旋转到目标角度d_theta=target_theta-self.current_theta#调整角度差到[-pi,pi]区间whiled_theta>math.pi:d_theta-=2*math.piwhiled_theta<-math.pi:d_theta+=2*math.pi#旋转twist=Twist()ifabs(d_theta)>tolerance:twist.angular.z=angular_speedifd_theta>0else-angular_speedwhileabs(d_theta)>toleranceandnotrospy.is_shutdown():self.vel_pub.publish(twist)self.rate.sleep()#更新角度差（实际中应该订阅位置，但这里简化，假设旋转是即时的，所以不更新，而是等待时间）#由于没有反馈，我们根据角速度和时间的乘积来估计self.current_theta+=twist.angular.z*(1.0/10.0)#时间间隔0.1sd_theta=target_theta-self.current_theta#重新调整角度差whiled_theta>math.pi:d_theta-=2*math.piwhiled_theta<-math.pi:d_theta+=2*math.piifabs(d_theta)<=tolerance:break#停止旋转twist.angular.z=0self.vel_pub.publish(twist)#直线移动twist=Twist()ifdistance>tolerance:twist.linear.x=linear_speedwhiledistance>toleranceandnotrospy.is_shutdown():self.vel_pub.publish(twist)self.rate.sleep()#更新位置self.current_x+=twist.linear.x*math.cos(self.current_theta)*(1.0/10.0)self.current_y+=twist.linear.x*math.sin(self.current_theta)*(1.0/10.0)dx=x-self.current_xdy=y-self.current_ydistance=math.sqrt(dx**2+dy**2)ifdistance<=tolerance:break#停止twist.linear.x=0self.vel_pub.publish(twist)#更新位置为目标位置（避免累积误差）self.current_x=xself.current_y=yself.current_theta=target_thetadefdraw_number(self,number,start_x,start_y):#定义数字路径（相对坐标，每个数字在1x1的格子内，左下角为(0,0)）paths={'0':[(0,0),(1,0),(1,1),(0,1),(0,0)],'1':[(0.5,0),(0.5,1)],'2':[(0,1),(1,1),(1,0.5),(0,0.5),(0,0),(1,0)],'7':[(0,1),(1,1),(0.5,0)],'8':[(0,0.5),(1,0.5),(1,1),(0,1),(0,0.5),(0,0),(1,0),(1,0.5)],'9':[(1,0),(1,1),(0,1),(0,0.5),(1,0.5)]}ifnumbernotinpaths:rospy.logwarn(f"Number{number}notdefined.")return#将相对坐标转换为绝对坐标（加上起始点）abs_points=[(start_x+p[0],start_y+p[1])forpinpaths[number]]#移动到第一个点（落笔已经在外面设置）forpointinabs_points:self.move_to(point[0],point[1])defrun(self,number_str):#初始位置设置已经在__init__中完成#设置每个数字的起始x坐标（从左到右，每个数字宽1，间隔0.2）start_x=1.0start_y=1.0spacing=1.2fori,numinenumerate(number_str):#抬笔移动到当前数字的起始位置（即该数字路径的第一个点的绝对位置）self.set_pen_status(0)#抬笔#获取当前数字的路径的第一个点（相对坐标）ifnumin['0','1','2','7','8','9']:#定义每个数字的第一个点（相对坐标）first_points={'0':(0,0),'1':(0.5,0),'2':(0,1),'7':(0,1),'8':(0,0.5),'9':(1,0)}first_point=first_points[num]abs_x=start_x+i*spacing+first_point[0]abs_y=start_y+first_point[1]#直接使用teleport进行快速移动（避免长时间移动）self.teleport(abs_x,abs_y,0.0)self.current_x=abs_xself.current_y=abs_yself.current_theta=0.0#落笔self.set_pen_status(2)#落笔，宽度为2#绘制当前数字，起始位置为(start_x+i*spacing,start_y)self.draw_number(num,start_x+i*spacing,start_y)else:rospy.logwarn(f"Skippingunknowncharacter:{num}")#完成self.set_pen_status(0)#抬笔rospy.loginfo("Finisheddrawing.")if__name__=='__main__':try:drawer=DrawNumbers()number_str="228702229"drawer.run(number_str)exceptrospy.ROSInterruptException:pass```####5.运行1.在一个终端中运行`roscore`。2.在另一个终端中运行`rosrunturtlesimturtlesim_node`。3.运行我们的节点：`rosrundraw_numbersdraw_numbers.py`（需要给脚本执行权限：`chmod+xdraw_numbers.py`）。注意：由于运动控制是开环的，没有位置反馈，所以可能会有累积误差。但因为我们每个数字绘制后都重新设置位置（通过抬笔后的快速移动），所以误差不会累积到下一个数字。###相关问题1.如何调整小乌龟的移动速度？2.如果轨迹不准确，如何校正？3.如何将轨迹保存为图片？4.除了数字，如何绘制其他形状或字母？</think>### 在Ubuntu 20.04上用ROS绘制数字轨迹"228702229" 要在Ubuntu 20.04上使用ROS的小乌龟模拟器绘制数字轨迹"228702229"，需要编写一个ROS节点控制乌龟按特定路径移动。以下是完整解决方案： #### 环境准备 1. **安装ROS Noetic**（Ubuntu 20.04对应版本）： ```bash sudo apt update sudo apt install ros-noetic-desktop-full echo "source /opt/ros/noetic/setup.bash" >> ~/.bashrc source ~/.bashrc sudo apt install python3-rosdep python3-rosinstall sudo rosdep init && rosdep update ``` 2. **创建工作空间**： ```bash mkdir -p ~/turtle_draw_ws/src cd ~/turtle_draw_ws catkin_make source devel/setup.bash ``` #### 实现步骤 ##### 1. 创建功能包和节点 ```bash cd ~/turtle_draw_ws/src catkin_create_pkg draw_numbers rospy geometry_msgs cd draw_numbers mkdir scripts && cd scripts ``` ##### 2. 编写Python控制脚本 `draw_228702229.py` ```python #!/usr/bin/env python3 import rospy from geometry_msgs.msg import Twist from turtlesim.msg import Pose from turtlesim.srv import SetPen import math class DrawNumber: def __init__(self): rospy.init_node('draw_number_node') self.pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10) rospy.wait_for_service('/turtle1/set_pen') self.set_pen = rospy.ServiceProxy('/turtle1/set_pen', SetPen) self.set_pen(0, 0, 0, 5, 0) # 设置画笔为黑色粗线 self.pose = Pose() rospy.Subscriber('/turtle1/pose', Pose, self.pose_callback) self.rate = rospy.Rate(10) def pose_callback(self, data): self.pose = data def move(self, distance, is_forward=True): vel_msg = Twist() if is_forward: vel_msg.linear.x = abs(distance) else: vel_msg.linear.x = -abs(distance) t0 = rospy.Time.now().to_sec() current_distance = 0 while current_distance < abs(distance): self.pub.publish(vel_msg) t1 = rospy.Time.now().to_sec() current_distance = vel_msg.linear.x * (t1 - t0) vel_msg.linear.x = 0 self.pub.publish(vel_msg) def rotate(self, angle_rad, clockwise=True): vel_msg = Twist() if clockwise: vel_msg.angular.z = -abs(angle_rad) else: vel_msg.angular.z = abs(angle_rad) t0 = rospy.Time.now().to_sec() current_angle = 0 while current_angle < abs(angle_rad): self.pub.publish(vel_msg) t1 = rospy.Time.now().to_sec() current_angle = abs(vel_msg.angular.z) * (t1 - t0) vel_msg.angular.z = 0 self.pub.publish(vel_msg) def draw_digit(self, digit_pattern): for segment in digit_pattern: self.move(segment['distance']) self.rotate(math.radians(segment['angle'])) def run(self): rospy.sleep(1) # 等待初始化完成 # 定义数字轨迹 (距离单位: 1.0 = 乌龟步长) digits = { '2': [ {'distance': 1.0, 'angle': 0}, # 右 {'distance': 0.5, 'angle': 90}, # 上 {'distance': 0.5, 'angle': -90}, # 右 {'distance': 0.5, 'angle': -90}, # 下 {'distance': 1.0, 'angle': 180} # 左 ], '8': [ {'distance': 0.5, 'angle': 0}, # 右 {'distance': 0.5, 'angle': 90}, # 上 {'distance': 0.5, 'angle': 90}, # 左 {'distance': 0.5, 'angle': 90}, # 下 {'distance': 0.5, 'angle': 90}, # 右 {'distance': 0.5, 'angle': 0} # 右（闭合） ], '7': [ {'distance': 1.0, 'angle': 0}, # 右 {'distance': 0.7, 'angle': -135} # 左下 ], '0': [ {'distance': 1.0, 'angle': 0}, # 右 {'distance': 1.0, 'angle': 90}, # 下 {'distance': 1.0, 'angle': 90}, # 左 {'distance': 1.0, 'angle': 90} # 上（闭合） ], '9': [ {'distance': 0.5, 'angle': 0}, # 右 {'distance': 0.5, 'angle': 90}, # 上 {'distance': 0.5, 'angle': 90}, # 左 {'distance': 0.5, 'angle': -90}, # 下 {'distance': 0.5, 'angle': 180} # 左 ] } # 绘制数字序列 228702229 sequence = "228702229" start_x, start_y = 2.0, 5.0 # 起始位置 for digit in sequence: # 移动到新数字起始位置 self.set_pen(0, 0, 0, 0, 0) # 抬笔 angle_to_start = math.atan2(start_y - self.pose.y, start_x - self.pose.x) self.rotate(angle_to_start - self.pose.theta) distance_to_start = math.sqrt((start_x - self.pose.x)**2 + (start_y - self.pose.y)**2) self.move(distance_to_start) self.set_pen(0, 0, 0, 5, 0) # 落笔 # 绘制当前数字 self.draw_digit(digits[digit]) # 更新下一个数字起始位置 start_x += 2.0 # 数字间距 rospy.loginfo("Drawing completed!") if __name__ == '__main__': try: drawer = DrawNumber() drawer.run() except rospy.ROSInterruptException: pass ``` ##### 3. 设置执行权限 ```bash chmod +x draw_228702229.py ``` #### 运行步骤 1. **启动ROS核心**： ```bash roscore ``` 2. **启动小乌龟模拟器**（新终端）： ```bash rosrun turtlesim turtlesim_node ``` 3. **运行绘图节点**（新终端）： ```bash cd ~/turtle_draw_ws source devel/setup.bash rosrun draw_numbers draw_228702229.py ``` #### 效果说明 - 乌龟会按顺序绘制数字"228702229" - 每个数字完成后抬笔移动到下一个位置 - 轨迹由直线段组成，通过旋转控制方向 - 数字间距约2个乌龟单位，确保不重叠 #### 优化建议 1. **轨迹精度**：调整`digits`字典中的距离/角度参数微调形状 2. **绘制速度**：修改`vel_msg.linear.x`值控制移动速度 3. **颜色设置**：通过`set_pen(r,g,b,width,off)`更改笔刷颜色和粗细 4. **初始位置**：修改`start_x, start_y`调整绘制起始点 ### 相关问题 1. 如何让乌龟绘制更复杂的图形（如汉字或logo）？ 2. 如何保存乌龟的运动轨迹为图像文件？ 3. ROS中如何控制多只乌龟同时绘制？ 4. 乌龟碰撞检测如何实现？[^1] 5. 如何优化轨迹生成算法减少绘制时间？[^2] [^1]: ROS基础操作与乌龟模拟器控制 [^2]: 机器人路径规划算法优化

相关知识

如何控制宠物狗的体重（从饮食、运动到心理健康）
从饮食、运动到心理，全方位呵护你的毛孩（从饮食、运动到心理）
如何控制泰迪犬的体重（从饮食、运动到日常护理）
ROS机器人在宠物领域的应用
插画教程之猫咪
简笔画教程之柯基
插画教程之宠物猫咪
插画教程之沙发上的法斗犬
调理狗狗肠胃，让宠物更健康（从饮食、运动到生活习惯）
插画教程之萌宠萨摩耶

网址: ROS动作编程之控制小乌龟运动到目标位置 https://www.mcbbbk.com/newsview1313029.html

上一篇: 小仓鼠可以吃什么蔬菜水果列表

下一篇: 实验动物饲养管理与使用委员会（I