Problems with custom parking control using movetoXY and setspeed.

[zsdfaker]

I used "movetoXY" and "setspeed" for custom parking control. However, after the vehicle has completed parking with a certain point in the parking space as the destination, it still moves. How can I solve this problem?

[namdre]

moveToXY does not register the vehicle with the parkingSpace and also forces a vehicle which is already parking back into the network. Since each of your spaces is already an individual parkingArea, calling traci.vehicle.setParkingAreaStop should be enough to have it park in that spot.

[zsdfaker]

@namdre Thank you for your reply. My intention is to make the vehicle park in with a more realistic trajectory into the parking space, so I use movetoXY to achieve this function.

[namdre]

In this case, you need to make sure that the last part of the maneuver only consists of a call to traci.vehicle.setParkingAreaStop without any moveToXY. If you right-click the vehicle and select 'show current route' the vehicle should be labeled "stopped" and the parameter dialog should have stop info: parking

[zsdfaker]

@namdre Well, actually what I mean is that I'm developing a custom parking approach to substitute for traci.vehicle.setParkingAreaStop. I utilize movetoXY to maneuver the car following the parking trajectory I've devised.

[namdre]

the purpose of setParkingAreaStop is to make sure that the vehicle registers with the parkingArea and stops moving. It should not prevent you from applying custom movement code beforehand.

[zsdfaker]

@namdre Thank you for your reply. Perhaps I didn't express myself clearly. Here's my running situation.

screenshot20250110.mp4

[zsdfaker]

I need to make the end point of the parking trajectory further back.

Screencast.2025-01-10.09.55.31.mp4

[namdre]

what is the status of the vehicle when it moves to the wrong spot? (see my previous post on "show current route" and parameter dialog stop info)

[zsdfaker]

The status of normal parking is "teleporting", while there is no status displayed in error cases. The stop info for both is blank.

[namdre]

This means that the vehicle hasn't actually reached the parking spot and isn't parking form the viewpoint of the simulation.
Which sumo options are you using in your simulation?

[zsdfaker]

import random
import os
import sys
import optparse
import traci
from sumolib import checkBinary
import pandas as pd
from Environment import Environment
from Allocation import Allocation
from Com_perception import Com_perception
from CrossLaneParking import CrossLaneParking
import math
import numpy as np
if 'SUMO_HOME' in os.environ:
    # print("great! sumo is in path!\n")
    sys.path.append(os.path.join(os.environ['SUMO_HOME'], 'tools'))
else:
    sys.exit("please declare environment variable 'SUMO_HOME'")

# 泊车轨迹（曲线+直线）生成函数
def generate_arc_reverse_trajectory(R, theta, L_back, delta_s):
    # 弧线段轨迹
    arc_points = []
    x_c, y_c = -R + 1.25, 0  # 起点坐标
    delta_theta = delta_s / R  # 每步的弧度增量
    num_arc_points = int(theta / delta_theta) + 1
    for i in range(num_arc_points):
        theta_i = np.pi/4 + i * delta_theta
        x = x_c + R * np.sin(theta_i)
        y = y_c + R * (1 - np.cos(theta_i))
        arc_points.append((x, y, theta_i))
    
    # 弧线结束点和方向角
    x_f, y_f, final_angle = arc_points[-1]  # 弧线结束点
    # final_angle = theta         # 弧线结束方向（弧度）

    # 倒车段轨迹
    reverse_points = []
    num_reverse_points = int(L_back / delta_s) + 1
    for i in range(num_reverse_points):
        s = i * delta_s
        x = x_f - s * np.cos(final_angle)
        y = y_f - s * np.sin(final_angle)
        reverse_points.append((x, y, final_angle))
    
    # 合并轨迹
    trajectory = arc_points + reverse_points
    return trajectory
