ROS 自动驾驶多点巡航

news2024/11/7 13:49:56

ROS 自动驾驶多点巡航:

1、首先创建工作空间:

基于我们的artca_ws;

2、创建功能包:

进入src目录,输入命令:

catkin_create_pkg point_pkg std_msgs rospy roscpp

test_pkg 为功能包名,后面两个是依赖;
在这里插入图片描述

3、创建python文件

我们通过vscode打开src下功能包:
创建 point.py:
在这里插入图片描述
代码内容写入 :

#!/usr/bin/env python  
import rospy  
import actionlib  
import collections
from actionlib_msgs.msg import *  
from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist  
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal  
from random import sample  
from math import pow, sqrt  
  
class MultiNav():  
    def __init__(self):  
        rospy.init_node('MultiNav', anonymous=True)  
        rospy.on_shutdown(self.shutdown)  
  
        # How long in seconds should the robot pause at each location?  
        self.rest_time = rospy.get_param("~rest_time", 10)  
  
        # Are we running in the fake simulator?  
        self.fake_test = rospy.get_param("~fake_test", False)  
  
        # Goal state return values  
        goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED','SUCCEEDED',  
                       'ABORTED', 'REJECTED','PREEMPTING', 'RECALLING',   
                       'RECALLED','LOST']  
  
        # Set up the goal locations. Poses are defined in the map frame.  
        # An easy way to find the pose coordinates is to point-and-click  
        # Nav Goals in RViz when running in the simulator.  
        # Pose coordinates are then displayed in the terminal  
        # that was used to launch RViz.  
 
        
        locations = collections.OrderedDict()  
        locations['point-1'] = Pose(Point(5.21, -2.07, 0.00), Quaternion(0.000, 0.000, -0.69, 0.72)) 
        locations['point-2'] = Pose(Point(3.50, -5.78, 0.00), Quaternion(0.000, 0.000, 0.99, 0.021))
        #locations['point-3'] = Pose(Point(-6.95, 2.26, 0.00), Quaternion(0.000, 0.000, 0.000, 1.000))
        #locations['point-4'] = Pose(Point(-6.50, 2.04, 0.00), Quaternion(0.000, 0.000, 0.000, 1.000))
        
        # Publisher to manually control the robot (e.g. to stop it)  
        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)  
  
        # Subscribe to the move_base action server  
        self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)  
        rospy.loginfo("Waiting for move_base action server...")  
  
        # Wait 60 seconds for the action server to become available  
        self.move_base.wait_for_server(rospy.Duration(10))  
        rospy.loginfo("Connected to move base server")  
          
        # A variable to hold the initial pose of the robot to be set by the user in RViz  
        initial_pose = PoseWithCovarianceStamped()  
        # Variables to keep track of success rate, running time, and distance traveled  
        n_locations = len(locations)  
        n_goals = 0  
        n_successes = 0  
        i = 0  
        distance_traveled = 0  
        start_time = rospy.Time.now()  
        running_time = 0  
        location = ""  
        last_location = ""  
        # Get the initial pose from the user  
        rospy.loginfo("Click on the map in RViz to set the intial pose...")  
        rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)  
        self.last_location = Pose()  
        rospy.Subscriber('initialpose', PoseWithCovarianceStamped, self.update_initial_pose) 
 
	keyinput = int(input("Input 0 to continue,or reget the initialpose!\n"))
	while keyinput != 0:
            rospy.loginfo("Click on the map in RViz to set the intial pose...")  
            rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)  
            rospy.Subscriber('initialpose', PoseWithCovarianceStamped, self.update_initial_pose) 
	    rospy.loginfo("Press y to continue,or reget the initialpose!")
	    keyinput = int(input("Input 0 to continue,or reget the initialpose!"))
 
        # Make sure we have the initial pose  
        while initial_pose.header.stamp == "":  
            rospy.sleep(1)  
        rospy.loginfo("Starting navigation test")  
  
        # Begin the main loop and run through a sequence of locations  
        for location in locations.keys():  
  
            rospy.loginfo("Updating current pose.")  
            distance = sqrt(pow(locations[location].position.x  
                           - initial_pose.pose.pose.position.x, 2) +  
                           pow(locations[location].position.y -  
                           initial_pose.pose.pose.position.y, 2))  
            initial_pose.header.stamp = ""  
  
            # Store the last location for distance calculations  
            last_location = location  
  
            # Increment the counters  
            i += 1  
            n_goals += 1  
  
            # Set up the next goal location  
            self.goal = MoveBaseGoal()  
            self.goal.target_pose.pose = locations[location]  
            self.goal.target_pose.header.frame_id = 'map'  
            self.goal.target_pose.header.stamp = rospy.Time.now()  
  
            # Let the user know where the robot is going next  
            rospy.loginfo("Going to: " + str(location))  
            # Start the robot toward the next location  
            self.move_base.send_goal(self.goal)  
  
            # Allow 5 minutes to get there  
            finished_within_time = self.move_base.wait_for_result(rospy.Duration(300))  
  
            # Check for success or failure  
            if not finished_within_time:  
                self.move_base.cancel_goal()  
                rospy.loginfo("Timed out achieving goal")  
            else:  
                state = self.move_base.get_state()  
                if state == GoalStatus.SUCCEEDED:  
                    rospy.loginfo("Goal succeeded!")  
                    n_successes += 1  
                    distance_traveled += distance  
                else:  
                    rospy.loginfo("Goal failed with error code: " + str(goal_states[state]))  
  
            # How long have we been running?  
            running_time = rospy.Time.now() - start_time  
            running_time = running_time.secs / 60.0  
  
            # Print a summary success/failure, distance traveled and time elapsed  
            rospy.loginfo("Success so far: " + str(n_successes) + "/" +  
                          str(n_goals) + " = " + str(100 * n_successes/n_goals) + "%")  
            rospy.loginfo("Running time: " + str(trunc(running_time, 1)) +  
                          " min Distance: " + str(trunc(distance_traveled, 1)) + " m")  
            rospy.sleep(self.rest_time)  
  
    def update_initial_pose(self, initial_pose):  
        self.initial_pose = initial_pose  
  
    def shutdown(self):  
        rospy.loginfo("Stopping the robot...")  
        self.move_base.cancel_goal()  
        rospy.sleep(2)  
        self.cmd_vel_pub.publish(Twist())  
        rospy.sleep(1)  
