一、配置广成CAN盒
1.设置有线网络ip
首先将CAN和与底盘和电脑相连
然后设置有限网络连接的IP。win10电脑打开设置->网络和Internet->以太网->更改适配器选项。右键以太网,点击属性
点击在这个
按照下面的填写
然后点击确定,ip就设置好了。
2.配置can盒
打开“CANet-Wifi-Config-V5.exe”程序
在连接设备窗口下选择设备类型为当前使用的 CAN 转换器的类型,IP 默认的为 192.168.1.10,然后点击【连接】。
在弹出的窗口点击【读参数】弹出下面的读取 CAN 中的配置信息
下面以CAN1 Info为例配置信息(云乐线控底盘的 CAN 波特率为 500kbps)
点击Can Baud,数值选择3
点击TCP Mode,数值选择3
点击Remote IP,将ip改成192.168.1.102
CAN2 Info同样按照上面的流程更改
然后点击【下载参数】
然后再点击【读参数】,发现CAN1,CAN2的配置更改成功
将can盒断电重启一下
3.使用网络调试助手查看can盒通信情况
打开网络调试助手,先发送一条指令,就可以得到反馈
下面发送指令测试接受情况
按照这个表格
发送28 00 00 01 20 40 A8 FD 00 00 00 00 00指令,小车可以转向
发送28 00 00 01 20 41 00 00 1E 00 00 00 00指令,小车可以前进
二、autoware连接速腾聚创激光雷达
将雷达插上电源,网线与电脑连接后,确保ip地址为192.168.1.102
打开激光雷达ros驱动的配置文件(RoboSense-LiDAR_ws/src/rslidar_sdk/config/config.yaml)
将frame_id改成/velodyne,将话题改成/points_raw
重新编译一下,然后启动激光雷达ros驱动以及点云转换文件
source devel/setup.bash
#运行rs_to_velodyne
roslaunch rs_to_velodyne rs_to_velodyne.launch
#运行ros驱动
roslaunch rslidar_sdk start.launch
在弹出的rviz中将frame_id改成/velodyne,将激光点云话题改成/points_raw,成功出现点云图像
三、编写驱动底盘的ros功能包
根据云乐小车的控制报文的含义以及相应的DBC文件,编写ros功能包,接受autoware发来的线速度与角速度,并通过DBC文件编码,用UDP下发到底盘。下面是编写好的python程序。
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
import socket
import cantools
from binascii import hexlify
from geometry_msgs.msg import TwistStamped
import math
# 弧度转角度系数
radian2angle = 57.29577951308232
# 创建socket套接字
udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) # AF_INET表示使用ipv4,默认不变,SOCK_DGRAM表示使用UDP通信协议
# 绑定端口port
local_addr = ("192.168.1.102", 8002) # 本地ip,端口号
udp_socket.bind(local_addr) # 绑定端口
# 加载dbc文件
db = cantools.database.load_file('../../04_YunleCAN_B.dbc')
'''
SCU_ShiftLevel_Req 1前进3后退 0-2
SCU_Steering_Wheel_Angle 转向角度 8-16
SCU_Target_Speed 目标速度 24-9
'''
SCU_JD01_1 = {'SCU_ACC_Mode': 0, 'SCU_Brake_Mode': 0, 'SCU_Drive_Mode_Req': 1}
SCU_JD01_2 = {'SCU_Brk_En': 0, 'SCU_GearNTorque_En': 0, 'GW_Left_Turn_Light_Req': 0,
'GW_Right_Turn_Light_Req': 0, 'GW_Hazard_Light_Req': 0, 'GW_Position_Light_Req': 0,
'GW_LowBeam_Req': 0, 'GW_HighBeam_Req': 0, 'GW_RearFogLight_Req': 0,
'GW_Horn_Req': 0, 'SCU_Brake_Coefficient': 0}
message_SCU_JD01_front = bytes([0x28, 0x00, 0x00, 0x01, 0x20]) # SCU_JD01的帧信息与帧ID
SCU_Target_Speed_data = {'SCU_Target_Speed' : 0}
SCU_ShiftLevel_Req_data = {'SCU_ShiftLevel_Req': 0}
Steer_Angle_data = {'SCU_Steering_Wheel_Angle' : 0}
# udp向底盘发送can协议
def udp_send_can(message_name):
udp_socket.sendto(message_name, ("192.168.1.10", 4002))
# 定义/twist_cmd话题的回调函数
def cmd_vel_callback(msg):
# 1. 接收autoware传来的线速度和角速度
SCU_Target_Speed = msg.twist.linear.x
angular_velocity = msg.twist.angular.z
print('SCU_Target_Speed:%f' % SCU_Target_Speed)
print('angular_velocity:%f' % angular_velocity)
# 2. 判断档位与角度
if (SCU_Target_Speed > 0):
# 档位
SCU_ShiftLevel_Req_data['SCU_ShiftLevel_Req'] = 1
# 角度
'''
转弯的角度 = arctan(( 角速度 / 线速度 ) * 车长 )
左转 9°,则-9/27*120=-40,256-40=216,216 转十六进制为 0xD8
'''
Steer_Angle = int(-math.atan((angular_velocity/SCU_Target_Speed)*1.2) * radian2angle)
print('Steer_Angle:%f' % Steer_Angle)
Steer_Angle_value = int(Steer_Angle * 4)
print('Steer_Angle_value:%f' % Steer_Angle_value)
Steer_Angle_data['SCU_Steering_Wheel_Angle'] = Steer_Angle_value
# 目标速度
SCU_Target_Speed_data['SCU_Target_Speed'] = SCU_Target_Speed
print(SCU_Target_Speed_data)
elif (SCU_Target_Speed < 0):
# 档位
SCU_ShiftLevel_Req_data['SCU_ShiftLevel_Req'] = 3
# 角度
Steer_Angle = int(-math.atan((angular_velocity/SCU_Target_Speed)*1) * radian2angle)
print('Steer_Angle:%f' % Steer_Angle)
Steer_Angle_value = int(Steer_Angle * 4)
print('Steer_Angle_value:%f' % Steer_Angle_value)
Steer_Angle_data['SCU_Steering_Wheel_Angle'] = Steer_Angle_value
# 目标速度
SCU_Target_Speed_data['SCU_Target_Speed'] = -SCU_Target_Speed
print(SCU_Target_Speed_data)
else:
# 档位
SCU_ShiftLevel_Req_data['SCU_ShiftLevel_Req'] = 0
# 角度
Steer_Angle = int(-math.atan((angular_velocity/SCU_Target_Speed)*1) * radian2angle)
print('Steer_Angle:%f' % Steer_Angle)
Steer_Angle_value = int(Steer_Angle * 4)
print('Steer_Angle_value:%f' % Steer_Angle_value)
Steer_Angle_data['SCU_Steering_Wheel_Angle'] = Steer_Angle_value
# 目标速度
SCU_Target_Speed_data['SCU_Target_Speed'] = SCU_Target_Speed
print(SCU_Target_Speed_data)
# 3. 发送can协议
SCU_ShiftLevel_Req_data.update(SCU_JD01_1)
SCU_ShiftLevel_Req_data.update(Steer_Angle_data)
SCU_ShiftLevel_Req_data.update(SCU_Target_Speed_data)
SCU_ShiftLevel_Req_data.update(SCU_JD01_2)
message = db.encode_message("SCU_NWD", SCU_ShiftLevel_Req_data)
message_angle = message_SCU_JD01_front + message
print(hexlify(message_angle).decode('ascii'))
udp_send_can(message_angle)
if __name__ == '__main__':
# 初始化ROS节点
rospy.init_node('cmd_vel_to_can')
# 订阅/twist_cmd话题
rospy.Subscriber('twist_cmd', TwistStamped, cmd_vel_callback)
# 启动ROS节点
rospy.spin()
四、录制ros包
启动激光雷达ros驱动以及点云转换文件
source devel/setup.bash
#运行rs_to_velodyne
roslaunch rs_to_velodyne rs_to_velodyne.launch
#运行ros驱动
roslaunch rslidar_sdk start.launch
然后使用下面指令录制激光雷达点云信息
打开终端输入
rosbag record /points_raw
遥控小车绕着场地走一圈,然后在刚才录制信息的终端按ctrl+c停止录制
五、建图
1.加载记录的rosbag
在simulation下加载刚刚录制的包,点击play后,立即点击pause
2.设置从base_link到velodyne坐标系的TF。
在 [Setup] 菜单中,点击[ TF ]和[Vehicle Model]按钮,其中x、y、z、yaw、pitch、roll表示真车雷达中心点与车身后轴中心点的相对位置关系(右手坐标系,真车后车轴为原点)。
3.设置从world到map转换
点击 [Map] 页面,点击 [TF] 的 [ref] 选择 /home/jywl01/autoware-1.14/src/autoware/documentation/autoware_quickstart_examples/launch/tf_local.launch 文件,这是加载默认world到map的坐标转换,打开tf_local.launch文件如下:
<launch>
<node pkg="tf" type="static_transform_publisher" name="world_to_map" args="0 0 0 0 0 0 /world /map 10" />
</launch>
4.在sensing下勾选下面的选项
5.在computing勾选ndt_mapping
6.制作点云地图
最后切换到Simulation ,再次点击 [Pause] 开始播放
可以在终端看到
注意:上图的最下面的(Process/Input): (315 / 315),这两个数字前一个数字表示正在处理的点云帧数,后一个表示加载的点云帧数。如果两个数字相差过大,会出现运行错误。如果前后两个数字相差1000以上,就要按[Simulaton] 页面的[Pause] 按键,暂停加载,等待一下正在处理的数字,两个数字重新接近之后,可以再次按 [Pause] 按键运行。
运行停止,表示地图生成完毕。
7.保存点云地图
打开 [Compulting] 下的 [Ndt_Mapping] 选项的 [app],如下所示:
点击 [ref] 选择保存地图的路径,将 [Filter Resolution] 参数设置为0.2,点击 [PCD OUTPUT]按钮,开始保存pcd文件。
8.查看地图文件
进入到保存pcd文件的路径,用pcl_viewer autoware-230627.pcd查看点云文件
到此为止,点云地图制作完毕。
六、录制航迹点
首先打开gazebo_ws/src/vehicle_sim-master/launcher/vehicle_sim_launcher/launch/world_test.launch文件,把第8行和第21行加载rviz的代码打开
1.启动autoware
打开终端输入
cd autoware-1.14
source install/setup.bash
roslaunch runtime_manager runtime_manager.launch
2.加载记录的rosbag
在simulation下加载刚刚录制的包,点击play后,立即点击pause
3.设置从base_link到velodyne坐标系的TF。
在 [Setup] 菜单中,点击[ TF ]和[Vehicle Model]按钮,其中x、y、z、yaw、pitch、roll表示真车雷达中心点与车身后轴中心点的相对位置关系(右手坐标系,真车后车轴为原点)。
4.完成从world到map的坐标系转换并加载点云地图
切换到autoware的Map页面下,点击TF右侧的Ref按钮,加载如下路径的launch文件,其中是从/world坐标系转换到/map坐标系的tf变换,最后点击TF按钮。
/home/jywl02/autoware-1.14/src/autoware/documentation/autoware_quickstart_examples/launch/tf_local.launch
点击Point Cloud右侧的Ref按钮,加载建立好的pcd点云地图,并点击Point Cloud按钮,进度条显示OK,则加载完毕。
5.设置相关滤波器
在sensing下勾选下面的选项,参数为默认参数
6.设置从map到base_link的转换
找到 [Computing] 左菜单栏下的 [ndt_matching] 选项,打开 [app] ,如下所示。设置好点击勾选
7.启动[vel_pose_connect]
找到 [Computing] 左菜单栏下的 [vel_pose_connect] ,打开 [app] 并确保选项 [Simulation_Mode] 没有被勾选(默认不勾选),退出并勾选 [vel_pose_connect]。
8.记录航迹点
在菜单栏 [Computing] 右边的 [waypoint_maker] 下的 [waypoint_saver]。点击 [app] 按钮,在打开的界面中点击 [Ref] 指定保存路径和文件名后点击 [OK] 。如下所示:
[app]在勾选框 [Save/current_velocity],可以保存waypoint的速度,参数[Interval]中的值表示采点间隔,默认1m。退出并勾选 [waypoint_saver]。
9.保存记录数据
最后切换到Simulation ,再次点击 [Pause] 开始播放
可以在rviz中看到
当rosbag播放完了,取消 [waypoint_saver] 勾选(自动保存waypoint)。
七、寻迹测试
1.启动激光雷达ros驱动以及点云转换文件
source devel/setup.bash
#运行rs_to_velodyne
roslaunch rs_to_velodyne rs_to_velodyne.launch
#运行ros驱动
roslaunch rslidar_sdk start.launch
2.启动autoware
打开终端输入
cd autoware-1.14
source install/setup.bash
roslaunch runtime_manager runtime_manager.launch
3.设置从base_link到velodyne坐标系的TF。
在 [Setup] 菜单中,点击[ TF ]和[Vehicle Model]按钮,其中x、y、z、yaw、pitch、roll表示真车雷达中心点与车身后轴中心点的相对位置关系(右手坐标系,真车后车轴为原点)。
4.完成从world到map的坐标系转换并加载点云地图
切换到autoware的Map页面下,点击TF右侧的Ref按钮,加载如下路径的launch文件,其中是从/world坐标系转换到/map坐标系的tf变换,最后点击TF按钮。
/home/jywl02/autoware-1.14/src/autoware/documentation/autoware_quickstart_examples/launch/tf_local.launch
点击Point Cloud右侧的Ref按钮,加载建立好的pcd点云地图,并点击Point Cloud按钮,进度条显示OK,则加载完毕。
5.设置相关滤波器
选择如下两个选项,设置一些参数,并且勾选这两个选项。
6.设置导航
(1)在 [Computing] 菜单栏左侧选择如下两个选项,设置一些参数,并且勾选这两个选项。
(2)在 [Computing] 菜单栏右侧 [Mission Planning] 中勾选 [lane_rule],[lane_stop],[lane_select] 三个选项,参数选择默认。
(3)在 [Computing] 菜单栏右侧的 [waypoint_planner] 中勾选[velocity_set] 选择,参数选择默认。
点击[astar_avoid]右侧的app,勾选上Enable Avoidance,之后再勾选[astar_avoid]
不勾选astar_avoid,则选择的模式为遇到障碍物不停止
勾选astar_avoid,则选择的模式为遇到障碍物就停止
(4)在菜单栏 [Computing] 右侧的 [waypoint_maker] 下的 [waypoint_loader] 加载waypoints。
选择 [Ref] 加载之前保存下来的路点文件,并点击 [Close] 关闭页面。勾选 [waypoint_loader] ,如下图所示:
(5)在 [Computing] 菜单栏,找到 [waypoint_follower] 下 [pure_pursuit] 与 [twist_filter] 的 [app] 进行设置。
[waypoint_follower] 的 [app] 在打开的菜单栏勾选 [Waypoint] 选项。
选择waypoint模式:根据自己的速度
dialog:根据autoware自己设置参数
上述操作完成后,云乐小车可以依据waypoints行驶。