# 泊车轨迹 贝塞尔曲线+圆弧+直线 生成函数
def generate_double_arc_trajectory(R, theta, L, delta_s):
    # 第一段轨迹
    arc1_points = []
    # 起点和终点
    P0 = np.array([0, 0])
    P3 = np.array([5.255, 2.67])

    # 控制点
    d1 = 1.5 #起点控制点距离
    d2 = 1.5 #终点控制点距离
    P1 = np.array([d1, 0])
    P2 = np.array([5.255 - d2, 2.67])

    # 贝塞尔曲线函数
    def bezier(t, P0, P1, P2, P3):
        return (1-t)**3 * P0 + 3*(1-t)**2 * t * P1 + 3*(1-t) * t**2 * P2 + t**3 * P3
    def bezier_derivative(t, P0, P1, P2, P3):
        return 3*(1-t)**2 * (P1 - P0) + 6*(1-t)*t * (P2 - P1) + 3*t**2 * (P3 - P2)
    num_arc1_points = 11
    t_values = np.linspace(0, 1, num_arc1_points)
    for i in t_values:
        point = np.array(bezier(i, P0, P1, P2, P3))
        print(f"bezier的输出点{point}")
        derivatives = np.array(bezier_derivative(i, P0, P1, P2, P3))
        angle = np.arctan2(derivatives[1], derivatives[0])
        arc1_points.append((point[0],point[1],angle))

    # 第1段结束点和方向角
    x_f1, y_f1 = 5.255, 2.67  # 第1段终点
    
    # 第2段轨迹
    arc2_points = []
    x_c2, y_c2 = x_f1, y_f1  # 第2段起点（即第1段终点）
    delta_theta = delta_s / R
    num_arc2_points = int(theta / delta_theta) + 1
    
    for i in range(num_arc2_points):
        theta_i = i * delta_theta
        x = x_c2 - R * np.sin(theta_i)
        y = y_c2 - R * (1 - np.cos(theta_i))
        arc2_points.append((x, y, theta_i))

    # 第3段轨迹
    arc3_points = []
    x_c3, y_c3 = arc2_points[-1][0], arc2_points[-1][1] # 第3段起点（即第2段终点）
    final_angle = np.pi/2
    num_reverse_points = int(L / delta_s) + 1
    for i in range(num_reverse_points):
        s = i * delta_s
        x = x_c3 - s * np.cos(final_angle)
        y = y_c3 - s * np.sin(final_angle)
        arc3_points.append((x, y, final_angle))
    # 合并轨迹
    trajectory = arc1_points + arc2_points + arc3_points

    return trajectory
# 停车位对应路点获取
def get_parking_position(parking_area):
    lane_id = traci.parkingarea.getLaneID(parking_area)
    lane_shape = traci.lane.getShape(lane_id)
    parking_start_pos = traci.parkingarea.getStartPos(parking_area)
    
    # 使用插值计算停车位置
    lane_length = traci.lane.getLength(lane_id)
    # parkingspace_start_pos = parking_start_pos + areatostart + parkingspace_width/2 

    ratio = parking_start_pos / lane_length
    
    # 如果是只有两个点的简单车道
    if len(lane_shape) == 2:
        x = lane_shape[0][0] + (lane_shape[1][0] - lane_shape[0][0]) * ratio
        y = lane_shape[0][1] + (lane_shape[1][1] - lane_shape[0][1]) * ratio
        return (x, y)
    else:
        # 对于复杂形状的车道，需要更复杂的插值计算
        # 这里使用简化版本，取最近的形状点
        total_length = 0
        target_length = lane_length * ratio
        
        for i in range(len(lane_shape) - 1):
            segment_length = ((lane_shape[i+1][0] - lane_shape[i][0])**2 + 
                            (lane_shape[i+1][1] - lane_shape[i][1])**2)**0.5
            if total_length + segment_length >= target_length:
                segment_ratio = (target_length - total_length) / segment_length
                x = lane_shape[i][0] + (lane_shape[i+1][0] - lane_shape[i][0]) * segment_ratio
                y = lane_shape[i][1] + (lane_shape[i+1][1] - lane_shape[i][1]) * segment_ratio
                return (x, y)
            total_length += segment_length
        
        return lane_shape[-1]  # 如果计算出错，返回车道终点
# 旋转矩阵计算
def compute_rotation_matrix(a, b, c, d):
    # 计算方向向量
    dx = c - a
    dy = d - b
    
    # 计算方向向量的长度
    L = np.sqrt(dx**2 + dy**2)
    
    # 单位方向向量的分量
    cos_theta = dx / L
    sin_theta = dy / L
    
    # 构造旋转矩阵
    rotation_matrix = np.array([
        [cos_theta, sin_theta],
        [-sin_theta, cos_theta]
    ])
    
    return rotation_matrix
# 坐标变换
def transform_to_global_coordinates(point, lane_angle, x, y, rotation_matrix):
    # 旋转矩阵的逆，用于从局部坐标系旋转回全局坐标系

    rotation_matrix_inv = rotation_matrix.T  # 旋转矩阵的转置即为其逆矩阵
    X_local, Y_local, angle_local = point[0],point[1],point[2]
    # 局部坐标转换到全局坐标
    point_local = np.array([X_local, Y_local])
    point_global_offset = np.dot(rotation_matrix_inv, point_local)
    
    # 加回局部坐标系原点的全局位置
    point_global = point_global_offset + np.array([x, y])
    
    # return np.append(point_global,angle_local* 180/np.pi + lane_angle)
    return np.append(point_global, lane_angle - angle_local* 180/np.pi)
