【仿真】一、基于ArbotiX和rviz仿真器控制机器人运动

一、基于ArbotiX和rviz仿真器控制机器人运动

ROS机器人开发实践: 源码链接

ArbotiX简介

ArbotiX是一款控制电机、舵机的控制板，并提供相应的ROS功能包，但是这个功能包的功能不仅可以驱动真实的ArbotiX控制板，它还提供一个差速控制器，通过接收速度控制指令更新机器人的joint状态，从而帮助我们实现机器人在rviz中的运动。 ArbotiX功能包安装完成后，就可以针对机器人模型进行配置了。配置步骤较为简单，不需要修改机器人的模型文件，只需要创建一个启动ArbotiX节点的launch文件，再创建一个控制器相关的配置文件即可。

安装ArbotiX：
git clone https://github.com/vanadiumlabs/arbotix_ros.git
ROS 软件包都应该放置在你的工作空间中的 src 目录下，需要用到arbotix_msgs和arbotix_python两个文件包
catkin_make
source ./devel/setup.bash

配置ArbotiX控制器：

ArbotiX功能包安装完成后，就可以针对机器人模型进行配置了。配置步骤较为简单，不需要修改机器人的模型文件，只需要创建一个启动ArbotiX节点的launch文件，再创建一个控制器相关的配置文件即可。这个launch文件和之前显示机器人模型的launch文件几乎一致，只是添加了启动arbotix_driver节点的相关内容：

<launch>
    <param name="/use_sim_time" value="false" />
	
	<!-- 加载机器人URDF/Xacro模型 -->
    <arg name="urdf_file" default="$(find xacro)/xacro --inorder '$(find mybot_pkg)/urdf/mrobot_with_rplidar.urdf.xacro'" />
	<param name="robot_description" command="$(arg urdf_file)" />
    
    <arg name="gui" default="false" />
	<param name="use_gui" value="$(arg gui)"/>

	<node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen">
        <rosparam file="$(find mybot_pkg)/config/fake_mrobot_arbotix.yaml" command="load" />
        <param name="sim" value="true"/>
    </node>

	<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />

    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher">
        <param name="publish_frequency" type="double" value="20.0" />
    </node>

	<node name="rviz" pkg="rviz" type="rviz" args="-d $(find mybot_pkg)/config/mrobot_arbotix.rviz" required="true" />
</launch>

<node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen">启动node节点
name="arbotix": 为这个节点设置一个名字，即arbotix。在ROS中，你可以通过这个名字来引用或查找这个节点。
pkg="arbotix_python": 指定这个节点来自arbotix_python这个ROS包。
type="arbotix_driver": 指定要运行的节点的可执行文件的名字是arbotix_driver。这通常是在arbotix_python包的bin或scripts目录下。
<rosparam file="$(find mybot_pkg)/config/fake_mrobot_arbotix.yaml" command="load" />将YAML文件中的参数加载到参数服务器上，command="load"表示覆盖

• 另外，可以看到src/arbotix_python/bin/arbotix_driver用 Python 编写的 ROS 节点。它通过串口与 ArbotiX 控制器进行通信，用于机器人控制。

arbotix_driver可以针对真实控制板进行控制，也可以在仿真环境中使用，需要配置“sim”参数为true。另外，该节点的启动还需要加载控制器相关的配置文件，该配置文件在功能包的config路径下。控制器命名为base_controller，类型是diff_controller，也就是差速控制器，刚好可以控制机器人模型的双轮差速运动。此外，还需要配置参考坐标系、底盘尺寸、PID控制等参数。

controllers: {
   base_controller: {type: diff_controller, base_frame_id: base_footprint, base_width: 0.26, ticks_meter: 4100, Kp: 12, Kd: 12, Ki: 0, Ko: 50, accel_limit: 1.0 }
}

运行launch：roslaunch mybot_pkg arbotix_mrobot_with_laser.launch ，可查看参数服务器rosparam list，查看参数值rosparam get /my_param

实现

首先我们构建一个新环境包demo_ws，构建一个带有激光雷达的arbotix机器人
source ./devel/setup.bash
roslaunch mybot_pkg arbotix_mrobot_with_laser.launch
之后我们需要一个小车控制指令
roslaunch mybot_teleop mrobot_teleop.launch

<launch>
  <node name="mybot_teleop" pkg="mybot_teleop" type="mrobot_teleop.py" output="screen">
    <param name="scale_linear" value="0.1" type="double"/>
    <param name="scale_angular" value="0.4" type="double"/>
    <!-- <remap from="/cmd_vel" to="/turtle1/cmd_vel"/>   -->
  
  </node>
</launch>

原理如下：
启动了一个/mrobot_teleop节点，发布/cmd_vel速度话题，从键盘读取键值创建并发布twist消息，核心代码如下：

# mrobot_teleop.py
rospy.init_node('mrobot_teleop')
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=5)
 # 创建并发布twist消息
twist = Twist()
twist.linear.x = control_speed; 
twist.linear.y = 0; 
twist.linear.z = 0
twist.angular.x = 0; 
twist.angular.y = 0; 
twist.angular.z = control_turn
pub.publish(twist)

• 可以启动小乌龟来演示一下
  启动小海龟仿真器：rosrun turtlesim turtlesim_node 可以看到小乌龟turtlesim节点会订阅/turtle1/cmd_vel话题，我们需要在launch文件中重定向一下<remap from="/cmd_vel" to="/turtle1/cmd_vel"/> 这样就可以使用键盘控制小乌龟移动

