rbx1中的follow

最新推荐文章于 2025-07-07 11:48:10 发布

原创最新推荐文章于 2025-07-07 11:48:10 发布 · 564 阅读

0 ·

CC 4.0 BY-SA版权

文章标签：

#follow

机器人同时被 2 个专栏收录

13 篇文章

订阅专栏

ros

12 篇文章

订阅专栏

部署运行你感兴趣的模型镜像

github地址：https://github.com/robotBing/openrobot_follower.git

视频展示地址：http://blog.jiutucao.com:9000/upload/2018/07/color_follow.mp4

1. 基本功能

1）跟随人行走

2）语音控制开始或停止

3）颜色标定

2. 跟随实现

1）代码:

#!/usr/bin/env python

import rospy

from roslib import message

from std_msgs.msg import String

from sensor_msgs import point_cloud2

from sensor_msgs.msg import PointCloud2

from geometry_msgs.msg import Twist

from math import copysign

class Follower():

int status = 0

def __init__(self):

rospy.init_node("follower")

rospy.on_shutdown(self.shutdown)

self.goal_z = rospy.get_param("~goal_z", 0.6)

self.z_threshold = rospy.get_param("~z_threshold", 0.05)

self.x_threshold = rospy.get_param("~x_threshold", 0.05)

self.z_scale = rospy.get_param("~z_scale", 1.0)

self.x_scale = rospy.get_param("~x_scale", 2.5)

self.max_angular_speed = rospy.get_param("~max_angular_speed", 2.0)

self.min_angular_speed = rospy.get_param("~min_angular_speed", 0.0)

self.max_linear_speed = rospy.get_param("~max_linear_speed", 0.3)

self.min_linear_speed = rospy.get_param("~min_linear_speed", 0.1

self.low_down_factor = rospy.get_param("~slow_down_factor", 0.8)

self.move_cmd = Twist()

self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=5)

self.depth_subscriber = rospy.Subscriber('point_cloud', PointCloud2, self.set_cmd_vel, queue_size=1)

rospy.Subscriber('/recognizer/output', String, self.speechcb)

rospy.loginfo("Subscribing to point cloud...")

rospy.wait_for_message('point_cloud', PointCloud2)