# 轨迹坐标变换
def trajectory_transform(trajectory, parking_area):
    transform_trajectory = []
    lane_id = traci.parkingarea.getLaneID(parking_area)
    lane_shape = traci.lane.getShape(lane_id)
    lane_angle = traci.lane.getAngle(lane_id)
    print(f"道路的角度：{lane_angle}")
    matrix = compute_rotation_matrix(lane_shape[0][0],lane_shape[0][1],lane_shape[1][0],lane_shape[1][1])
    origin_x,origin_y = get_parking_position(parking_area)
    for point in trajectory:
        point_global = transform_to_global_coordinates(point,lane_angle,origin_x,origin_y,matrix)
        transform_trajectory.append(point_global)
    return transform_trajectory
# 车位颜色变化函数
def change_parkingspace_color(parking_area, color):
    parking_width = 2.5
    parking_height = 5
    lane_width = 2.67
    origin_x,origin_y = get_parking_position(parking_area)
    lane_id = traci.parkingarea.getLaneID(parking_area)
    lane_shape = traci.lane.getShape(lane_id)
    parkingspace_local = np.array([[0,-lane_width/2], [0,-lane_width/2-parking_height], [parking_width,-lane_width/2-parking_height], [parking_width,-lane_width/2]])
    matrix = compute_rotation_matrix(lane_shape[0][0],lane_shape[0][1],lane_shape[1][0],lane_shape[1][1])
    rotation_matrix_inv = matrix.T
    polygon_shape = [] 
    for coor in parkingspace_local:
        point_global_offset = np.dot(rotation_matrix_inv, coor)
        point_global = point_global_offset + np.array([origin_x, origin_y])
        polygon_shape.append(point_global)
    polygon_id = f"{parking_area}_space"
    try:
        traci.polygon.add(polygon_id, polygon_shape, color=color, fill=False, layer=0, lineWidth=0.2)
        return True
    except:
        return False

# 获取停车位中心坐标    
def get_parkingarea_center(parking_area):
    parking_width = 2.5
    parking_height = 5
    lane_width = 2.67
    origin_x,origin_y = get_parking_position(parking_area)
    lane_id = traci.parkingarea.getLaneID(parking_area)
    lane_shape = traci.lane.getShape(lane_id)
    parkingspace_local = np.array([parking_width,-lane_width/2])
    matrix = compute_rotation_matrix(lane_shape[0][0],lane_shape[0][1],lane_shape[1][0],lane_shape[1][1])
    rotation_matrix_inv = matrix.T
    point_global_offset = np.dot(rotation_matrix_inv, parkingspace_local)
    point_global = point_global_offset + np.array([origin_x, origin_y])
    return point_global


# 车位颜色清除
def remove_parkingspace_color(parking_area):
    polygon_id = f"{parking_area}_space"
    try:
        traci.polygon.remove(polygon_id)
        return True
    except:
        return False
# 自定义泊车
def custom_parking(veh_id, parking_area, trajectory):
    # 获取目标停车位置
    target_x, target_y = get_parking_position(parking_area)
    trajectory_out = trajectory_transform(trajectory,parking_area)
    trajectory_len = len(trajectory_out)
    # 获取车道信息
    lane_id = traci.parkingarea.getLaneID(parking_area)
    
    # 获取车辆当前位置
    current_x, current_y = traci.vehicle.getPosition(veh_id)
    
    def distance_to_target(x1, y1, x2, y2):
        return math.sqrt((x2-x1)**2 + (y2-y1)**2)
    
    # 泊车状态机
    APPROACHING = 0
    ALIGNING = 1
    BACKING = 2
    FINAL_ADJUSTMENT = 3
    
    state = APPROACHING
    t = 0
    while True:
        current_x, current_y = traci.vehicle.getPosition(veh_id)
        current_angle = traci.vehicle.getAngle(veh_id)
        dist_to_target = distance_to_target(current_x, current_y, target_x, target_y)
        print(f"车辆当前角度：{current_angle}")
        if state == APPROACHING:
            print("APPROACHING state")
            # 降速接近目标点
            if dist_to_target > 3:
                traci.vehicle.slowDown(veh_id, 5.0, 2.0)
            else:
                state = BACKING
                traci.vehicle.slowDown(veh_id, 2.0, 1.0)
                                
        elif state == BACKING:
            # 倒车入库
            print("BACKING state")
            # steps = 10
            for i in range(trajectory_len):
                # progress = 1/len(trajectory_out)
                x = trajectory_out[i][0]  
                y = trajectory_out[i][1] 
                angle = trajectory_out[i][2] 
                print(f"第{i}步微调坐标为:（{x},{y}）")
                print(f"第{i}步微调角度为：{angle}")
                
                traci.vehicle.moveToXY(veh_id, lane_id, 0, x, y, angle, 3)
                traci.simulationStep()   
            state = FINAL_ADJUSTMENT
                
        elif state == FINAL_ADJUSTMENT:
            print("FINAL_ADJUSTMENT state")
            # 最终位置微调
            final_x, final_y, final_angle = trajectory_out[-1][0], trajectory_out[-1][1], trajectory_out[-1][2]
            print(f"最终微调坐标为:（{final_x},{final_y}）最终角度为：{final_angle}")
            traci.vehicle.moveToXY(veh_id, lane_id, 0, final_x, final_y, final_angle, 3)
            traci.vehicle.setSpeed(veh_id, 0)
            # traci.simulationStep()
            break
            
        traci.simulationStep()
        
    # 设置最终停车状态
    # traci.vehicle.setSpeed(veh_id, 0)
    return True

