Get best of the week

台车机器人2.0。第2部分。rviz中的管理和不存在中的管理

在关于自动驾驶家用车2.0 的上一篇文章中，我们设法研究了如何改善预算机器人的里程表，使用slam算法和负担得起的激光雷达实现可接受的2d房间地图的构建，在构建项目时阐明其他问题。这次，让我们看看自主导航如何在rviz中工作，负责什么，我们将实现允许您离开rviz的程序。还考虑rviz的一些“美容要素”，这些要素使ROS机器人的生活变得更轻松。

图片

让我们从练习开始。


机器人的任务：独立地从A点移动到B点，或者更好地，立即移动到B点。

假定在其上构建该项目的linorobot项目的脚本（节点）将在机器人上启动，并对发生的事情进行可视化并进行控制从外部PC，虚拟机（以下称为“ PC”）。

就我而言，是从购物车中的软件（其核心是树莓派3b单板）安装的：在PC上的Ubuntu 16.04，ROS Kinetic，在PC上：Ubuntu 18.04，ROS Melodic。

在去某个地方之前，必须设置机器人的尺寸-长度和宽度。
为此，如项目页面所示，您必须输入适当的值：

roscd linorobot/param/navigation
nano costmap_common_params.yaml

正确的足迹参数：

footprint: [[-x, -y], [-x, y], [x, y], [x, -y]]

其中x =机械手的长度/ 2和y =宽度/ 2
*理想地，对于机械手，您可以绘制urdf模型，如先前在机械手推车的上一个项目中所做的那样。

但是，如果不这样做，导航将不会受到太大影响，而是“ TF轴”将在地图上移动，而不是机器人模型。如何通过在地图上稍微展示一下机器人的视觉效果来改善情况，同时又不浪费时间构建urdf模型，我们将进一步讨论。

我们开始一个环境


在机器人上，在两个不同的终端中执行：

roslaunch linorobot bringup.launch
roslaunch linorobot navigate.launch

在PC上，运行rviz visual shell：
roscd lino_visualize/rviz
rviz -d navigate.rviz

如果一切顺利，则可以在PC上观察机器人在rviz中的位置：



TF轴在rviz中可见，并且可以在“显示”面板上添加Poligon选项。多边形-这些只是开始时介绍的机器人的占地面积大小，可让您查看机器人的尺寸。 * TF轴从图片中的轴可见：TF-map-地图和TF-base-link-机器人本身，其余轴在rviz中被静音，以免彼此合并。

来自激光雷达的数据与地图上的墙壁略有不同，机器人没有定位。

如果从该位置移开，算法将尝试进行调整，但是您也可以通过使用“二维姿势估计”按钮直接在rviz中指定起始位置来提供帮助：



接下来，在地图上的rviz可视化窗口中，用绿色的粗箭头指示机器人的初始位置和方向-初始姿势（您可以多次执行此操作）：



明确位置后，base_link相对于地图将略有偏移：



手动调整初始位置时，激光雷达与地图的边界重合的绝对精度不会然后，该算法将自行纠正所有问题。

反正你去之前


每次将rviz放置在起始位置，如果机器人在地面上这样从某处开始就这样占领。因此，我们将使用该服务（请记住，除了ROS中的主题之外还有服务），在访问该服务时，它将读取“机器人在地图上的位置”，然后我们将使用此数据。

在PC上创建脚本（在哪里都无所谓）

get_pose.py
#! /usr/bin/env python
import rospy
from std_srvs.srv import Empty, EmptyResponse # Import the service message python classes generated from Empty.srv.
from geometry_msgs.msg import PoseWithCovarianceStamped, Pose
robot_pose = Pose()
def service_callback(request):
    print ("Robot Pose:")
    print (robot_pose)
    return EmptyResponse() # the service Response class, in this case EmptyResponse    
def sub_callback(msg):
    global robot_pose
    robot_pose = msg.pose.pose   
rospy.init_node('service_server') 
my_service = rospy.Service('/get_pose_service', Empty , service_callback) # create the Service called get_pose_service with the defined callback
sub_pose = rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped, sub_callback)
rospy.spin() # mantain the service open.


启动后，终端中的脚本将不显示任何内容，并安静地等待对其进行访问。由于在rviz中使用图形界面，我们最近设置了机器人的初始位置，因此您可以参考我们的新脚本并找出该位置。在不使用脚本关闭终端的情况下（例如之前启动的带有节点的终端），在另一个终端中，我们将执行：

rosservice call /get_pose_service