def trunc(f, n):  
  
    # Truncates/pads a float f to n decimal places without rounding  
    slen = len('%.*f' % (n, f))  
    return float(str(f)[:slen])  
  
if __name__ == '__main__':  
    try:  
        MultiNav()  
        rospy.spin()  
    except rospy.ROSInterruptException:  
        rospy.loginfo("AMCL navigation test finished.")  

4、编译:

nano@nano-desktop:~/artcar_ws/src$ cd ..
nano@nano-desktop:~/artcar_ws$ catkin build 

在这里插入图片描述

5、案例实操;

启动小车并进入到相应环境:

(1)打开终端,启动底盘环境,输入如下命令:

$ roslaunch artcar_nav artcar_bringup.launch

(2)启动导航程序:

$ roslaunch artcar_nav artcar_move_base.launch

(3)启动RVIZ:

(4)获取点位:

 rostopic echo /move_base_sile/goal 

获取点位

roscar@roscar-virtual-machine:~/artcar_simulation/src$ rostopic echo /move_base_simple/goal 
WARNING: no messages received and simulated time is active.
Is /clock being published?
header: 
  seq: 0
  stamp: 
    secs: 405
    nsecs: 141000000
  frame_id: "odom"
pose: 
  position: 
    x: 5.21420097351
    y: -2.07076597214
    z: 0.0
  orientation: 
    x: 0.0
    y: 0.0
    z: -0.69109139328
    w: 0.722767380375