def get_options():
    optParser = optparse.OptionParser()
    optParser.add_option("--nogui", action="store_true",
                         default=False, help="run the commandline version of sumo")
    options, args = optParser.parse_args()
    return options

def main():
    options = get_options()
    if options.nogui:
        sumoBinary = checkBinary('sumo')
    else:
        sumoBinary = checkBinary('sumo-gui')
    traci.start([sumoBinary, "-c", "sumo_data/task_map_1114.sumocfg", "--tripinfo-output", "tripinfo.xml","--vehroute-output", "vehroutes.xml",
                "--quit-on-end"])  # ,"--start"

    SetMaxspeed = list(
        map(lambda x: traci.edge.setMaxSpeed(x, 3), list(traci.edge.getIDList())))  #####将所有道路的最大速度设为6m/s

    # 实例化Environment类
    Env=Environment()
    ParkingInformation=Env.get_ParkingInformation()

    # 实例化分配策略
    AL=Allocation(ParkingInformation)

    # 实例化全感知模块
    PER=Com_perception(ParkingInformation)

    # “车辆--停车区域”关系表
    VehiclePark=pd.DataFrame(columns=['Veh', 'park'])
    cross_lane_handler = CrossLaneParking()
    # 存储入口点位置
    entry_positions = {
        1: traci.lane.getShape("-37_0")[0],  # 第一个入口
        2: traci.lane.getShape("25_0")[0],   # 第二个入口
        3: traci.lane.getShape("24_0")[0]    # 第三个入口
    }
    
    entry_edges = {
        1: "-37",
        2: "25",
        3: "24"
    }
    # 参数设定
    R = 4      # 转弯半径 (m)
    theta = np.pi/2  # 转弯角度 (90°)
    L = 1.0      # 直线段长度 (m)
    delta_s = 0.1    # 离散间隔 (m)
    # 生成轨迹点序列
    trajectory = generate_double_arc_trajectory(R, theta, L, delta_s)
    for step in range(10000):
        try:
            if step % 20 == 0:
  
                # 处理第一个入口的车辆
                vehid = f"v_1_{step}"
                
                # 使用入口位置寻找最近车位
                entry_pos = entry_positions[1]
                parkingArea_1 = AL.get_nearest_parking(entry_pos)
                
                if parkingArea_1:
                    
                    # 添加车辆和初始路径
                    tripid = f"trip_1_{step}"
                    origin = entry_edges[1]
                    lane_id = traci.parkingarea.getLaneID(parkingArea_1)
                    destination = traci.lane.getEdgeID(lane_id)
                    
                    traci.route.add(tripid, [origin, destination])
                    traci.vehicle.add(vehid, tripid, typeID="myType")
                    
                    # 记录到VehiclePark
                    VehiclePark = pd.concat([
                        VehiclePark,
                        pd.DataFrame([{'Veh': vehid, 'park': parkingArea_1}])
                    ], ignore_index=True)
                    
                    # 更新全感知模块
                    PER.Add_update(parkingArea_1)
                    # 预约
                    out = change_parkingspace_color(parkingArea_1, (0, 255, 20, 255))
                    try:
                        success = custom_parking(vehid, parkingArea_1, trajectory)
                        if success:
                            print(f"Vehicle {vehid} parked successfully")
                        else:
                            print(f"Parking failed for vehicle {vehid}")
                    except Exception as e:
                        print(f"Error during parking: {str(e)}")
                    remove_parkingspace_color(parkingArea_1)
                    out = change_parkingspace_color(parkingArea_1, (255, 255, 0, 255))
                    if out:
                        print(f"draw {parkingArea_1} parkingspace success!!")
                    else:
                        print(f"draw {parkingArea_1} parkingspace fail!!")

            PER.Leave_update(VehiclePark)
            traci.simulationStep()

            
        except Exception as e:
            print(f"Error: {e}")
            continue

if __name__ == '__main__' :
    main()

this is my test code

[zsdfaker]

After using traci.vehicle.setParkingAreaStop, even if the vehicle is already in the parking space, it will still go back onto the road and re-execute the preset parking maneuvers. What should I do to make the vehicle park using my custom-defined parking maneuvers?