diff --git "a/interactive_markers\346\216\247\345\210\266\345\260\217\350\275\246\344\271\213\346\216\247\345\210\266\347\257\207.md" "b/interactive_markers\346\216\247\345\210\266\345\260\217\350\275\246\344\271\213\346\216\247\345\210\266\347\257\207.md" new file mode 100644 index 0000000000000000000000000000000000000000..d0c6a02addbcb2c4ae7204ecc56c6112507d955c --- /dev/null +++ "b/interactive_markers\346\216\247\345\210\266\345\260\217\350\275\246\344\271\213\346\216\247\345\210\266\347\257\207.md" @@ -0,0 +1,266 @@ +# interactive_markers控制小车之控制篇 + +上一篇我们介绍了如何利用interactive_markers创建出一个能在rviz中随鼠标移动的控件。这一篇，我们来给这个虚拟模型注入灵魂，让它能真正让我们的真实机器人跟着它动起来。 + +本文将先略述需要机器人移动的常规做法，然后引入到本例中，最终实现让真实机器跟随虚拟模型移动的目的。 + +## 1、开发环境 + +（1）ubuntu20.04+ROS1(noetic) + +（2）松灵机器人Scout仿真模型（） + + + +## 2、控制机器人移动的常规做法 + +​ 一般而言，我们在做真实机器人ROS开发过程中不会仅仅实现让机器人移动这个目的，而是会给机器一些额外的任务（挖坑、填坑之旅）。最常见的是让机器人在行进过程中边行进边检测，遇到要检测的目标则停止执行转而去做我们实现调度的任务；再者，现在加机械臂的机器可能会在行进过程中进行码垛工作；再者，移动机器人一直面临的路径规划问题，即行进过程中要自己预测路线，自己避障。这些都是在实际项目中我们让机器移动要做的任务。那么一般会怎么去解决呢？ + +​ 以路径规划为例，我们会给它设定一个全局路径规划器，一个局部路径规划器。全局路径规划器负责生成一条从出发点到目标点的“安全”路径；而局部路径规划器则会在机器实时移动的过程中进行避障。 + +​ 是的，这就是我们在实际开发中要做的事情。如果要机器全自动移动，那么就要设定一个全局和一个局部；如果是人为控制，那么是不是就默认了我们知道了全局路径规划器，我们要做的就是做好局部路径规划。 + +​ 举一个通俗的例子就是，我们在移动小海龟的过程中，我们想让小海龟从左下角到有上角是不是会径直走对角呢？其实就是我们大脑默认处理了全局路径规划，那么局部路径规划呢？在你行进过程中，突然出现了几个不能走的点，那你肯定会操控键盘让他不走那。这就是局部路径规划器了。 + +## 3、interactive_markers控制小车 + +​ 到这里，是不是有那么一点感受，我们拖动虚拟控件操控小车就是在已知全局路径的情况下去做好局部路径。这个局部路径怎么做呢？回归到上一讲，我们提到这个做法其实就是模仿小海龟的移动！其实这个东西你就可以理解为做好局部路径规划器！！！ + +​ 接下来，让我们看代码： + + + + + +![img](file:///C:/Users/admin/AppData/Local/Temp/msohtmlclip1/01/clip_image002.jpg) + +## 4、分析代码逻辑 + +**（1）拿到真实机器人的坐标和虚拟机器人的坐标，并将其转换为四元素（方便计算）。** + +这一步的是必须要做的，相信大家也都可以想到。再看具体实现先说listener.lookupTransform。如下图所示，这个函数计算出来的就是trans, rot就是turtle1和turtle2之间的是一个坐标系的变换值，可以看到两个坐标系都是base_link，这里写出来的目的是，方便后续开发。而euler_from_quaternion函数就是将旋转矩阵的坐标值变换成四元素，这一块在后续的文章中我们会再深入的探讨。这就是第一块的运行逻辑和关于代码部分的介绍。 + +![img](file:///C:/Users/admin/AppData/Local/Temp/msohtmlclip1/01/clip_image004.jpg) + + + +**（2）判断虚拟机器人和真实机器人的位置关系。**这里其实是一个纯粹的数学关系，用最简单的欧式距离公式判断距离，反正切函数计算角度即可。如下所示 + +​ distance = sqrt(delta_x * delta_x + delta_y * delta_y) + +​ theta = atan2(delta_y, delta_x) + +**（3）第三块实现的是调整角度，及时处理错误的数据并发放速度** + + 前两个if判断语句是检测真实机器人和虚拟模型的角度差，ifdelta_theta < -pi；如果差值过大，那么直接加上一个-pi即可，原因是整个变换差值为2pi。接下来两个if语句则是负责调整角度的同时发布x轴速度。这样的原因是在角度十分相近的情况下要特别处理好线速度的快慢。所有单独设置了cmd_vel.linear.x = min(0.2, distance * vx_kp)语句用以调整。 + +**（4）最后拿到上述数据，发布Twist** + +cmd_vel_pub.publish(cmd_vel)，此处可参考小海龟移动。 + + + +代码开源： + +``` +#!/usr/bin/env python +# -*- coding: UTF-8 -*- +#author:LQL +import rospy +import copy +import time + +from interactive_markers.interactive_marker_server import * +from interactive_markers.menu_handler import * +from visualization_msgs.msg import * +from geometry_msgs.msg import Point,Pose,Twist +from tf.broadcaster import TransformBroadcaster +from tf.listener import TransformListener +from tf.transformations import * +from std_msgs.msg import String +from geometry_msgs.msg import Quaternion, PointStamped,PoseStamped + + +import actionlib +from actionlib_msgs.msg import * +from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal + +from random import random +from math import * + +server = None +menu_handler = MenuHandler() +br = None +counter = 0 + +global new_goal, pos, vx_kp, theta_kp +vx_kp = 2 +theta_kp=4 +new_goal=False +pos=Pose() + +def processFeedback(feedback): + + global new_goal,pos + pos = feedback.pose + new_goal = True + server.applyChanges() + + +def makeArrow( msg ): + marker = Marker() + + marker.type = Marker.ARROW + marker.scale.x = msg.scale * 2.0 + marker.scale.y = msg.scale * 0.5 + marker.scale.z = msg.scale * 0.5 + marker.color.r = 1 + marker.color.g = 0 + marker.color.b = 0 + marker.color.a = 0.8 + + return marker + +def makeAdd( msg ): + marker = Marker() + + marker.type = Marker.CYLINDER + marker.scale.x = msg.scale * 0.5 + marker.scale.y = msg.scale * 0.5 + marker.scale.z = msg.scale * 3 + marker.color.r = 0 + marker.color.g = 0 + marker.color.b = 1 + marker.color.a = 1.0 + + return marker + +def makeMark(): + marker=Marker() + marker.header.frame_id='base_link' + marker.header.stamp=rospy.Time.now() + marker.ns="basic_shapes" + marker.id=10 + marker.type=Marker.ARROW + marker.scale.x = 0.1 + marker.scale.y = 0.5 + marker.scale.z = 5 + marker.color.r = 0 + marker.color.g = 1 + marker.color.b = 0 + marker.color.a = 0.3 + marker.action=Marker.ADD + marker.lifetime=rospy.Duration() + return marker + + +def makeArrowControl( msg ): + control = InteractiveMarkerControl() + control.always_visible = True + control.markers.append( makeArrow(msg) ) + msg.controls.append( control ) + return control + +def saveMarker( int_marker ): + server.insert(int_marker, processFeedback) + +def makeGoalMarker(position): + int_marker = InteractiveMarker() + int_marker.header.frame_id = "base_link" + int_marker.pose.position = position + int_marker.scale = 0.5 + + int_marker.name = "goal" + int_marker.description = "goal" + + control = InteractiveMarkerControl() + control.orientation.w = 1 + control.orientation.x = 0 + control.orientation.y = 1 + control.orientation.z = 0 + control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS + control.always_visible = True + control.markers.append(makeArrow(int_marker)) + int_marker.controls.append(copy.deepcopy(control)) + control = InteractiveMarkerControl() + control.orientation.w = 1 + control.orientation.x = 0 + control.orientation.y = 1 + control.orientation.z = 0 + control.interaction_mode = InteractiveMarkerControl.MOVE_PLANE + control.markers.append(makeAdd(int_marker)) + + int_marker.controls.append(copy.deepcopy(control)) + del control + + server.insert(int_marker, processFeedback) + menu_handler.apply( server, int_marker.name ) + +if __name__ == "__main__": + rospy.init_node("interactive_controls") + listener= TransformListener() + markerpub=rospy.Publisher('visualization_marker',Marker,queue_size=1) + server = InteractiveMarkerServer("interactive_controls") + while True: + time.sleep(1) + try: + (trans, rot) = listener.lookupTransform('base_link', 'base_link', rospy.Time(0)) + position = Point(trans[0], trans[1], trans[2]+0.2) + print(position) + except Exception: + print(e) + position = Point(0, 0, 0) + else: + break + makeGoalMarker(position) + server.applyChanges() + cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1) + + rate = rospy.Rate(20) + + pi = 3.141592653 + a_tol = 0.2 + pub_count = 0 + + while not rospy.is_shutdown(): + cmd_vel = Twist() + if new_goal: + pub_count = 5 + new_goal = False + (trans, rot) = listener.lookupTransform('base_link', 'base_link', rospy.Time(0)) + x, y = pos.position.x, pos.position.y + x0, y0 = trans[0], trans[1] + delta_x, delta_y = x - x0, y - y0 + distance = sqrt(delta_x * delta_x + delta_y * delta_y) + (roll, pitch, yaw) = euler_from_quaternion(rot) + (roll2, pitch2, yaw2) = euler_from_quaternion((pos.orientation.x,pos.orientation.y,pos.orientation.z,pos.orientation.w)) + theta = atan2(delta_y, delta_x) + delta_theta = theta - yaw + + distance = sqrt(delta_x * delta_x + delta_y * delta_y) + theta = atan2(delta_y, delta_x) + delta_theta = theta - yaw + if delta_theta < -pi: + delta_theta += pi * 2 + elif delta_theta > pi: + delta_theta -= pi * 2 + if pi - a_tol > abs(delta_theta) > a_tol: + cmd_vel.linear.x = min(0.2, distance * vx_kp) + else: + cmd_vel.linear.x = min(0.4, distance * vx_kp) + if delta_theta > pi / 2: + delta_theta = -pi + delta_theta + cmd_vel.linear.x = -cmd_vel.linear.x + elif delta_theta < -pi / 2: + delta_theta = pi - delta_theta + cmd_vel.linear.x = -cmd_vel.linear.x + cmd_vel.angular.z = delta_theta * theta_kp + if pub_count>0: + cmd_vel_pub.publish(cmd_vel) + pub_count -= 1 + markerpub.publish(makeMark()) + rate.sleep() + + +``` +