ArbotiX同理launch文件中构建/arbotix节点，来自arbotix_driver可执行文件

	<node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen">
        <rosparam file="$(find mybot_pkg)/config/fake_mrobot_arbotix.yaml" command="load" />
        <param name="sim" value="true"/>
    </node>

/arbotix节点订阅cmd_vel话题，然后驱动模型运动，

查看话题：rostopic list

查看详细信息：
rostopic info <topic_name>
rosmsg show geometry_msgs/Twist
rostopic echo /cmd_vel

手撕代码

diff_controller差速控制

# arbotix_driver中
# 默认载入舵机类
self.controllers = [ServoController(self, "servos"), ]
controllers = rospy.get_param("~controllers", dict()) # 读取参数服务器

for name, params in controllers.items():
          # rospy.loginfo(f"controllers:{name} {params}") 输出1

    try:
         controller = controller_types[params["type"]](self, name)
         self.controllers.append( controller )
         # rospy.loginfo(f"self.controllers:{self.controllers}") 输出2
         pause = pause or controller.pause
            	

上述代码，使用rospy.get_param("~controllers", dict())从参数服务器读入参数，可以看到输出1差速电机控制类diff_controller是我们的yaml参数传入的


输出2使用rospy.loginfo输出self.controllers分别为舵机控制类、差速电机控制类diff_controller.DiffController，

# arbotix_driver中
while not rospy.is_shutdown():
	try:
		# update controllers
		for controller in self.controllers:
			controller.update()	

上述代码，循环调用controller.update() 参数，方法位于diff_controller.py，发布里程计（odometry）信息，用于计算和更新机器人的位置、方向和速度，并基于这些数据控制机器人的运动。

# diff_controller.py
class DiffController(Controller):
	def __init__(self, device, name):
		self.th = 0
        self.dx = 0
        # subscriptions
        rospy.Subscriber("cmd_vel", Twist, self.cmdVelCb) # 接受Twist话题消息
        self.odomPub = rospy.Publisher("odom", Odometry, queue_size=5) #构建odom话题
        self.odomBroadcaster = TransformBroadcaster()

上述代码，diff_controller.py中当 cmd_vel 话题上有新消息发布时，self.cmdVelCb方法被自动调用。

 def cmdVelCb(self,req):
        """ Handle movement requests. """
        # rospy.loginfo(f"cmdVelCb:req:{req}") 
        self.last_cmd = rospy.Time.now()
        if self.fake:
            self.dx = req.linear.x        # m/s 是机器人的线速度
            self.dr = req.angular.z       # rad/s  绕z轴的旋转速度

上述代码，输出req可以看到即是Twist消息

# diff_controller.py                  
def update(self):
        now = rospy.Time.now()
        if now > self.t_next:
            elapsed = now - self.then
            self.then = now
            elapsed = elapsed.to_sec()  # 将持续时间对象转换成秒
		
            if self.fake:
                x = cos(self.th)*self.dx*elapsed # x方向上的位移增量
                y = -sin(self.th)*self.dx*elapsed # y方向上的位移增量
                self.x += cos(self.th)*self.dx*elapsed # 更新机器人的全局坐标 (self.x 和 self.y)
                self.y += sin(self.th)*self.dx*elapsed
                self.th += self.dr*elapsed # 更新机器人的朝向 (self.th)

上述代码，首先检查当前时间 now 是否超过了下一次更新时间 self.t_next，若超过则执行计算时间差elapsed 。仿真模式下运行，代码通过一系列数学计算模拟机器人在二维空间中的运动。

# diff_controller.py   
quaternion = Quaternion()
quaternion.x = 0.0 
quaternion.y = 0.0
quaternion.z = sin(self.th/2)
quaternion.w = cos(self.th/2)
self.odomBroadcaster.sendTransform(
    (self.x, self.y, 0), 
    (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
    rospy.Time.now(),
    self.base_frame_id,
    self.odom_frame_id
    )

上述代码，一个典型的ROS (Robot Operating System) 里程计广播实现，主要用于更新并发布机器人的位置和方向。1.首先创建四元数，quaternion.x = 0.0 和 quaternion.y = 0.0四元数的x和y分量被设置为0，因为在二维空间中的旋转仅需要考虑z轴。quaternion.z = sin(self.th/2) 和 quaternion.w = cos(self.th/2)这里通过机器人当前的朝向 self.th 来计算四元数的z分量和w分量。2.发送变换tf (Transform)，self.odomBroadcaster.sendTransform() 方法用于将变换（位置和方向）发送到ROS的tf（变换）库。

# diff_controller.py   
odom = Odometry()
odom.header.stamp = now
odom.header.frame_id = self.odom_frame_id
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.child_frame_id = self.base_frame_id
odom.twist.twist.linear.x = self.dx
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = self.dr
self.odomPub.publish(odom)

接下来，diff_controller继续发布里程计信息。

/arbotlx

/arbotlx 发布/tf，/odom，/diagnostics，/joint_states四个话题
arbotix_driver中引入发布类publishers.py

# arbotix_driver
from arbotix_python.publishers import *  # 发布器类
self.diagnostics = DiagnosticsPublisher()  # 创建一个诊断信息发布器对象
self.joint_state_publisher = JointStatePublisher()  # 创建一个关节状态发布器对象