也学ros这个东西学了好长时间了，但是在ROS中进行强化学习并最终部署到实车这个过程一直都出现各种问题，实验室也没什么相关的积累，自己一个人搞就很痛苦。这次看论文时候看到别人公布的源码，于是去学习了一下别人怎么做的这个流程，真的是受益匪浅。
最终也能够实现从虚拟训练到实车部署这整个过程了！
现在把整个流程总结下来，后面关键部分的描述都在注释里面，希望实验室之后的学弟学妹们能轻松做实验带带学长发文章，也希望能够帮到各位刚接触ROS的同学们~！
（另外不知道为啥直接上传的md文件还会出现一些奇怪的格式问题，但也懒得处处改了，各位担待着看吧）
但是说实话复现了很多算法了，目前来看 没有一个算法实车部署效果和虚拟仿真环境训练效果一样好的 （即便是论文里声称可以直接来用）。这个案例实车部署时，也总会出现原地转圈、踟蹰不前这种情况。如果有人真的用过什么可以在实车上也跑的很好的算法，也麻烦在评论区或者私信里面说一下！
一 、运行一个示例
1、前提准备
（1）下载代码
使用了 Goal-Driven Autonomous Exploration Through Deep Reinforcement Learning 这篇论文作为例子，上面的流程可以给我们一个很好的示范，其github网址为：
https://github.com/reiniscimurs/DRL-robot-navigation将其解压，我们可以得到如下的文件结构：
其中 **catkin_ws **为ROS工作空间，包含了gazebo仿真环境、p3dx小车模型、velody激光雷达模型等。但其主要任务就是打开一个仿真环境
TD3 中包含了文章中介绍的算法，其内部结构如下：
velodyne_env 是发送订阅数据、处理数据、获取状态、计算奖励等功能的代码，相当于gym中的环境部分
train_velodyne_td3 是训练代码，其中包括了网络结构以及训练部分。
（2）python环境
我们需要在Ubuntu环境中安装Anaconda，创建虚拟环境，安装好如pytorch之类的库，这部分想必各位既然已经开始做ROS了都有经验，不再赘述。
（3）ROS环境
用ROS当然需要安装ROS环境！我这里用的是Ubuntu 20.04 +ROS1 noetic版本
2、 进行仿真训练
（1）对工作空间进行编译
进入到catkin_ws工作空间内，打开一个终端
cd ~/DRL-robot-navigation-main/catkin_wscatkin_make在编译过程中，可能会出现各种各样的库缺失，具体缺失的内容可以读取报错信息，然后安装
sudo apt install ros-noetic-$ 你缺失的库 $（2）运行训练文件
进入到TD3文件夹下，打开终端，cd到路径，python运行，这里会发现可能还是会缺少很多的库，这里我们使用pip install 安装
python train_velodyne_td3.py按照其github上面所说，这样就可以进行训练了。但实际操作过程中发现这样无法打开gazebo，也会出现各种奇怪的报错，于是对其进行了如下更改
（3）更改部分
①手动创建launch文件
cd ./DRL-robot-navigation-main/catkin_ws/src/multi_robot_scenario/launch我们可以看到这里面的文件，TD3.world，这里面就是其gazebo 的仿真环境文件，我们可以创建一个新的launch文件，用于打开这个.world文件
创建一个新的launch文件，命名为 TD3_world.launch
在其中输入如下内容
<launch> <include file="$(find gazebo_ros)/launch/empty_world.launch"><arg name="world_name" value="$(find multi_robot_scenario)/launch/TD3.world"/><arg name="paused" value="false"/><arg name="use_sim_time" value="true"/><arg name="gui" value="true"/><arg name="headless" value="false"/><arg name="debug" value="false"/></include></launch>②打开环境
打开一个新的终端
cd DRL-robot-navigation-main/catkin_ws/catkin_makesource devel/setup.shroslaunch multi_robot_scenario TD3_world.launch 
那么我们就能够打开这个仿真环境了，但是我们发现这里并没有我们需要的机器人模型，所以我们还需要打开launch文件夹下的pioneer3dx.gazebo.launch文件，则新建一个终端
cd DRL-robot-navigation-main/catkin_ws/source devel/setup.shroslaunch multi_robot_scenario pioneer3dx.gazebo.launch 那么我们就可以在仿真环境内看到移动机器人了
③修改velody_env.py文件
接下来我们 python运行 train_velody_td3.py时候会发现，依旧会报一堆乱七八糟的错误。
我们需要对如下内容进行更改：
a: 将文件中所有的**”r1″改为“p3dx”**
包括82行、116行、130行
这是由于如果不修改的话，他会将机器人命名为r1，但是我们直接打开，机器人是p3dx，当然也可以在打开过程中修改他的名字r1，但是对于初学者来说直接修改可能更容易理解。
b:注释掉它文件中打开launch文件的部分
主要是98行-113行，但是注意104行初始化节点不要注释
（4）开始训练
接下来就可以愉快的训练了！我个人更喜欢直接在vscode中训练
这里需要选择相应的环境
然后直接run就行了，我们发现它可以运行了！
二、具体部分分析
1、Gazebo仿真环境部分
按照第一大部分运行Gazebo后，通过rostopic list 可以看到目前有如下的话题在ROS中存在
gazebo仿真环境如图所示:
2、velody_env.py文件
（1）init 关键部分
rospy.init_node("gym", anonymous=True) #初始化ros节点#--------------------------发布内容----------------------------self.vel_pub = rospy.Publisher("/p3dx/cmd_vel", Twist, queue_size=1)#用于发布移动机器人的控制信息self.set_state = rospy.Publisher("gazebo/set_model_state", ModelState, queue_size=10)#用于设置移动机器人的位置、角度信息self.publisher = rospy.Publisher("goal_point", MarkerArray, queue_size=3)#用于发布目标点self.publisher2 = rospy.Publisher("linear_velocity", MarkerArray, queue_size=1)#线速度 MarkerArraty算法中无实际作用self.publisher3 = rospy.Publisher("angular_velocity", MarkerArray, queue_size=1) #角速度 MarkerArray算法中无实际作用#--------------------------服务内容----------------------------self.unpause = rospy.ServiceProxy("/gazebo/unpause_physics", Empty)#恢复仿真self.pause = rospy.ServiceProxy("/gazebo/pause_physics", Empty)#暂停仿真self.reset_proxy = rospy.ServiceProxy("/gazebo/reset_world", Empty)#重置仿真环境#--------------------------订阅内容----------------------------self.velodyne = rospy.Subscriber("/velodyne_points", PointCloud2, self.velodyne_callback, queue_size=1) #订阅激光雷达发布的点云self.odom = rospy.Subscriber("/p3dx/odom", Odometry, self.odom_callback, queue_size=1)#订阅里程计信息（2）step部分
def step(self, action):target = False# 发布机器人的动作，这里的step输入是action，action[0]是线速度，action[1]是角速度vel_cmd = Twist()vel_cmd.linear.x = action[0]vel_cmd.angular.z = action[1]self.vel_pub.publish(vel_cmd)self.publish_markers(action)#这里要注意了！！！！！# 首先unpause，发布消息后，开始仿真rospy.wait_for_service("/gazebo/unpause_physics")try:self.unpause()except (rospy.ServiceException) as e:print("/gazebo/unpause_physics service call failed")# 接下来需要停一小段时间，让小车以目前的速度行驶一段时间，TIME_DELTA = 0.1time.sleep(TIME_DELTA)# 然后我们停止仿真，开始观测目前的状态rospy.wait_for_service("/gazebo/pause_physics")try:passself.pause()except (rospy.ServiceException) as e:print("/gazebo/pause_physics service call failed")# 检测碰撞done, collision, min_laser = self.observe_collision(self.velodyne_data)# 雷达状态v_state = []v_state[:] = self.velodyne_data[:]laser_state = [v_state]# 从里程计信息中获取小车朝向self.odom_x = self.last_odom.pose.pose.position.xself.odom_y = self.last_odom.pose.pose.position.yquaternion = Quaternion(self.last_odom.pose.pose.orientation.w,self.last_odom.pose.pose.orientation.x,self.last_odom.pose.pose.orientation.y,self.last_odom.pose.pose.orientation.z,)euler = quaternion.to_euler(degrees=False)angle = round(euler[2], 4)# 计算到目标点的距离distance = np.linalg.norm([self.odom_x - self.goal_x, self.odom_y - self.goal_y])# 计算偏离角度skew_x = self.goal_x - self.odom_xskew_y = self.goal_y - self.odom_ydot = skew_x * 1 + skew_y * 0mag1 = math.sqrt(math.pow(skew_x, 2) + math.pow(skew_y, 2))mag2 = math.sqrt(math.pow(1, 2) + math.pow(0, 2))beta = math.acos(dot / (mag1 * mag2))if skew_y < 0:if skew_x < 0:beta = -betaelse:beta = 0 - betatheta = beta - angleif theta > np.pi:theta = np.pi - thetatheta = -np.pi - thetaif theta < -np.pi:theta = -np.pi - thetatheta = np.pi - theta# 判断是否到达了目标点if distance < GOAL_REACHED_DIST:target = Truedone = True# 输出机器人状态robot_state = [distance, theta, action[0], action[1]]state = np.append(laser_state, robot_state)# 计算奖励reward = self.get_reward(target, collision, action, min_laser)# 返回状态、奖励、是否结束、是否到达目标点return state, reward, done, target那么我们可以总结出来step的流程
①发布cmd_vel控制话题
②开始仿真，运行0.1s（也可以更改），停止仿真
③获取目前的状态信息
④计算奖励
⑤返回状态、奖励、是否结束等信息
（3）reset部分
def reset(self):# 重置环境rospy.wait_for_service("/gazebo/reset_world")try:self.reset_proxy()except rospy.ServiceException as e:print("/gazebo/reset_simulation service call failed")angle = np.random.uniform(-np.pi, np.pi)quaternion = Quaternion.from_euler(0.0, 0.0, angle)object_state = self.set_self_statex = 0y = 0position_ok = Falsewhile not position_ok:x = np.random.uniform(-4.5, 4.5)y = np.random.uniform(-4.5, 4.5)position_ok = check_pos(x, y)# 下面通过 /gazebo/set_model_state 话题更改移动机器人的初始位置object_state.pose.position.x = xobject_state.pose.position.y = y# object_state.pose.position.z = 0.object_state.pose.orientation.x = quaternion.xobject_state.pose.orientation.y = quaternion.yobject_state.pose.orientation.z = quaternion.zobject_state.pose.orientation.w = quaternion.wself.set_state.publish(object_state)self.odom_x = object_state.pose.position.xself.odom_y = object_state.pose.position.y# 更改一个目标点self.change_goal()# 更改环境中的小方块障碍物self.random_box()self.publish_markers([0.0, 0.0])#开始仿真rospy.wait_for_service("/gazebo/unpause_physics")try:self.unpause()except (rospy.ServiceException) as e:print("/gazebo/unpause_physics service call failed")#执行一段时间time.sleep(TIME_DELTA)#停止仿真，获得目前的状态rospy.wait_for_service("/gazebo/pause_physics")try:self.pause()except (rospy.ServiceException) as e:print("/gazebo/pause_physics service call failed")v_state = []v_state[:] = self.velodyne_data[:]laser_state = [v_state]distance = np.linalg.norm([self.odom_x - self.goal_x, self.odom_y - self.goal_y])skew_x = self.goal_x - self.odom_xskew_y = self.goal_y - self.odom_ydot = skew_x * 1 + skew_y * 0mag1 = math.sqrt(math.pow(skew_x, 2) + math.pow(skew_y, 2))mag2 = math.sqrt(math.pow(1, 2) + math.pow(0, 2))beta = math.acos(dot / (mag1 * mag2))if skew_y < 0:if skew_x < 0:beta = -betaelse:beta = 0 - betatheta = beta - angleif theta > np.pi:theta = np.pi - thetatheta = -np.pi - thetaif theta < -np.pi:theta = -np.pi - thetatheta = np.pi - thetarobot_state = [distance, theta, 0.0, 0.0]state = np.append(laser_state, robot_state)#返回状态值return state那么我们可以总结出来step的流程
①使用 /gazebo/reset_world 重置仿真环境
②设置环境中的小车、障碍物等的位置
③开始仿真，运行0.1s（也可以更改），停止仿真
④获取目前的状态信息
⑤返回状态信息
（4）reward部分
@staticmethoddef get_reward(target, collision, action, min_laser):if target:return 100.0elif collision:return -100.0else:r3 = lambda x: 1 - x if x < 1 else 0.0return action[0] / 2 - abs(action[1]) / 2 - r3(min_laser) / 2这里即计算reward的部分，我们可以自己设定奖励函数，返回reward
（5）总结
那么我们在env部分最关键的代码就介绍完了！
这一部分其实本质上和在gym中没有什么不同，唯一的区别就是这里通过话题的订阅发布获取了环境状态信息。
同时有一个最重要的部分是，我们需要在每一次执行action的时候，让这个action运行那么0.1或者0.05秒，这样我们的小车才能够正常的运行一段距离。如果不设置这个运行时间的话，小车的action就会以最快的速度发布，那么根本就没有办法正常运行，step也会瞬间爆炸。
3、train_velodyne_td3.py文件
这一部分就是训练网络参数的python文件了。
其中的1-287行是TD3网络结构以及各种参数设定，这部分就不介绍了。
从288行的while是训练的主要部分，这部分本质上和在gym中也没有什么不同。拿出其中最重要的流程说一下。
#291if done:''''''#314state = env.reset()done = False# 这部分是当结束本回合后的重置部分，使用env.reset()重置环境，给小车和障碍物重新分配位置，并对其他变量初始化''''''#325action = network.get_action(np.array(state))#这部分是将我们观测获得的状态输入到神经网络之中，从中获取动作''''''#348a_in = [(action[0]+1)/2,action[1]]next_state, reward, done, target = env.step(a_in)#这里我们将动作输入到环境之中，进行step操作，step函数返回next_state,reward,done,target那么我们可以看到这其实和任何强化学习中的训练步骤都基本一样，最后保存网络参数即可。
三、如何部署到实车上
（1）仿真训练得到好的网络
经过仿真训练后，能够得到ac网络各自的网络参数
（2）网络参数载入
class Actor(nn.Module):def __init__(self, state_dim, action_dim):super(Actor, self).__init__()self.layer_1 = nn.Linear(state_dim, 800)self.layer_2 = nn.Linear(800, 600)self.layer_3 = nn.Linear(600, action_dim)self.tanh = nn.Tanh()def forward(self, s):s = F.relu(self.layer_1(s))s = F.relu(self.layer_2(s))a = self.tanh(self.layer_3(s))return a# TD3 networkclass TD3(object):def __init__(self, state_dim, action_dim):# Initialize the Actor networkself.device = torch.device("cuda" if torch.cuda.is_available() else "cpu")# cuda or cpuself.actor = Actor(state_dim, action_dim)self.actor.load_state_dict(torch.load("/home/polya/DRL-robot-navigation-main/pytorch_models180/TD3_final_actor.pth"))self.actor = self.actor.to(self.device)self.max_action = 1def get_action(self, state):# Function to get the action from the actorstate = torch.Tensor(state.reshape(1, -1)).to(self.device)action = self.actor(state).cpu().data.numpy().flatten()action = action.clip(-self.max_action,self.max_action)linear_v = (action[0]+1)/2angle_v = action[1]return linear_v,angle_vdef load(self, filename, directory):self.actor.load_state_dict(torch.load("%s/%s_actor.pth" % (directory, filename)))这里我们其实只需要一个actor网络就可以了，函数则只需要一个get_action
当然也可以直接按照之前的网络来写，这样你还可以顺便收集实际数据，进行进一步训练。
（3）实车信息获取与处理
#-------------------Pub and Sub--------------self.GoalPub = rospy.Publisher("/GoalPoint",Marker,queue_size=10)self.LaserSub = rospy.Subscriber("/scan",LaserScan,self.LaserScanCallBack)self.OdomSub = rospy.Subscriber("/odom",Odometry,self.OdomCallBack)self.goalxSub = rospy.Subscriber("/goal_x",Float32,self.goalxCallBack)self.goalySub = rospy.Subscriber("/goal_y",Float32,self.goalyCallBack)这里我是用了一个Publisher 发布/GoalPoint 话题，这个就是把目标点做成Marker发出去，仅仅是为了在Rviz中进行显示
订阅：1、/scan 激光雷达信息，这里我用的是一个2D激光雷达，获取的扫描信息转成了PointCloud2点云信息并用于后面的计算，当然我在训练过程中也用的是一个2D激光雷达进行的训练
2、/odom 里程计信息，用于实时获取实车目前的位置和速度，当然这里你也可以换成什么gps imu之类的信息，只需要最后都转变成为训练时对应的状态输入
3、/goal_x和/goal_y 。 这里是接收我们目标位置的坐标，因为偷懒直接写了两个float32 Subscriber来接收，后面可以改成自定义信息或者什么里程计信息
接下来是整合信息，返回state状态数组
def GetStates(self): #其实这里和之前的velody_env.py中获取信息的方式大同小异target =Falsev_state = []v_state[:] = self.velodyne_data[:]#雷达状态输入laser_state = [v_state]self.odom_x = self.last_odom.pose.pose.position.xself.odom_y = self.last_odom.pose.pose.position.yquaternion = Quaternion(self.last_odom.pose.pose.orientation.w,self.last_odom.pose.pose.orientation.x,self.last_odom.pose.pose.orientation.y,self.last_odom.pose.pose.orientation.z,)euler = quaternion.to_euler(degrees=False)angle = round(euler[2], 4)distance = np.linalg.norm([self.odom_x - self.goal_x, self.odom_y - self.goal_y])skew_x = self.goal_x - self.odom_xskew_y = self.goal_y - self.odom_ydot = skew_x * 1 + skew_y * 0mag1 = math.sqrt(math.pow(skew_x, 2) + math.pow(skew_y, 2))mag2 = math.sqrt(math.pow(1, 2) + math.pow(0, 2))beta = math.acos(dot / (mag1 * mag2))if skew_y < 0:if skew_x < 0:beta = -betaelse:beta = 0 - betatheta = beta - angleif theta > np.pi:theta = np.pi - thetatheta = -np.pi - thetaif theta < -np.pi:theta = -np.pi - thetatheta = np.pi - thetaself.dis = distanceif distance < GOAL_REACHED_DIST:target = True#机器人状态输入robot_state = [distance, theta, self.last_odom.twist.twist.linear.x,self.last_odom.twist.twist.angular.z]print(self.dis)print(distance)print(robot_state)state = np.append(laser_state,robot_state)# 最后我们返回小车的状态输入，包括雷达输入和机器人状态输入以及是否到达了目标位置return state,target（4）简单的执行部分
rospy.init_node("my_car") #初始化节点car = mycar()#实例化实车信息获取类# --------偷懒的目标点发布---------------xpub = rospy.Publisher("/goal_x",Float32,queue_size=10)ypub = rospy.Publisher("/goal_y",Float32,queue_size=10)xpub.publish(2.5)ypub.publish(0.5)network = TD3(24,2) #实例化TD3网络pub = rospy.Publisher("/cmd_vel",Twist,queue_size=10) #/cmd_vel发布，用于控制小车的运动rate = rospy.Rate(10) #等待频率，相当于仿真环境中的DELTA_TIMEwhile not rospy.is_shutdown():state,target = car.GetStates() #获取状态信息if not target:linear_x,angular_z = network.get_action(np.array(state)) #将状态输入到网络之中，输出线速度和角速度cmd_vel = Twist() #将速度放到Twist中发布cmd_vel.linear.x = linear_x/5 #这里发现最快1m/s的话还是太快了，所以又手动降为最大0.2cmd_vel.angular.z = angular_z/5pub.publish(cmd_vel) #发布消息if state[20]<COLLISION_DIST: #简易判断碰撞pub.publish(Twist())breakif target: #简易判断是否到达目标点pub.publish(Twist())breakrate.sleep() #停止一段时间，让这里设置为0.1s，那么控制频率就是10hz然后运行，我们可以打开rviz，看到小车的移动了！
不过这里我用的小车其实仿真环境中的不是一个小车，em但是他们的控制方式也比较相似吧，最后也能实现相应的效果，最正确的方式是将你们自己小车的urdf导入到gazebo中进行仿真！
对了，这里用的是jupyter来写的，我发现在jupyter里面搞ROS的算法部分真的是很方便啊！值得一试！