室外组二代车上手记录

import subprocess
import time
import os

# ps aux | grep ros | xargs -n 1 kill -9
# 杀死所有ros相关的终端

# stdout=subprocess.DEVNULL 和 stderr=subprocess.DEVNULL 会将标准输出和标准错误重定向到 DEVNULL，这意味着它们不会被打印到终端。
# 启动 Run_car.launch，不打印输出

subprocess.Popen("roslaunch racecar Run_car.launch", shell=True, stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
print("Run_car.launch已执行")
time.sleep(3)

# 启动 Run_gmapping.launch，不打印输出
subprocess.Popen("roslaunch racecar Run_gmapping_wy.launch", shell=True, stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
print("Run_gmapping已执行")
time.sleep(3)

# 启动 python 脚本，打印输出，按g保存文件，小写！
cmd = "python3 /racecar/src/auto_car/scripts/racecar_teleop.py"
os.system(cmd)

# subprocess是非阻塞的，os.system(cmd)是阻塞的

elif key == 'g' :
    cmd = "rosrun map_server map_saver -f /racecar/src/auto_car/rengong"
    os.system(cmd)
    break

import os
import rospy
from geometry_msgs.msg import PointStamped, PoseStamped
from nav_msgs.msg import Path
from scipy.interpolate import splprep, splev
import numpy as np
import json
from visualization_msgs.msg import Marker, MarkerArray
import time
import subprocess

print("是否加载历史json文件: \n 1.是 2.否，丢弃旧数据\n请输入数字1或2：")
data = []
clicked_point = -1
number = input()
if number == "1":
    with open('/racecar/src/auto_car/scripts/waypoint.json',"r") as f:
        data = json.load(f)["waypoint"]
        f.close()

# os.system(f"gnome-terminal -e 'bash -c \"roslaunch /home/racecar/car_2024_rengong/src/auto_go/launch/path_click.launch; exec bash\"'")
subprocess.Popen("roslaunch /racecar/src/auto_car/launch/path_click.launch", shell=True, stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
rospy.sleep(3)
rospy.init_node('way_point_click', anonymous=True)


# 在rviz里显示箭头
def show_arrow():
    global clicked_point,data
    marker_array = MarkerArray()
    for i,d in enumerate(data):
        marker = Marker()
        marker.header.frame_id = "map"
        marker.type = marker.ARROW
        marker.action = marker.ADD
        marker.ns = "marker"
        marker.scale.x = 0.5
        marker.scale.y = 0.1
        marker.scale.z = 0.1
        marker.color.a = 1.0
        marker.color.r = 0.0
        marker.color.g = 1.0
        marker.color.b = 0.0
        if i==clicked_point:
            marker.color.r = 1.0
            marker.color.g = 0.0
            marker.color.b = 0.0
        marker.pose.position.x = d[0]
        marker.pose.position.y = d[1]
        marker.pose.position.z = 0.0
        marker.pose.orientation.w = d[2]
        marker.pose.orientation.x = d[3]
        marker.pose.orientation.y = d[4]
        marker.pose.orientation.z = d[5]
        marker_array.markers.append(marker)
    id = 0
    for m in marker_array.markers:
        m.id = id
        id += 1
    marker_pub.publish(marker_array)

def click_goal_callback(msg):
    global data, clicked_point
    if clicked_point==-1:
        if len(data)>0 and (msg.pose.position.x-data[-1][0])**2 + (msg.pose.position.y-data[-1][1])**2 < 0.1:
            # 保存数据
            with open('/racecar/src/auto_car/scripts/waypoint.json',"w") as f:
                json.dump({"waypoint":data},f)
                f.close()
                print("保存成功")
            return
        data.append([msg.pose.position.x,msg.pose.position.y,msg.pose.orientation.w,msg.pose.orientation.x,msg.pose.orientation.y,msg.pose.orientation.z])
    else:
        data[clicked_point][0] = msg.pose.position.x
        data[clicked_point][1] = msg.pose.position.y
        data[clicked_point][2] = msg.pose.orientation.w
        data[clicked_point][3] = msg.pose.orientation.x
        data[clicked_point][4] = msg.pose.orientation.y
        data[clicked_point][5] = msg.pose.orientation.z
        clicked_point = -1
    show_arrow()

def clicked_callback(msg):
    # 寻找最近的点
    global data, clicked_point
    x = msg.point.x
    y = msg.point.y
    if len(data) == 0:
        return
    min_dis = 100000
    min_index = 0
    for i in range(len(data)):
        dis = (x-data[i][0])**2 + (y-data[i][1])**2
        if dis < min_dis:
            min_dis = dis
            min_index = i
    if min_dis<0.1:
        clicked_point = min_index
    else:
        clicked_point = -1
    show_arrow()
        
        
marker_pub = rospy.Publisher("/my_marker", MarkerArray, queue_size=5)
rospy.Subscriber("/move_base_simple/goal", PoseStamped, click_goal_callback)
rospy.Subscriber("clicked_point", PointStamped, clicked_callback)
show_arrow()
# 订阅move_base_sa
rospy.spin()

import subprocess
import time
import os

subprocess.Popen("roslaunch racecar Run_car.launch", shell=True, stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
print("Run_car.launch已执行")
time.sleep(3)
subprocess.Popen("roslaunch racecar amcl_nav_wy.launch", shell=True, stdout=subprocess.DEVNULL, stderr=subprocess.DEVNULL)
print("amcl_nav_wy.launch已执行")
time.sleep(2)

# 启动 python 脚本
cmd = "python3 /racecar/src/auto_car/scripts/wj_go_hh.py"
os.system(cmd)

import rospy
from geometry_msgs.msg import Twist
from geometry_msgs.msg import PoseWithCovarianceStamped

import math
import numpy as np

from wj_pid import PID

import json
import time

from nav_msgs.msg import Odometry
from std_msgs.msg import String, Float64
from move_base_msgs.msg import MoveBaseActionResult
from actionlib_msgs.msg import GoalStatusArray
from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped

# 参数配置，
# 角度从车右方逆时针旋转
Angle_min = 50.0 #右拐最大角
Angle_max = 130.0 #左拐最大角
Angle_mid = 90.0  #中间值

Speed_max = 1700
My_Speed = 1630

now_Goal=[0,0,0,1]

Distance_min = 1.0


is_start_light = 1  #是否开启红绿灯定点？
light_index = 0 
light_flag = 0

is_white = 1   #是开启斑马线定点？
white_go_index = 0
while_back_index = 0
white_flag = 0

is_rest = 0 #开启定点纠偏
rest_index = 0
rest_post = [6.18,-1.40]
rest_flag = 0

# 参数修改
start_light_post = [[8.39,-0.1],[10.8,-0.4],[12.6,-0.2],[7.25,-1.87],[10,-2],[12.3,-1.9]]
white_go = [3.0, -0.5]
while_back = [3.5,-1.1]

def find_closest_point(start_light_post, post):
    '''
    my_list格式为[[a,b],[c,d],[e,f]...]
    my_post格式为[a,b]
    该函数会返回my_list距离my_post最近的坐标的下标
    '''
    # 提取每个点的 x 和 y 坐标
    points = np.array(start_light_post)[:, :2]
    # 计算每个点与post的距离
    distances = np.sqrt((points[:, 0] - post[0])**2 + (points[:, 1] - post[1])**2)
    # 返回距离最小的点的下标
    return np.argmin(distances)


def send_goal(to_Goal, map_frame):
    goal_msg = PoseStamped()
    goal_msg.header.stamp = rospy.Time.now()
    goal_msg.header.frame_id = 'map'  # 坐标系应与目标点一致，通常是地图坐标系
    goal_msg.pose.position.x = to_Goal[0]  # 替换为目标点的 x 坐标
    goal_msg.pose.position.y = to_Goal[1]  # 替换为目标点的 y 坐标
    goal_msg.pose.position.z = 0.0  # 替换为目标点的 z 坐标
    # 设置目标点的姿态，通常不需要设置
    goal_msg.pose.orientation.x = 0.0
    goal_msg.pose.orientation.y = 0.0
    goal_msg.pose.orientation.z = to_Goal[3]
    goal_msg.pose.orientation.w = to_Goal[2]
    # 发布目标点
    goal_publisher.publish(goal_msg)
    rospy.loginfo("Goal sent!")
 
class go_to:
    def __init__(self):
        self.pose_amcl = rospy.Subscriber('/odometry/filtered',Odometry,self.amcl_pose_callback,queue_size=1)
        # self.pose_amcl = rospy.Subscriber("/amcl_pose", PoseWithCovarianceStamped, self.amcl_pose_callback, queue_size=10)
        self.now_position = [0,0,0,1]
        
    def amcl_pose_callback(self, data):
        position = data.pose.pose.position
        orientation = data.pose.pose.orientation
        self.now_position = [round(position.x, 3), round(position.y, 3), round(orientation.z, 3), round(orientation.w, 3)]
        

    
class controll:
    def __init__(self,Angle_min,Angle_max,Angle_mid,Speed_max,My_Speed):
        self.pub_speed = rospy.Publisher('/car/cmd_vel', Twist, queue_size=1)
        self.Angle_min=Angle_min
        self.Angle_max=Angle_max
        self.Angle_mid=Angle_mid
        self.Speed_max=Speed_max
        self.My_Speed=My_Speed
        self.pid = PID(Kp=1.2, Ki=0, Kd=0, setpoint=0)

        self.pub_stop = rospy.Publisher('/light', String, queue_size=1)
    
    def go_goal(self,To_Goal,Now_Goal):
        '''
        To_Goal表示目标点，为一个列表，内有4个数据，依次为，位置x,位置y,方向z,方向w
        Now_Goal表示当前位置
        ''' 
        vector_point=[To_Goal[0]-Now_Goal[0],To_Goal[1]-Now_Goal[1],0]
        print("vector_point"+str(vector_point))
        ang = quaternion_to_euler(Now_Goal[2], Now_Goal[3])
        print("ang"+str(ang))
        ang = ang*math.pi/180
        print("ang"+str(ang))
        vector_angle = [round(math.cos(ang), 3),round(math.sin(ang), 3),0]
        print("vector_angle"+str(vector_angle))
        final_ang = angle_between_vectors(vector_angle, vector_point)
        return final_ang
    
    def angle_control(self,angle_1):
        angle_2 = self.Angle_mid + self.pid.update(angle_1)
        print("angle_2", angle_2)

        # 舵机限幅
        if angle_2 < self.Angle_min:
            return self.Angle_min
        elif angle_2 > self.Angle_max:
            return self.Angle_max
        else:
            return angle_2
            
    def publish_velocity(self, linear_x, angular_z):
        twist = Twist()
        twist.linear.x = linear_x
        twist.linear.y = 0
        twist.linear.z = 0
        twist.angular.z = angular_z
        twist.angular.x = 0
        twist.angular.y = 0
        self.pub_speed.publish(twist)

def work1():
    rospy.spin()
    # print("to_Goal",to_Goal)
    # print("now_Goal",go_to_obj.now_position)      



if __name__ == "__main__": 
    with open('/racecar/src/auto_car/scripts/waypoint.json', 'r') as f:
        data = json.load(f)
        f.close()

    index_point=0
    waypoint = data["waypoint"]
    print(waypoint)
    point_len = len(waypoint)    

    print("go?")
    www = 5
    aaa=input()

    start_time = time.time()

    if is_start_light == 1:
        # 开启定点
        light_index = find_closest_point(waypoint, start_light_post[int(www)])
        print("最经点的下标为：" + str(light_index))
    white_go_index = find_closest_point(waypoint,white_go)
    while_back_index = find_closest_point(waypoint,while_back)
    rest_index = find_closest_point(waypoint,rest_post)

    rospy.init_node("wj_go", anonymous=True)
    go_to_obj = go_to()
    control = controll(Angle_min,Angle_max,Angle_mid,Speed_max,My_Speed)
    goal_publisher = rospy.Publisher('/move_base_simple/goal', PoseStamped, queue_size=10)
    map_frame = rospy.get_param('~map_frame', 'map' )
    time.sleep(1)
    # t1 = Thread(target=work1)
    # t1.start() 
    to_Goal = [waypoint[index_point][0],waypoint[index_point][1],waypoint[index_point][2],waypoint[index_point][5]]
    send_goal(to_Goal,map_frame)
    while True:
        now_Goal = go_to_obj.now_position
        distance = np.sqrt((to_Goal[0]-now_Goal[0])**2+(to_Goal[1]-now_Goal[1])**2)
        if (distance <= Distance_min and index_point <= point_len-1):

            if index_point == point_len -1 :
                pass
                print("over")
                finish_time = time.time()
                print(finish_time-start_time)
                while True:   
                    control.publish_velocity(1200,90)             
            index_point = index_point + 1



            if index_point == rest_index and  is_rest == 1 and rest_flag == 0:
                now_time = time.time()
                control.pub_stop.publish("red")
                while time.time()-now_time < 2.3:
                    pass
                    #control.publish_velocity(1200,90)
                control.pub_stop.publish("green")

                rest_flag = 1  



            if index_point == light_index and is_start_light==1 and light_flag == 0:

                now_time = time.time()
                control.pub_stop.publish("red")
                while time.time()-now_time < 2.3:
                    pass
                    #control.publish_velocity(1200,90)
                control.pub_stop.publish("green")

                light_flag = 1  


            if index_point == white_go_index and is_white ==1 and white_flag == 0:

                now_time = time.time()
                control.pub_stop.publish("red")
                while time.time()-now_time < 2.3:
                    pass
                    #control.publish_velocity(1200,90)
                control.pub_stop.publish("green")

                white_flag = 1   

            if index_point == while_back_index and is_white ==1 and white_flag == 1:

                now_time = time.time()
                control.pub_stop.publish("red")
                while time.time()-now_time < 2.3:
                    pass
                    #control.publish_velocity(1200,90)
                control.pub_stop.publish("green")

                white_flag = 2

            to_Goal = [waypoint[index_point][0],waypoint[index_point][1],waypoint[index_point][2],waypoint[index_point][5]]



            send_goal(to_Goal,map_frame)