一、实验内容

1、从课程中心下载odom_noise.py到工作空间
2、修改launch文件，加入odom_noise节点
3、修改urdf，将odom发布的底盘坐标系改掉
4、在房间中找到至少一个特征点，计算该特征点位置的激光特征和map坐标
5、修改扫地节点，发布map->odom坐标系转换关系
6、在rviz中查看小车位置以及odom坐标系原点位置变化

二、相关知识

机器人的坐标体系：
运动是相对的，不同的参考点看到的运动形态各不相同
机器人是由大量电机、关节等组成的复杂机械系统，机器人与 大地之间、机器人模块与模块之间、机器人与搬运物体之间存 在复杂的相对运动。

机器人常见的7种坐标体系：

里程计坐标系：里程计坐标系的原点，处于机器人运动的起始地点

初始状态可能与地图坐标系重叠(在地图原点)，随着时间变 化

运动打滑、起伏不平、累积误差等都会导致里程计坐标系与世界坐标系 的映射关系呈现时变特性。

Ros的坐标映射工具：TF

TF库的目的是实现系统中任一个点在所有坐标系之间的坐标变换，也就是说，只要给定一个坐标系下的一个点的坐标，就能获得这个点在其他坐标系的坐标.
使用tf功能包:
a. 监听tf变换: 接收并缓存系统中发布的所有参考系变换，并从中查询所需要的参考系变换。
b.广播 tf变换: 向系统中广播参考系之间的坐标变换关系。

小球发布的TF变换：

人体发布的TF变换：

三、实验步骤

1、下载新的odom_noise.py文件到工作空间，这里我放到了sprits文件夹中

2、 修改launch文件，加入新的odometer_noise节点

主要是加入一段关于odom_noise节点的内容

注意，这里需要把“my_first_package”修改成对应的工作目录
完整的代码如下：

<launch><include file="$(find gazebo_ros)/launch/empty_world.launch"><arg name="world_name" value="$(find my_first_package)/world/wd5"/><arg name="use_sim_time" value="true"/><arg name="gui" value="false"/></include><param name="robot_description" command="cat $(find my_first_package)/urdf/car.urdf"/><node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher"></node><node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"></node><node pkg="gazebo_ros" type="spawn_model" name="gazebo_spawn" args="-file $(find my_first_package)/urdf/car.urdf -urdf -x 0 -y 0 -z 0 -model MYROBOT3"/>		<node pkg="my_first_package" type="odom_noise.py" name="noise"><param name="baseframe" value="base_link"/></node><node pkg="my_first_package" name="rode" type="rode.py" output="screen"/>	<env name="GAZEBO_RESOURCE_PATH" value="$(find my_first_package)"/><node name="rviz" pkg="rviz" type="rviz"/>
</launch>

3、修改参数urdf文件，将odom发布的地盘坐标系改掉

主要是修改以下内容;

完整代码如下：