在终端中使用脚本执行请求后，get_pose.py将显示机械手的位置：



保存这些指示符并将它们添加到机械手上amcl节点的设置中，以便每次操作时从指定位置启动机械手：

nano linorobot/launch/include/amcl.launch

加
参数
<param name="initial_pose_x" value="0.548767569629"/>
<param name="initial_pose_y" value="0.218281839179"/>
<param name="initial_pose_z" value="0.0"/>
<param name="initial_orientation_z" value="0.128591756735"/>
<param name="initial_orientation_w" value="0.991697615254"/>


现在，在启动时，机器人将已经从某个位置启动，并且也不需要在rviz中手动设置它：



终于要去了


我们将重新启动开头给出的机器人和PC的命令，并在rviz中查看机器人。

我们将使用“二维NAV目标” rviz将机器人发送到灵魂所希望的任何地方：



如您所见，机器人不仅可以指示行程距离，还可以指示运动方向。
在上面的示例中，我们向前行驶一点，转身后退。

在rviz中，还可以看到熟悉的“绿色箭头” Amcl粒子群，根据程序显示了机器人所在的位置。旅行后，他们的注意力接近机器人的真实位置，总数减少了。机器人的运行速度足够快-0.3 m / s，因此该程序需要更多时间来定位机器人。

使用“ 2D NAV目标”在rviz中导航以了解机器人行进的距离和方向。但是更方便的是没有操作员参与rviz的旅行。

因此，我们在地图上突出显示了我们要去的景点。为此，请使用“ 2D NAV Goal”将机器人移动到地图上的所需点。此时，它将访问get_pose.py服务。接下来，所有点的坐标将被传送到旅行程序。

为什么不仅仅将您手中的机器人转移到兴趣点呢？问题是构建它们的算法“ Amcl粒子群”。即使机器人位于关注点，并且手动指定了其位置，其位置的变体也将“喷涂”在关注点自身周围的区域上。因此，您仍然必须走。

让我们从代号为“ corridor”的点开始：



我们将通过调用先前创建的服务来保存机器人在所需位置时的姿势：

rosservice call /get_pose_service

接下来-“入口”：



最后一点-“厨房”：



现在是时候创建旅行计划了-

MoveTBtoGoalPoints.py
#!/usr/bin/env python
import rospy
import actionlib
# Use the actionlib package for client and server
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
# Define Goal Points and orientations for TurtleBot in a list
"""
pose x,y,x; orientation: x,y,z,w
"""
GoalPoints = [ [(5.31514576482, 1.36578380326, 0.0), (0.0, 0.0, -0.498454937648, 0.866915610157)], #corridor
		[(5.77521810772, -1.2464049907, 0.0), (0.0, 0.0,  -0.959487358308, 0.281751680114)], #prixozaya
		[(2.14926455346, -2.7055208156, 0.0), (0.0, 0.0,  -0.99147032254, 0.130332649487)]] #kitchen
# The function assign_goal initializes goal_pose variable as a MoveBaseGoal action type.
def assign_goal(pose):
    goal_pose = MoveBaseGoal()
    goal_pose.target_pose.header.frame_id = 'map'
    goal_pose.target_pose.pose.position.x = pose[0][0]
    goal_pose.target_pose.pose.position.y = pose[0][1]
    goal_pose.target_pose.pose.position.z = pose[0][2]
    goal_pose.target_pose.pose.orientation.x = pose[1][0]
    goal_pose.target_pose.pose.orientation.y = pose[1][1]
    goal_pose.target_pose.pose.orientation.z = pose[1][2]
    goal_pose.target_pose.pose.orientation.w = pose[1][3]
 
    return goal_pose
if __name__ == '__main__':
    rospy.init_node('MoveTBtoGoalPoints')
    # Create a SimpleActionClient of a move_base action type and wait for server.
    client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
    client.wait_for_server()
    # for each goal point in the list, call the action server and move to goal
    for TBpose in GoalPoints:
        TBgoal = assign_goal(TBpose)
        # For each goal point assign pose
        client.send_goal(TBgoal)
        client.wait_for_result()
        # print the results to the screen
        #if(client.get_state() == GoalStatus.SUCCEEDED):
        #    rospy.loginfo("success")
        #else:        
        #   rospy.loginfo("failed")


*在GoalPoints中，我们将添加之前保存的行程点坐标。代码中有一个解释，说明参数按顺序排列：pose x，y，x;方向：x，y，z，w-[（5.31514576482，1.36578380326，0.0），（0.0，0.0，-0.498454937648，0.866915610157）]