rospy.loginfo("Ready to follow!"

def speechcb(self,msg):

command = msg.data

if command == 'stop':

self.status = 0

else if command == 'move' :

self.status = 1

def set_cmd_vel(self, msg):

x = y = z = n = 0

for point in point_cloud2.read_points(msg, skip_nans=True):

pt_x = point[0]

pt_y = point[1]

pt_z = point[2]

x += pt_x

y += pt_y

z += pt_z

n += 1

if n:

x /= n

y /= n

z /= n

if (abs(z - self.goal_z) > self.z_threshold):

linear_speed = (z - self.goal_z) * self.z_scale

self.move_cmd.linear.x = copysign(max(self.min_line

ar_speed,

min(self.max_linear_speed, abs(linear_speed))), linear_speed)

else:

self.move_cmd.linear.x *= self.slow_down_factor

if (abs(x) > self.x_threshold):

angular_speed = -x * self.x_scale

self.move_cmd.angular.z = copysign(max(self.min_angular_speed,

min(self.max_angular_speed, abs(angular_speed))), angular_speed)

else:

self.move_cmd.angular.z *= self.slow_down_factor

else:

self.move_cmd.linear.x *= self.slow_down_factor

self.move_cmd.angular.z *= self.slow_down_factor

if self.status == 0:

self.cmd_vel_pub.publish(twist())

else:

self.cmd_vel_pub.publish(self.move_cmd)

def shutdown(self):

rospy.loginfo("Stopping the robot...")

self.depth_subscriber.unregister()

rospy.sleep(1)

self.cmd_vel_pub.publish(Twist())

rospy.sleep(1)

if __name__ == '__main__':

try:

Follower()

rospy.spin()

except rospy.ROSInterruptException:

rospy.loginfo("Follower node terminated.")

2）基本思路

订阅点云（pointcloud2）话题，点云数据包括图像的宽度，高度和一个个点的信息。

根据点云数据找出人的质心点，然后将质心点和图像的中间点和设定的距离点比较，

左右偏移则往/cmd_vel话题上发布左右消息，前后则发布前后消息

3） launch文件

filter_field_name: z

filter_limit_min: 0.3

filter_limit_max: 1.6

filter_limit_negative: False

leaf_size: 0.02

</rosparam>

</node>

filter_field_name: x

filter_limit_min: -0.3

filter_limit_max: 0.3

filter_limit_negative: False

</rosparam>

</node>

filter_field_name: y

filter_limit_min: -0.5

filter_limit_max: -0.1

filter_limit_negative: False

</rosparam>

</node>

goal_z: 0.8

z_threshold: 0.025

x_threshold: 0.025

z_scale: 1.0

x_scale: 3.0

max_angular_speed: 5.0

min_angular_speed: 0.1

max_linear_speed: 0.4

min_linear_speed: 0.05

</rosparam>

</node>

</launch>

4）launch详解

nodelet是ros自带的节点管理系统，类似于java里的多线程

利用VoxelGrid算法，对pointcloud的深度信息过滤

然后利用passthrough算法，对x ，y坐标过滤

过滤可以降低计算量，又不会影响follow结果

最后启动follow节点

remap表示话题的映射，相当于给话题改名

5）参数详解

filter_field_name: 选择过滤的坐标系（x，y，z）

filter_limit_min: 最短的过滤信息，即最短的检测距离

filter_limit_max: 最大的过滤信息，即最大的检测距离

leaf_size: 算法切块的大小

goal_z: 距离目标的距离

z_threshold: 深度偏差量

x_threshold: x方向偏差量，即允许人在图像的哪个范围

z_scale: 距离权重，控制线速度

x_scale: x方向的权重，控制角速度

max_angular_speed: 最大角速度

min_angular_speed: 最小角速度

max_linear_speed: 最大线速度

min_linear_speed: 最小线速度

3. 语音实现

1）概述

利用ros自带的语音识别包pocketsphinx，订阅/input/recognizer话题，获得识别结果，然后给follow函数设置一个使能变量，在语音的回调函数里对使能变量做操作

2）环境配置

sudo apt-get install ros-indigo-turtlebot-bringup \

ros-indigo-turtlebot-create-desktop ros-indigo-openni-* \

ros-indigo-openni2-* ros-indigo-freenect-* ros-indigo-usb-cam \

ros-indigo-laser-* ros-indigo-hokuyo-node \

ros-indigo-audio-common gstreamer0.10-pocketsphinx \

ros-indigo-pocketsphinx ros-indigo-slam-gmapping \

ros-indigo-joystick-drivers python-rosinstall \

ros-indigo-orocos-kdl ros-indigo-python-orocos-kdl \

python-setuptools ros-indigo-dynamixel-motor-* \

libopencv-dev python-opencv ros-indigo-vision-opencv \

ros-indigo-depthimage-to-laserscan ros-indigo-arbotix-* \

ros-indigo-turtlebot-teleop ros-indigo-move-base \

ros-indigo-map-server ros-indigo-fake-localization \

ros-indigo-amcl git subversion mercurial

3）代码重写

#!/usr/bin/env python

import rospy

from roslib import message

from sensor_msgs import point_cloud2

from sensor_msgs.msg import PointCloud2

from geometry_msgs.msg import Twist

from std_msgs.msg import String

from math import copysig

class Follower():

def __init__(self):

rospy.init_node("follower")

rospy.on_shutdown(self.shutdown)

self.goal_z = rospy.get_param("~goal_z", 1.5)

self.z_threshold = rospy.get_param("~z_threshold", 0.05)

self.x_threshold = rospy.get_param("~x_threshold", 0.05)

self.z_scale = rospy.get_param("~z_scale", 0.7)

self.x_scale = rospy.get_param("~x_scale", 2.2)

self.max_angular_speed = rospy.get_param("~max_angular_speed", 2.0)

self.min_angular_speed = rospy.get_param("~min_angular_speed", 0.0) self.max_linear_speed = rospy.get_param("~max_linear_speed", 0.3)

self.min_linear_speed = rospy.get_param("~min_linear_speed", 0.1)

self.slow_down_factor = rospy.get_param("~slow_down_factor", 0.8)

self.move_cmd = Twist(）

self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)

self.sta = 1

self.speech_sub = rospy.Subscriber('recognizer/output',String,self.speechcb,queue_size=1)

# Subscribe to the point cloud

self.depth_subscriber = rospy.Subscriber('point_cloud', PointCloud2, self.set_cmd_vel, queue_size=10)

rospy.loginfo("Subscribing to point cloud...")

# Wait for the pointcloud topic to become available

rospy.wait_for_message('point_cloud', PointCloud2)

rospy.loginfo("Ready to follow!")

def speechcb(self,msg):

if msg.data == 'stop':

self.sta=0

elif msg.data == 'move':

self.sta=1

def set_cmd_vel(self, msg):

# Initialize the centroid coordinates point count

x = y = z = n = 0

# Read in the x, y, z coordinates of all points in the cloud

for point in point_cloud2.read_points(msg, skip_nans=True):

pt_x = point[0]

pt_y = point[1]

pt_z = point[2]

x += pt_x

y += pt_y

z += pt_z

n += 1

if n:

x /= n

y /= n

z /= n

if (abs(z - self.goal_z) > self.z_threshold):

linear_speed = (z - self.goal_z) * self.z_scale

self.move_cmd.linear.x = copysign(max(self.min_linear_speed,

min(self.max_linear_speed, abs(linear_speed))), linear_speed)

else:

self.move_cmd.linear.x *= self.slow_down_factor

if (abs(x) > self.x_threshold):

angular_speed = -x * self.x_scale

self.move_cmd.angular.z = copysign(max(self.min_angular_speed,

min(self.max_angular_speed, abs(angular_speed))), angular_speed)

else:

self.move_cmd.angular.z *= self.slow_down_factor

else:

self.move_cmd.linear.x *= self.slow_down_factor

self.move_cmd.angular.z *= self.slow_down_factor

if self.sta == 0:

self.cmd_vel_pub.publish(Twist())

else:

self.cmd_vel_pub.publish(self.move_cmd)

def shutdown(self):

rospy.loginfo("Stopping the robot...")

self.depth_subscriber.unregister()

rospy.sleep(1)

self.cmd_vel_pub.publish(Twist())

rospy.sleep(1)

if __name__ == '__main__':

try:

Follower()

rospy.spin()

except rospy.ROSInterruptException:

rospy.loginfo("Follower node terminated.")

4）Launch重写

</node>

filter_field_name: z

filter_limit_min: 0.3

filter_limit_max: 1.6

filter_limit_negative: False

leaf_size: 0.02

</rosparam>

</node>

filter_field_name: x

filter_limit_min: -0.3

filter_limit_max: 0.3

filter_limit_negative: False

</rosparam>

</node>

filter_field_name: y

filter_limit_min: -0.5

filter_limit_max: -0.1

filter_limit_negative: False

</rosparam>

</node>

goal_z: 1.2

z_threshold: 0.025

x_threshold: 0.025

z_scale: 0.6

x_scale: 2.1

max_angular_speed: 3.0

min_angular_speed: 0.1

max_linear_speed: 0.4

min_linear_speed: 0.05

</rosparam>

</node>

</launch>

4. 颜色标定实现

1 ) 概述