<robot name="demo_model">
<material name="blue">
<color rgba="1 0 1 1"/>
</material><link name="left_wheel1">
<visual>
<geometry>
<cylinder length="0.06" radius="0.1"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 -0.03"/>
</visual>
<inertial>
<mass value="50" />
<inertia ixx="5.0" ixy="0.0" ixz="0.0"
iyy="5.0" iyz="0.0"
izz="5.0" />
<origin rpy="0 0 0" xyz="0 0 -0.03"/>
</inertial>
<collision>
<geometry>
<cylinder length="0.06" radius="0.1"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 -0.03"/>
</collision>
</link><link name="right_wheel1">
<visual>
<geometry>
<cylinder length="0.06" radius="0.1"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 -0.03"/>
</visual>
<inertial>
<mass value="50" />
<inertia ixx="5.0" ixy="0.0" ixz="0.0"
iyy="5.0" iyz="0.0"
izz="5.0" />
<origin rpy="0 0 0" xyz="0 0 -0.03"/>
</inertial>
<collision>
<geometry>
<cylinder length="0.06" radius="0.1"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 -0.03"/>
</collision>
</link><link name="left_wheel2">
<visual>
<geometry>
<cylinder length="0.06" radius="0.1"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 -0.03"/>
</visual>
<inertial>
<mass value="50" />
<inertia ixx="5.0" ixy="0.0" ixz="0.0"
iyy="5.0" iyz="0.0"
izz="5.0" />
<origin rpy="0 0 0" xyz="0 0 -0.03"/>
</inertial>
<collision>
<geometry>
<cylinder length="0.06" radius="0.1"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 -0.03"/>
</collision>
</link><link name="right_wheel2">
<visual>
<geometry>
<cylinder length="0.06" radius="0.1"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 -0.03"/>
</visual>
<inertial>
<mass value="50" />
<inertia ixx="5.0" ixy="0.0" ixz="0.0"
iyy="5.0" iyz="0.0"
izz="5.0" />
<origin rpy="0 0 0" xyz="0 0 -0.03"/>
</inertial>
<collision>
<geometry>
<cylinder length="0.06" radius="0.1"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 -0.03"/>
</collision>
</link><link name="base_link">
<visual>
<geometry>
<box size="0.6 0.4 0.1"/>
</geometry>
</visual>
<collision>
<geometry>
<box size="0.6 0.4 0.2"/>
</geometry>
</collision>
<inertial>
<mass value="50" />
<origin xyz="0 0 0" />
<inertia ixx="5.0" ixy="0.0" ixz="0.0"
iyy="5.0" iyz="0.0"
izz="5.0" />
</inertial>
</link><link name="hokuyo_link">
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</collision>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</visual>
<inertial>
<mass value="1e-5" />
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
</inertial>
</link><gazebo reference="base_link">
<material>YourMaterialName</material>
</gazebo><joint name="base_to_left_wheel1" type="continuous">
<origin rpy="1.5708 0 0" xyz="0.2 0.14 -0.05"/>
<parent link="base_link"/>
<child link="left_wheel1"/>
</joint><joint name="base_to_right_wheel1" type="continuous">
<origin rpy="1.5708 0 0" xyz="0.2 -0.20 -0.05"/>
<parent link="base_link"/>
<child link="right_wheel1"/>
</joint><joint name="base_to_left_wheel2" type="continuous">
<origin rpy="1.5708 0 0" xyz="-0.2 0.14 -0.05"/>
<parent link="base_link"/>
<child link="left_wheel2"/>
</joint><joint name="base_to_right_wheel2" type="continuous">
<origin rpy="1.5708 0 0" xyz="-0.2 -0.20 -0.05"/>
<parent link="base_link"/>
<child link="right_wheel2"/>
</joint><joint name="hokuyo_joint" type="fixed">
<origin xyz="0 0 0.2" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="hokuyo_link"/>
</joint><gazebo>
<plugin name="object_controller" filename="libgazebo_ros_planar_move.so">
<commandTopic>/cmd_vel</commandTopic>
<odometryTopic>/odom</odometryTopic>
<odometryFrame>/odom</odometryFrame>
<odometryRate>20.0</odometryRate>
<robotBaseFrame>nonesense</robotBaseFrame>
</plugin>
</gazebo><gazebo reference="hokuyo_link">
<sensor type="ray" name="head_hokuyo_sensor">
<pose>0 0 0 0 0 0</pose>
<visualize>false</visualize>
<update_rate>40</update_rate>
<ray>
<scan>
<horizontal>
<samples>2000</samples>
<resolution>1</resolution>
<min_angle>-3.14</min_angle>
<max_angle>3.14</max_angle>
</horizontal>
</scan>
<range>
<min>0.10</min>
<max>30.0</max>
<resolution>0.01</resolution>00
</range>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.01</stddev>
</noise>
</ray>
<plugin name="gazebo_ros_head_hokuyo_controller" filename="libgazebo_ros_laser.so">
<topicName>scan</topicName>
<frameName>/hokuyo_link</frameName>
</plugin>
</sensor>
</gazebo>
</robot>

4、在房间中找到至少一个特征点，计算该特征点位置的激光特征和map坐标。修改扫地节点，发布map->odom坐标系转换关系

这里主要需要修改上次实验的扫地代码，可以按照老师ppt上的来，我的代码思路是这样的：
由于我需要进行两次的转换，走一个“之”字形才能到达最里面的房间，所以，我干脆直接把两个转换点作为我的坐标转换特征点。当我到达这两个点的时候，我的扫地机器人开始正常的运作，而我只需要在到达这两个点的时候进行坐标转换即可。
坐标转换需要订阅新的话题。
完整代码如下：