---
header: 
  seq: 1
  stamp: 
    secs: 422
    nsecs:  52000000
  frame_id: "odom"
pose: 
  position: 
    x: 3.50902605057
    y: -5.78046607971
    z: 0.0
  orientation: 
    x: 0.0
    y: 0.0
    z: 0.999777096296
    w: 0.0211129752124
---

(5)修改point.py文件中点位数据的位置:

在这里插入图片描述

(6 ) 然后开启终端执行:

nano@nano-desktop:~/artcar_ws/src/point_pkg/src$ ./point.py 

在这里插入图片描述

此时确定位置是否准确,准确的话,在此终端中输入:0
小车开始多点运行。

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.coloradmin.cn/o/1819563.html

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈,一经查实,立即删除!

相关文章

新等保2.0防护体系方案

等保2.0防护体系 吉祥学安全知识星球🔗除了包含技术干货:Java代码审计、web安全、应急响应等,还包含了安全中常见的售前护网案例、售前方案、ppt等,同时也有面向学生的网络安全面试、护网面试等。 最近在写一些咨询相关的材料&…

怎么把日常的文件做二维码?适用excel、word、pdf制作二维码的方法

文件转换成二维码是将文件转成一个链接,将链接做成二维码之后,扫码就可以访问该链接中的文件内容,通过这种方式来实现文件的快速分享。将文件生成二维码能够随时修改内容,可以更新替换当前文件,不断的通过一个二维码来…

数据通信与网络

计算机网络的组成 计算机网络是由计算机系统、网络节点和通信链路等组成的系统。 逻辑上分为资源子网和通信子网。 CCP:communication control processor 通信控制处理机,网络节点,交换机、路由器等设备。 逻辑组成: &#xf…

二阶段提交(2pc)协议

二阶段提交(2pc)协议 1、 简介 二阶段提交算法是一个分布式一致性算法,强一致、中心化的原子提交协议,主要用来解决分布式事务问题。在单体spring应用中我们往往通过一个Transactional注解就可以保证方法的事务性,但…

线性代数|机器学习-P13计算特征值和奇异值

文章目录 1. 特征值1.1 特征值求解思路1.1 相似矩阵构造 1. 特征值 1.1 特征值求解思路 我们想要计算一个矩阵的特征值,一般是用如下公式: ∣ ∣ A − λ I ∣ ∣ 0 → λ 1 , λ 2 , ⋯ , λ n \begin{equation} ||A-\lambda I||0\rightarrow \lamb…

CPN Tools学习——时间和队列【重要】

-Timed Color Sets 时间颜色集 -Token Stamps 令牌时间戳 -Event Clock 全局/事件/模拟时钟 -Time Delays on Transitions过渡的时间延迟 - List Color Set列表颜色集 - Queue排队 1.时间颜色集 在定时CPN模型令牌中有: (1)象征性的颜…

CTFHUB-SQL注入-Cookie注入

由于本关是cookie注入,就不浪费时间判断注入了,在该页面使用 burp工具 抓包,修改cookie后面,加上SQL语句,关掉burp抓包,就可以在题目页面显示结果了 判断字段数量 发现字段数量是2列 使用id-1 union sele…

智慧工地:构筑未来建筑的智能脉络

在科技日新月异的今天,智慧城市的建设已不再局限于城市生活的方方面面,而是深入到了城市发展的每一个细胞——工地。本文旨在深度剖析智慧工地的核心价值、关键技术及对建筑业转型升级的深远影响。 一、智慧工地:定义与愿景 智慧工地是指运…

~$开头的临时文件是什么?可以删除吗?

(2023.12.4) 在进行Word文档编辑的时候,都会产生一个以~$开头的临时文件,它会自动备份文档编辑内容,若是正常关闭程序,这个文档就会自动消失;而在非正常情况下关闭word文档,如断电&…