利用opencv提供的python接口，处理图像信息，利用cv_bridge将ros话题上发布的摄像头消息数据转化成为opencv可以处理的图像数据。

2 ）代码见github项目

3 ) launch

</node>

rate: 20

max_z: 2.0 # How far out do we want to detect

min_z: 0.1

goal_z: 0.7

z_threshold: 0.1

x_threshold: 0.3

z_scale: 1.0 # forward/back scale

x_scale: 1.3 # left/right scale

max_rotation_speed: 1.0

min_rotation_speed: 0.1

max_linear_speed: 0.2

min_linear_speed: 0.02

scale_roi: 0.9

</rosparam>

</node>

</launch>

4 ）launch详解

rate: 消息回调速度，一般不用动，如果出现卡顿再改动

max_z: 检测的最大距离

min_z: 检测的最小距离

goal_z: 保持机器和目标的距离

z_threshold: 距离的可控区域

x_threshold: 图像的可控区域，即在屏幕上画个矩形，让目标保持在矩形内

z_scale: 线速度的权值

x_scale: 角速度的权值

max_rotation_speed: 最大角速度

min_rotation_speed: 最小角速度

max_linear_speed: 最大线速度

min_linear_speed: 最小线速度

scale_roi: 对roi话题上发布的信息的缩小范围

1. 运行底盘驱动

rosrun openrobot Node_p_chassis

2 .打开摄像头驱动（实验室用的奥比中光深度摄像头）

roslaunch astra_launch astra.launch

3.启动camshift 发布/roi

roslaunch openrobot_follow camshift.launch

这时候屏幕上会出现3个窗口，在有图像的窗口选择需要跟随的物体。这时候程序会计算物体的颜色，另外两个窗口其中一个会出现颜色的柱状图，另一个窗口出现颜色柱状图。物体移动绿色的框会跟着移动。尽量选中颜色鲜艳的物体。4.启动跟随

roslaunch openrobot_follow color_follow.launch

您可能感兴趣的与本文相关的镜像

Langchain-Chatchat

AI应用

Langchain

Langchain-Chatchat 是一个基于 ChatGLM 等大语言模型和 Langchain 应用框架实现的开源项目，旨在构建一个可以离线部署的本地知识库问答系统。它通过检索增强生成 (RAG) 的方法，让用户能够以自然语言与本地文件、数据库或搜索引擎进行交互，并支持多种大模型和向量数据库的集成，以及提供 WebUI 和 API 服务