#!/usr/bin/env python
# -*- coding:utf-8 -*-
import rospy
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
import tf#坐标转换函数库
from nav_msgs.msg import Odometryrospy.init_node('lasr_listener',anonymous=False)
pub = rospy.Publisher('/cmd_vel',Twist,queue_size=10)
vel_cmd=Twist()
x=0
flag=0
y=0
br=tf.TransformBroadcaster()#坐标转换所需要的的变量
xx=0#存储转换值的变量
yy=0
odom=0#存储话题返回值的变量def odomCallback(odom0):#obom回调函数，给予我当前坐标系的参数global odomodom=odom0def callback(scan):#scan回调函数global flag,y,xx,yy,br,odom#这一步一定要有if flag==0:index=int((0-scan.angle_min)/scan.angle_increment)rospy.loginfo('index:{0} range:{1}'.format(index,scan.ranges[index]))x=scan.ranges[index]if x>0.3:vel_cmd.linear.x=0.5xx=0#一开始不需要进行坐标系转换，将转换值设置为0yy=0else:vel_cmd.linear.x=0xx=0-odom.pose.pose.position.x#到达第一个转换点的时候，得到转换值yy=0-odom.pose.pose.position.yflag=1br.sendTransform((xx,yy,0),(0,0,0,1),rospy.Time.now(),"odom","map")#通过话题发送转换值，对坐标系进行转换elif flag==1:index=int((-3.1415926/2-scan.angle_min)/scan.angle_increment)rospy.loginfo('index:{0} range:{1}'.format(index,scan.ranges[index]))x=scan.ranges[index]if x>0.3:vel_cmd.linear.y=-0.5else:vel_cmd.linear.y=0xx=0-odom.pose.pose.position.xyy=0-odom.pose.pose.position.yflag=2br.sendTransform((xx,yy,0),(0,0,0,1),rospy.Time.now(),"odom","map")elif flag==2:index=int((0-scan.angle_min)/scan.angle_increment)rospy.loginfo('index:{0} range:{1}'.format(index,scan.ranges[index]))x=scan.ranges[index]if x>0.3:vel_cmd.linear.x=0.5else:vel_cmd.linear.x=0xx=0-odom.pose.pose.position.xyy=0-odom.pose.pose.position.yflag=3br.sendTransform((xx,yy,0),(0,0,0,1),rospy.Time.now(),"odom","map")elif flag==3:index=int((3.1415926/2-scan.angle_min)/scan.angle_increment)rospy.loginfo('index:{0} range:{1}'.format(index,scan.ranges[index]))x=scan.ranges[index]if x>0.3:vel_cmd.linear.y=0.5else:vel_cmd.linear.y=0xx=0-odom.pose.pose.position.xyy=0-odom.pose.pose.position.yflag=4y=y+1br.sendTransform((xx,yy,0),(0,0,0,1),rospy.Time.now(),"odom","map")elif flag==4:index=int((0-scan.angle_min)/scan.angle_increment)rospy.loginfo('index:{0} range:{1}'.format(index,scan.ranges[index]))x=scan.ranges[index]index2=int((3.1415926-scan.angle_min)/scan.angle_increment)x2=scan.ranges[index2]if x2<0.3:vel_cmd.linear.x = 0flag=7else:if x<0.3+0.1*(y):vel_cmd.linear.x=-0.5else:vel_cmd.linear.x=0flag=5xx=0-odom.pose.pose.position.xyy=0-odom.pose.pose.position.ybr.sendTransform((xx,yy,0),(0,0,0,1),rospy.Time.now(),"odom","map")elif flag==5:index=int((-3.1415926/2-scan.angle_min)/scan.angle_increment)rospy.loginfo('index:{0} range:{1}'.format(index,scan.ranges[index]))x=scan.ranges[index]if x>0.3:vel_cmd.linear.y=-0.5else:vel_cmd.linear.y=0xx=0-odom.pose.pose.position.xyy=0-odom.pose.pose.position.yflag=6y=y+1br.sendTransform((xx,yy,0),(0,0,0,1),rospy.Time.now(),"odom","map")elif flag==6:index=int((0-scan.angle_min)/scan.angle_increment)rospy.loginfo('index:{0} range:{1}'.format(index,scan.ranges[index]))x=scan.ranges[index]index2=int((3.1415926-scan.angle_min)/scan.angle_increment)x2=scan.ranges[index2]if x2<0.3:vel_cmd.linear.x = 0flag=7else:if x<0.3+0.1*(y):vel_cmd.linear.x=-0.5else:vel_cmd.linear.x=0flag=3xx=0-odom.pose.pose.position.xyy=0-odom.pose.pose.position.ybr.sendTransform((xx,yy,0),(0,0,0,1),rospy.Time.now(),"odom","map")else:vel_cmd.linear.x=0vel_cmd.linear.y=0br.sendTransform((xx,yy,0),(0,0,0,1),rospy.Time.now(),"odom","map")pub.publish(vel_cmd)def listener():rospy.Subscriber('/scan',LaserScan,callback)rospy.Subscriber('/odom_noise',Odometry,odomCallback)rospy.spin()if __name__=='__main__':listener()

5、在rviz中查看小车位置和odom坐标系原点位置变化

这里我们最好建立两个坐标系，即把两个坐标系都显示出来，就可以清楚地看到map坐标系和odom坐标系的偏移。

本文来自互联网用户投稿，文章观点仅代表作者本人，不代表本站立场，不承担相关法律责任。如若转载，请注明出处。 如若内容造成侵权/违法违规/事实不符，请点击【内容举报】进行投诉反馈！