与先前在机器人和PC上运行的节点并行运行MoveTBtoGoalPoints.py脚本，请参见结果：



如您所见，机器人成功地驶向了走廊，并在去厨房的路上看见自己正对着镜子并被卡住。不幸的是，在与镜子会面时，请使用激光雷达。

该怎么办？

可以在反光镜之前和之后添加中间点，降低机器人的速度，沿着路径在反光镜上粘贴不透明的胶带。

但这是下一步，但是现在我们将考虑在没有完成rviz的情况下从A点到B点的任务。现在，原则上，我们不需要外部PC，因为旅行脚本会将我们带到地图上的所需点。

但是，我们不要急于将PC与rviz一起丢弃，因为linorobot项目无法“开箱即用”地正确运行，并且您需要在导航堆栈上“处理文件”。

现在是时候开始反对了-让我说说，自主导航在哪里，机器人向前，向后，向后，甚至驶向后视镜前进了n个步？

好，仅此而已，除了使ROS脱颖而出的一些细节。

让我们看看这些细节。

假设一些不负责任的公民沿着机器人的路径留下了无生命的物体。此项目不在提供给机器人进行导航的地图上：



将会发生什么？

让我们看看：



如您所见，激光雷达看到了一个障碍物，算法在地图上将其标记并下令绕行。

最后，为rviz添加一些美感


rviz可视编辑器具有有趣的插件，默认情况下未安装。在使用rviz时，它们的使用可以增加很多有趣的观点。

带有插件的存储库在这里。

在我们的环境中的PC上安装插件：

roscd; cd ../src;
catkin_create_pkg my_rviz_markers rospy
cd my_rviz_markers/
cd src
git clone https://github.com/jsk-ros-pkg/jsk_visualization
git clone https://github.com/ClementLeBihan/rviz_layer
cd ..
rosdep install --from-paths -y -r src
cd ~/linorobot_ws/
catkin_make

在rviz中安装后，应显示其他插件。

现在，让我们添加例如图标：



在my_rviz_markers / samples文件夹中，创建一个脚本：

pictogram_array_objects_demo.py
#!/usr/bin/env python
import rospy
import math
from jsk_rviz_plugins.msg import Pictogram, PictogramArray
from random import random, choice
rospy.init_node("pictogram_object_demo_node")
p = rospy.Publisher("/pictogram_array", PictogramArray,  queue_size=1)

r = rospy.Rate(1)
actions = [Pictogram.JUMP, Pictogram.JUMP_ONCE, Pictogram.ADD, 
           Pictogram.ROTATE_X, Pictogram.ROTATE_Y, Pictogram.ROTATE_Z]
pictograms = ["home","fa-robot"]
object_frames = ["map","base_link"]

while not rospy.is_shutdown():
    
    arr = PictogramArray()
    arr.header.frame_id = "/map"
    arr.header.stamp = rospy.Time.now()
    for index, character in enumerate(pictograms):
        msg = Pictogram()
        msg.header.frame_id = object_frames[index]
        msg.action = actions[2]
        msg.header.stamp = rospy.Time.now()
        msg.pose.position.x = 0
        msg.pose.position.y = 0
        msg.pose.position.z = 0.5
        # It has to be like this to have them vertically orient the icons.
        msg.pose.orientation.w = 0.7
        msg.pose.orientation.x = 0
        msg.pose.orientation.y = -0.7
        msg.pose.orientation.z = 0
        msg.size = 0.5
        msg.color.r = 25 / 255.0
        msg.color.g = 255 / 255.0
        msg.color.b = 240 / 255.0
        msg.color.a = 1.0
        msg.character = character
        arr.pictograms.append(msg)
    p.publish(arr)
    r.sleep()


让我们使其可执行：

sudo chmod +x pictogram_array_objects_demo.py

在my_rviz_markers /启动文件夹中，创建要启动的启动：

<launch>
  <node pkg="my_rviz_markers" type="pictogram_array_objects_demo.py" name="pictogram_array_objects_demo" </node>
</launch>

它照常启动（此时所有其他节点也应正常工作）：

roslaunch my_rviz_markers pictogram_array_objects_demo.launch

将标记放在地图上


标记有助于理解该点在地图上的位置。

与前面的示例一样，在my_rviz_markers / samples文件夹中创建一个脚本：

basic_marker.py
#!/usr/bin/env python

import rospy
from visualization_msgs.msg import Marker
from geometry_msgs.msg import Point