智能座舱软件性能与可靠性的评估和改进

随着智能汽车的不断发展,智能座舱在性能与可靠性上暴露出体验不佳、投诉渐多的问题,本文从工程化的角度简述了如何构建智能座舱软件的评估框架,以及如何持续改进其性能和可靠性。 1. 智能座舱软件性能和可靠性表现不佳 据毕马威发布的《2023…

线程池前置知识

并发和并行 并发是指在单核CPU上,多个线程占用不同的CPU时间片。线程在物理上还是串行执行的,但是由于每个线程占用的CPU时间片非常短(比如10ms),看起来就像是多个线程都在共同执行一样,这样的场景称作并发…

认识线性调频信号(LFM)和脉冲压缩

目录 1. 线性调频(LFM)信号:2.Matlab仿真3.脉冲压缩 微信公众号获取更多FPGA相关源码: 1. 线性调频(LFM)信号: 在时域中,一个理想的线性调频信号或脉冲持续时间为T秒,…

CNAS认证是什么?怎么做?

在全球化日益深入的今天,产品质量和安全已经成为企业生存和发展的重要基石。而在这个过程中,CNAS认证作为一种权威性的认可机制,发挥着不可替代的作用。那么,CNAS认证究竟是什么?我们又该如何进行这一认证过程呢&#…

派能协议,逆变器测试问题记录

问题一:逆变器无法进行逆变 通过抓取逆变器与bms的通讯报文,如下: 根据派能协议,报文标黄的对应充放电状态,30 30对应的数据为0 0,说明充放电状态全部置0,导致逆变器无法逆变。 问题二&#xf…

安装好IDEA后,就能够直接开始跑代码了吗?

我实习的第一天,睿哥叫我安装了IDEA,然后我就照做了。 之后,我把gitlab的代码拉下来后,发现好像没有编译运行的按钮,所以我就跑去问睿哥。睿哥当时看了看后,发现原来我没有安装JDK,他就叫我安装…

下载elasticsearch-7.10.2教程

1、ES官网下载地址 Elasticsearch:官方分布式搜索和分析引擎 | Elastic 2、点击下载Elasticsearch 3、点击 View past releases,查看过去的版本 4、选择版本 Elasticsearch 7.10.2,点击 Download,进入下载详情 5、点击 LINUX X8…

LeetCode435无重叠区间

题目描述 给定一个区间的集合 intervals ,其中 intervals[i] [starti, endi] 。返回 需要移除区间的最小数量,使剩余区间互不重叠 。 解析 由于要删除尽可能少的区间 ,因此区间跨度大的一定是要先删除的,这样就有两种贪心思想了…

【ARM Cache 及 MMU 系列文章 6.2 -- ARMv8/v9 如何读取 Cache 内部数据并对其进行解析?】

请阅读【ARM Cache 及 MMU/MPU 系列文章专栏导读】 及【嵌入式开发学习必备专栏】 文章目录 Direct access to internal memoryL1 cache encodingsL1 Cache Data 寄存器Cache 数据读取代码实现测试结果Direct access to internal memory 在ARMv8架构中,缓存(Cache)是用来加…

D 25章 进程的终止

D 25章 进程的终止 440 25.1 进程的终止:_exit()和exit() 440 1. _exit(int status), status 定义了终止状态,父进程可调用 wait 获取。仅低8位可用, 调用 _exit() 总是成功的。 2.程序一般不会调用 _exit(), 而是…

海外盲盒APP系统开发:开拓国际盲盒市场

在互联网的传播下,盲盒在国内外都掀起了风潮,我国盲盒将具有文化元素的盲盒商品投向海外市场中,获得了海外消费者的喜爱,给我国盲盒企业提供了新的商业机遇。盲盒的未知性让玩家在拆盲盒的过程中享受到更多的惊喜感,为…