class MarkerBasics(object):
    def __init__(self):
        self.marker_objectlisher = rospy.Publisher('/marker_basic', Marker, queue_size=1)
        self.rate = rospy.Rate(1)
        self.init_marker(index=0,z_val=0)
    
    def init_marker(self,index=0, z_val=0):
        self.marker_object = Marker()
        self.marker_object.header.frame_id = "/map"
        self.marker_object.header.stamp    = rospy.get_rostime()
        self.marker_object.ns = "start"
        self.marker_object.id = index
        self.marker_object.type = Marker.SPHERE
        self.marker_object.action = Marker.ADD

        my_point = Point()
        my_point.z = z_val
        #self.marker_object.pose.position = my_point
        self.marker_object.pose.position.x = 5.31514576482
        self.marker_object.pose.position.y = 1.36578380326 
        self.marker_object.pose.orientation.x = 0
        self.marker_object.pose.orientation.y = 0
        self.marker_object.pose.orientation.z = -0.498454937648
        self.marker_object.pose.orientation.w = 0.866915610157
        self.marker_object.scale.x = 0.2
        self.marker_object.scale.y = 0.2
        self.marker_object.scale.z = 0.2
    
        self.marker_object.color.r = 0.0
        self.marker_object.color.g = 0.0
        self.marker_object.color.b = 1.0
        # This has to be, otherwise it will be transparent
        self.marker_object.color.a = 1.0

        # If we want it for ever, 0, otherwise seconds before desapearing
        self.marker_object.lifetime = rospy.Duration(0)
    
    def start(self):
        while not rospy.is_shutdown():
            self.marker_objectlisher.publish(self.marker_object)
            self.rate.sleep()
   
if __name__ == '__main__':
    rospy.init_node('marker_basic_node', anonymous=True)
    markerbasics_object = MarkerBasics()
    try:
        markerbasics_object.start()
    except rospy.ROSInterruptException:
        pass


在my_rviz_markers /启动文件夹中，创建要启动的启动：

basic_marker.launch
<launch>
  <node pkg="my_rviz_markers" type="basic_marker.py" name="basic_marker" />
</launch>


跑：

roslaunch my_rviz_markers basic_marker.launch

添加到rviz：



添加图形


这将显示两个框架之间的距离和角度：



在my_rviz_markers /启动文件夹中，创建要启动的启动：

overlay_meny_my.launch overlay_menu_my.launch
<launch>
  <node pkg="my_rviz_markers" type="haro_overlay_complete_demo.py" name="overlay_menu" />
</launch>


在my_rviz_markers / samples文件夹中，创建脚本：

haro_overlay_complete_demo.py
#!/usr/bin/env python

from jsk_rviz_plugins.msg import OverlayText, OverlayMenu
from std_msgs.msg import ColorRGBA, Float32
import rospy
import math
import random
from geometry_msgs.msg import Twist

class HaroOverlay(object):
    def __init__(self):
        
        self.text_pub = rospy.Publisher("/text_sample", OverlayText, queue_size=1)
        
        self.plot_value_pub = rospy.Publisher("/plot_value_sample", Float32, queue_size=1)
        self.piechart_value_pub = rospy.Publisher("/piechart_value_sample", Float32, queue_size=1)
        self.menu_publisher = rospy.Publisher("/test_menu", OverlayMenu, queue_size=1)
        self.plot_value = 0
        self.piechart_value = 0
        self.max_distance_from_object = 10.0
        
        self.subs = rospy.Subscriber("/haro_base_link_to_person_standing_tf_translation", Twist, self.twist_callback)
        
        self.counter = 0
        self.rate = rospy.Rate(100)
        self.overlaytext = self.update_overlaytext()
        self.menu = self.update_overlay_menu()

    def twist_callback(self, msg):
        self.plot_value = msg.linear.x
        self.piechart_value = msg.angular.z

    def update_overlaytext(self, number=1.23, number2=20):  
        
        text = OverlayText()
        text.width = 200
        text.height = 400
        text.left = 10
        text.top = 10
        text.text_size = 12
        text.line_width = 2
        text.font = "DejaVu Sans Mono"
        text.text = """Distance=  %s.
        Angle=%s.
        Counter = <span style="color: green;">%d.</span>
        """ % (str(number), str(number2) ,self.counter)
        text.fg_color = ColorRGBA(25 / 255.0, 1.0, 240.0 / 255.0, 1.0)
        text.bg_color = ColorRGBA(0.0, 0.0, 0.0, 0.2)
        
        return text
    
    def update_overlay_textonly(self, new_text):
        self.overlaytext.text = new_text


    def update_overlay_menu(self):
        menu = OverlayMenu()
        menu.title = "HaroSystemMode"
        menu.menus = ["Sleep", "Searching", "MovingInCircles","Waiting"]
        
        menu.current_index = self.counter % len(menu.menus)
        return menu
        
    def update_overlay_menu_haro_tf(self):
        menu = OverlayMenu()
        menu.title = "HaroDistanceFromPerson"
        menu.menus = ["FarAway", "CloseBy", "Target", "OtherWayRound"]
        
        fraction = 10.0
        
        if self.piechart_value < (math.pi/fraction):
            if self.plot_value >= self.max_distance_from_object:
                index = 0
            elif self.plot_value >= (self.max_distance_from_object/ fraction) and self.plot_value < self.max_distance_from_object:
                index = 1
            elif self.plot_value < (self.max_distance_from_object/fraction):
                index = 2
            
        else:
            index = 3
            
        menu.current_index = index
            
        return menu

    def start_dummy_demo(self):
        
        while not rospy.is_shutdown():
            self.overlaytext = self.update_overlaytext()
            self.menu = self.update_overlay_menu()
            
            if self.counter % 100 == 0:
                self.menu.action = OverlayMenu.ACTION_CLOSE
            
            self.text_pub.publish(self.overlaytext)
            # If values are very high it autoadjusts so be careful
            self.value_pub.publish(math.cos(self.counter * math.pi * 2 / 1000))
            self.menu_publisher.publish(self.menu)
            
            self.rate.sleep()
            self.counter += 1
    
    def start_harodistance_demo(self):
        
        while not rospy.is_shutdown():
            self.overlaytext = self.update_overlaytext(number=self.plot_value, number2=self.piechart_value)
            self.menu = self.update_overlay_menu_haro_tf()
            
            self.text_pub.publish(self.overlaytext)
            
            self.plot_value_pub.publish(self.plot_value)
            self.piechart_value_pub.publish(self.piechart_value)
            
            self.menu_publisher.publish(self.menu)
            
            self.rate.sleep()
            self.counter += 1
  
  
def dummy_overlay_demo():
    rospy.init_node('distance_overlay_demo_node', anonymous=True)
    haro_overlay_object = HaroOverlay()
    try:
        #haro_overlay_object.start_dummy_demo()
        haro_overlay_object.start_harodistance_demo()
    except rospy.ROSInterruptException:
        pass

if __name__ == '__main__':
    dummy_overlay_demo()


第二个脚本是

tf_haro_to_object_listener.py
#!/usr/bin/env python
import sys
import rospy
import math
import tf
from geometry_msgs.msg import Twist, Vector3

if __name__ == '__main__':
    rospy.init_node('tf_listener_haro_to_person')

    listener = tf.TransformListener()

    if len(sys.argv) < 3:
        print("usage: tf_haro_to_object_listener.py parent child")
    else:
        follower_model_name = sys.argv[1]
        model_to_be_followed_name = sys.argv[2]
        
        topic_to_publish_name = "/"+str(follower_model_name)+"_to_"+str(model_to_be_followed_name)+"_tf_translation"
        # We will publish a Twist message but it's positional data not speed, just reusing an existing structure.
        tf_translation = rospy.Publisher(topic_to_publish_name, Twist ,queue_size=1)
        
        rate = rospy.Rate(10.0)
        ctrl_c = False
        
        follower_model_frame = "/"+follower_model_name
        model_to_be_followed_frame = "/"+model_to_be_followed_name
        
        def shutdownhook():
            # works better than the rospy.is_shut_down()
            global ctrl_c
            ctrl_c = True

        rospy.on_shutdown(shutdownhook)
        
        while not ctrl_c:
            try:
                (trans,rot) = listener.lookupTransform(follower_model_frame, model_to_be_followed_frame, rospy.Time(0))
                translation_object = Vector3()
                translation_object.x = trans[0]
                translation_object.y = trans[1]
                translation_object.z = trans[2]
                
                angular = math.atan2(translation_object.y, translation_object.x)
                linear = math.sqrt(translation_object.x ** 2 + translation_object.y ** 2)
                cmd = Twist()
                cmd.linear.x = linear
                cmd.angular.z = angular
                tf_translation.publish(cmd)
                
            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                continue
            rate.sleep()


它是这样开始的（相信所有其他主要节点和rviz已经在运行）：

roslaunch linorobot overlay_meny_my.launch
python tf_haro_to_object_listener.py base_link map

我们将看到base_link框架相对于地图的角度和距离。

未完待续。

More articles:


All Articles