From bf697cb021362ce94a6bd5188b3579de336a61f7 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Thu, 18 Jan 2024 17:55:01 +0100 Subject: [PATCH 01/19] feat: integrate trajectory planner in docker compose --- build/docker-compose.yml | 2 +- build/docker/agent/Dockerfile | 8 +++ code/agent/config/rviz_config.rviz | 2 +- code/planning/src/local_planner/test_traj.py | 65 ++++++++++++++++++++ 4 files changed, 75 insertions(+), 2 deletions(-) create mode 100644 code/planning/src/local_planner/test_traj.py diff --git a/build/docker-compose.yml b/build/docker-compose.yml index 70123763..7c490c30 100644 --- a/build/docker-compose.yml +++ b/build/docker-compose.yml @@ -62,7 +62,7 @@ services: shm_size: 2gb #command: bash -c "sleep 10 && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/opt/leaderboard/leaderboard/autoagents/npc_agent.py --host=carla-simulator --track=SENSORS" #command: bash -c "sleep 10 && roslaunch agent/launch/dev.launch" - command: bash -c "sleep 10 && sudo chown -R carla:carla ../code/ && sudo chmod -R a+w ../code/ && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/workspace/code/agent/src/agent/agent.py --host=carla-simulator --track=MAP" + command: bash -c "sleep 10 && sudo chown -R carla:carla ../code/ && sudo chmod -R a+w ../code/ && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/workspace/code/agent/src/agent/agent.py --host=paf23-carla-simulator-1 --track=MAP" logging: driver: "local" diff --git a/build/docker/agent/Dockerfile b/build/docker/agent/Dockerfile index 5a9c37c0..378da43f 100644 --- a/build/docker/agent/Dockerfile +++ b/build/docker/agent/Dockerfile @@ -162,14 +162,22 @@ RUN source ~/.bashrc && pip install -r /workspace/requirements.txt # Add agent code COPY --chown=$USERNAME:$USERNAME ./code /workspace/code/ +RUN cd /workspace/code/planning/src/local_planner +RUN git clone https://github.com/erdos-project/frenet_optimal_trajectory_planner.git +RUN cd frenet_optimal_trajectory_planner && source ./build.sh # Link code into catkin workspace RUN ln -s /workspace/code /catkin_ws/src +# + +# RUN sudo ./build.sh # re-make the catkin workspace RUN source /opt/ros/noetic/setup.bash && catkin_make ADD ./build/docker/agent/entrypoint.sh /entrypoint.sh + + # set the default working directory to the code WORKDIR /workspace/code diff --git a/code/agent/config/rviz_config.rviz b/code/agent/config/rviz_config.rviz index 5c10eca9..f27c44d8 100644 --- a/code/agent/config/rviz_config.rviz +++ b/code/agent/config/rviz_config.rviz @@ -166,7 +166,7 @@ Visualization Manager: Offset: X: 0 Y: 0 - Z: 39 + Z: 703 Pose Color: 255; 85; 255 Pose Style: None Queue Size: 10 diff --git a/code/planning/src/local_planner/test_traj.py b/code/planning/src/local_planner/test_traj.py new file mode 100644 index 00000000..bd59a273 --- /dev/null +++ b/code/planning/src/local_planner/test_traj.py @@ -0,0 +1,65 @@ +# import sys +import numpy as np +import matplotlib.pyplot as plt +# sys.path.append('/erdos') + +from FrenetOptimalTrajectory.fot_wrapper import run_fot + +wp = wp = np.r_[[np.full((50), 983.5889666959667)], + [np.linspace(5370.016106881272, 5399.016106881272, 50)]].T +initial_conditions = { + 'ps': 0, + 'target_speed': 6, + 'pos': np.array([983.5807552562393, 5370.014637890163]), + 'vel': np.array([5, 1]), + 'wp': wp, + 'obs': np.array([[983.568124548765, 5386.0219828457075, + 983.628124548765, 5386.0219828457075]]) +} + +hyperparameters = { + "max_speed": 25.0, + "max_accel": 15.0, + "max_curvature": 15.0, + "max_road_width_l": 3.0, + "max_road_width_r": 0, + "d_road_w": 0.5, + "dt": 0.2, + "maxt": 20.0, + "mint": 6.0, + "d_t_s": 0.5, + "n_s_sample": 2.0, + "obstacle_clearance": 2, + "kd": 1.0, + "kv": 0.1, + "ka": 0.1, + "kj": 0.1, + "kt": 0.1, + "ko": 0.1, + "klat": 1.0, + "klon": 1.0, + "num_threads": 0, # set 0 to avoid using threaded algorithm +} + +result_x, result_y, speeds, ix, iy, iyaw, d, s, speeds_x, \ + speeds_y, misc, costs, success = run_fot(initial_conditions, + hyperparameters) + +if success: + print("Success!") + print("result_x: ", result_x) + print("result_y: ", result_y) + fig, ax = plt.subplots(1, 2) + + ax[0].scatter(wp[:, 0], wp[:, 1], label="original") + ax[0].scatter([983.568124548765, 983.628124548765], + [5386.0219828457075, 5386.0219828457075], label="object") + ax[0].set_xticks([983.518124548765, 983.598124548765]) + ax[1].scatter(result_x, result_y, label="frenet") + ax[1].scatter([983.568124548765, 983.628124548765], + [5386.0219828457075, 5386.0219828457075], label="object") + ax[1].set_xticks([983.518124548765, 983.598124548765]) + plt.legend() + plt.show() +else: + print("Failure!") From 2de1325b0a45a62c779dfc173f0a0b26ba5b477b Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Fri, 19 Jan 2024 16:07:37 +0100 Subject: [PATCH 02/19] feat: intallation of Frenet tool in docker file --- build/docker-compose.yml | 4 +- build/docker/agent/Dockerfile | 21 +++- .../src/global_plan_distance_publisher.py | 2 +- code/planning/launch/planning.launch | 4 +- .../src/global_planner/global_planner.py | 4 +- .../src/local_planner/motion_planning.py | 119 +++++++++++++++++- code/planning/src/local_planner/test_traj.py | 31 ++--- 7 files changed, 157 insertions(+), 28 deletions(-) diff --git a/build/docker-compose.yml b/build/docker-compose.yml index 7c490c30..10431837 100644 --- a/build/docker-compose.yml +++ b/build/docker-compose.yml @@ -61,8 +61,8 @@ services: tty: true shm_size: 2gb #command: bash -c "sleep 10 && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/opt/leaderboard/leaderboard/autoagents/npc_agent.py --host=carla-simulator --track=SENSORS" - #command: bash -c "sleep 10 && roslaunch agent/launch/dev.launch" - command: bash -c "sleep 10 && sudo chown -R carla:carla ../code/ && sudo chmod -R a+w ../code/ && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/workspace/code/agent/src/agent/agent.py --host=paf23-carla-simulator-1 --track=MAP" + command: bash -c "sleep 10 && roslaunch agent/launch/dev.launch" + # command: bash -c "sleep 10 && sudo chown -R carla:carla ../code/ && sudo chmod -R a+w ../code/ && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/workspace/code/agent/src/agent/agent.py --host=paf23-carla-simulator-1 --track=MAP" logging: driver: "local" diff --git a/build/docker/agent/Dockerfile b/build/docker/agent/Dockerfile index 378da43f..acf7edf7 100644 --- a/build/docker/agent/Dockerfile +++ b/build/docker/agent/Dockerfile @@ -162,15 +162,24 @@ RUN source ~/.bashrc && pip install -r /workspace/requirements.txt # Add agent code COPY --chown=$USERNAME:$USERNAME ./code /workspace/code/ -RUN cd /workspace/code/planning/src/local_planner -RUN git clone https://github.com/erdos-project/frenet_optimal_trajectory_planner.git -RUN cd frenet_optimal_trajectory_planner && source ./build.sh +# Install Frenet Optimal Trajectory Planner +RUN sudo mkdir /erdos +RUN sudo mkdir /erdos/dependencies +RUN sudo mkdir /erdos/dependencies/frenet_optimal_trajectory_planner +# Needed to resolve dependencies correctly inside freent_optimal_trajectory_planner +ENV PYLOT_HOME=/erdos + +ENV FREENET_HOME=/erdos/dependencies/frenet_optimal_trajectory_planner +RUN sudo chown $USERNAME:$USERNAME $PYLOT_HOME +RUN sudo chown $USERNAME:$USERNAME $FREENET_HOME + +RUN git clone https://github.com/erdos-project/frenet_optimal_trajectory_planner.git $FREENET_HOME +RUN cd $FREENET_HOME && source ./build.sh + +ENV PYTHONPATH=$PYTHONPATH:/erdos/dependencies # Link code into catkin workspace RUN ln -s /workspace/code /catkin_ws/src -# - -# RUN sudo ./build.sh # re-make the catkin workspace RUN source /opt/ros/noetic/setup.bash && catkin_make diff --git a/code/perception/src/global_plan_distance_publisher.py b/code/perception/src/global_plan_distance_publisher.py index 40da367c..4e4cf37b 100755 --- a/code/perception/src/global_plan_distance_publisher.py +++ b/code/perception/src/global_plan_distance_publisher.py @@ -47,7 +47,7 @@ def __init__(self): # Change comment for dev_launch self.global_plan_subscriber = self.new_subscription( CarlaRoute, - "/carla/" + self.role_name + "/global_plan", + "/paf/" + self.role_name + "/global_plan", self.update_global_route, qos_profile=1) # self.global_plan_subscriber = self.new_subscription( diff --git a/code/planning/launch/planning.launch b/code/planning/launch/planning.launch index 81e3ef7d..149cd4ba 100644 --- a/code/planning/launch/planning.launch +++ b/code/planning/launch/planning.launch @@ -16,13 +16,13 @@ - + diff --git a/code/planning/src/global_planner/global_planner.py b/code/planning/src/global_planner/global_planner.py index e39cb4be..14c161ba 100755 --- a/code/planning/src/global_planner/global_planner.py +++ b/code/planning/src/global_planner/global_planner.py @@ -57,7 +57,7 @@ def __init__(self): # uncomment /paf/hero/global_plan and comment /carla/... for dev_launch self.global_plan_sub = self.new_subscription( msg_type=CarlaRoute, - topic='/carla/' + self.role_name + '/global_plan', + topic='/paf/' + self.role_name + '/global_plan', callback=self.global_route_callback, qos_profile=10) # self.global_plan_sub = self.new_subscription( @@ -84,7 +84,7 @@ def __init__(self): self.logdebug('PrePlanner-Node started') # uncomment for self.dev_load_world_info() for dev_launch - # self.dev_load_world_info() + self.dev_load_world_info() def global_route_callback(self, data: CarlaRoute) -> None: """ diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index 92949cad..d4ab5f2c 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -2,12 +2,21 @@ # import rospy # import tf.transformations import ros_compatibility as roscomp +import rospy +import tf.transformations + from ros_compatibility.node import CompatibleNode from rospy import Publisher, Subscriber from std_msgs.msg import String, Float32 +from nav_msgs.msg import Path +from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion + + import numpy as np # from behavior_agent.msg import BehaviorSpeed +from frenet_optimal_trajectory_planner.FrenetOptimalTrajectory.fot_wrapper \ + import run_fot from perception.msg import Waypoint, LaneChange import behavior_speed as bs @@ -40,8 +49,17 @@ def __init__(self): self.__acc_speed = 0.0 self.__stopline = None # (Distance, isStopline) self.__change_point = None # (Distance, isLaneChange, roadOption) - # Subscriber + self.trajectory_sub = self.new_subscription( + Path, + f"/paf/{self.role_name}/trajectory", + self.__set_trajectory, + qos_profile=1) + self.current_pos_sub = self.new_subscription( + PoseStamped, + f"/paf/{self.role_name}/current_pos", + self.__set_current_pos, + qos_profile=1) self.curr_behavior_sub: Subscriber = self.new_subscription( String, f"/paf/{self.role_name}/curr_behavior", @@ -67,12 +85,111 @@ def __init__(self): qos_profile=1) # Publisher + self.traj_pub: Publisher = self.new_publisher( + Path, + f"/paf/{self.role_name}/trajectory", + qos_profile=1) self.velocity_pub: Publisher = self.new_publisher( Float32, f"/paf/{self.role_name}/target_velocity", qos_profile=1) self.logdebug("MotionPlanning started") + self.published = False + self.current_pos = None + + def __set_current_pos(self, data: PoseStamped): + """set current position + + Args: + data (PoseStamped): current position + """ + self.current_pos = np.array([data.pose.position.x, + data.pose.position.y]) + + def __set_trajectory(self, data: Path): + """get current trajectory global planning + + Args: + data (Path): Trajectory waypoints + """ + if len(data.poses) and not self.published > 0: + self.published = True + np_array = np.array(data.poses) + selection = np_array[5:17] + waypoints = self.convert_pose_to_array(selection) + self.logerr(waypoints) + obs = np.array([[waypoints[5][0]-0.5, waypoints[5][1], waypoints[5][0]+0.5, waypoints[5][1]-2]]) + initial_conditions = { + 'ps': 0, + 'target_speed': self.__acc_speed, + 'pos': np.array([983.5807552562393, 5370.014637890163]), + 'vel': np.array([5, 1]), + 'wp': waypoints, + 'obs': obs + } + hyperparameters = { + "max_speed": 25.0, + "max_accel": 15.0, + "max_curvature": 15.0, + "max_road_width_l": 3.0, + "max_road_width_r": 0.5, + "d_road_w": 0.5, + "dt": 0.2, + "maxt": 20.0, + "mint": 6.0, + "d_t_s": 0.5, + "n_s_sample": 2.0, + "obstacle_clearance": 0.1, + "kd": 1.0, + "kv": 0.1, + "ka": 0.1, + "kj": 0.1, + "kt": 0.1, + "ko": 0.1, + "klat": 1.0, + "klon": 1.0, + "num_threads": 0, # set 0 to avoid using threaded algorithm + } + result_x, result_y, speeds, ix, iy, iyaw, d, s, speeds_x, \ + speeds_y, misc, costs, success = run_fot(initial_conditions, + hyperparameters) + if success: + print("Success!") + print("result_x: ", result_x) + print("result_y: ", result_y) + result = [] + for i in range(len(result_x)): + position = Point(result_x[i], result_y[i], 703) + quaternion = tf.transformations.quaternion_from_euler(0, + 0, + iyaw[i]) + orientation = Quaternion(x=quaternion[0], y=quaternion[1], + z=quaternion[2], w=quaternion[3]) + pose = Pose(position, orientation) + pos = PoseStamped() + pos.header.stamp = rospy.Time.now() + pos.header.frame_id = "global" + pos.pose = pose + result.append(PoseStamped) + path = Path() + path.header.stamp = rospy.Time.now() + path.path_backup.header.frame_id = "global" + path.poses = list(np_array[:5]) + result + list(np_array[17:]) + self.traj_pub.publish(path) + + def convert_pose_to_array(self, poses: np.array): + """convert pose array to numpy array + + Args: + poses (np.array): pose array + + Returns: + np.array: numpy array + """ + def get_x_y(pose): + return np.array([pose.pose.position.x, pose.pose.position.y]) + return np.vectorize(get_x_y)(poses) def update_target_speed(self, acc_speed, behavior): be_speed = self.get_speed_by_behavior(behavior) diff --git a/code/planning/src/local_planner/test_traj.py b/code/planning/src/local_planner/test_traj.py index bd59a273..05a00c73 100644 --- a/code/planning/src/local_planner/test_traj.py +++ b/code/planning/src/local_planner/test_traj.py @@ -1,26 +1,28 @@ -# import sys import numpy as np + +from frenet_optimal_trajectory_planner.FrenetOptimalTrajectory.fot_wrapper \ + import run_fot + import matplotlib.pyplot as plt -# sys.path.append('/erdos') -from FrenetOptimalTrajectory.fot_wrapper import run_fot -wp = wp = np.r_[[np.full((50), 983.5889666959667)], - [np.linspace(5370.016106881272, 5399.016106881272, 50)]].T +wp = np.r_[[np.full((50), 983.5889666959667)], + [np.linspace(5370.016106881272, 5399.016106881272, 50)]].T +obs = np.array([[983.568124548765, 5384.0219828457075, + 983.628124548765, 5386.0219828457075]]) initial_conditions = { 'ps': 0, 'target_speed': 6, 'pos': np.array([983.5807552562393, 5370.014637890163]), 'vel': np.array([5, 1]), 'wp': wp, - 'obs': np.array([[983.568124548765, 5386.0219828457075, - 983.628124548765, 5386.0219828457075]]) + 'obs': obs } hyperparameters = { "max_speed": 25.0, "max_accel": 15.0, - "max_curvature": 15.0, + "max_curvature": 20.0, "max_road_width_l": 3.0, "max_road_width_r": 0, "d_road_w": 0.5, @@ -49,16 +51,17 @@ print("Success!") print("result_x: ", result_x) print("result_y: ", result_y) + print("yaw!", iyaw) fig, ax = plt.subplots(1, 2) ax[0].scatter(wp[:, 0], wp[:, 1], label="original") - ax[0].scatter([983.568124548765, 983.628124548765], - [5386.0219828457075, 5386.0219828457075], label="object") - ax[0].set_xticks([983.518124548765, 983.598124548765]) + ax[0].scatter([obs[0, 0], obs[0, 2]], + [obs[0, 1], obs[0, 3]], label="object") + ax[0].set_xticks([obs[0, 0], obs[0, 2]]) ax[1].scatter(result_x, result_y, label="frenet") - ax[1].scatter([983.568124548765, 983.628124548765], - [5386.0219828457075, 5386.0219828457075], label="object") - ax[1].set_xticks([983.518124548765, 983.598124548765]) + ax[1].scatter([obs[0, 0], obs[0, 2]], + [obs[0, 1], obs[0, 3]], label="object") + ax[1].set_xticks([obs[0, 0], obs[0, 2]]) plt.legend() plt.show() else: From 5c910859d2f55c7af02ce3aca72ecb6827e47381 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Thu, 18 Jan 2024 17:55:01 +0100 Subject: [PATCH 03/19] feat: integrate trajectory planner in docker compose --- build/docker-compose.yml | 5 +- build/docker/agent/Dockerfile | 8 +++ code/agent/config/rviz_config.rviz | 2 +- code/planning/src/local_planner/test_traj.py | 65 ++++++++++++++++++++ 4 files changed, 77 insertions(+), 3 deletions(-) create mode 100644 code/planning/src/local_planner/test_traj.py diff --git a/build/docker-compose.yml b/build/docker-compose.yml index 2e9b1d57..7c490c30 100644 --- a/build/docker-compose.yml +++ b/build/docker-compose.yml @@ -61,8 +61,9 @@ services: tty: true shm_size: 2gb #command: bash -c "sleep 10 && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/opt/leaderboard/leaderboard/autoagents/npc_agent.py --host=carla-simulator --track=SENSORS" - # command: bash -c "sleep 10 && roslaunch agent/launch/dev.launch" - command: bash -c "sleep 10 && sudo chown -R carla:carla ../code/ && sudo chmod -R a+w ../code/ && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/workspace/code/agent/src/agent/agent.py --host=carla-simulator --track=MAP" + #command: bash -c "sleep 10 && roslaunch agent/launch/dev.launch" + command: bash -c "sleep 10 && sudo chown -R carla:carla ../code/ && sudo chmod -R a+w ../code/ && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/workspace/code/agent/src/agent/agent.py --host=paf23-carla-simulator-1 --track=MAP" + logging: driver: "local" environment: diff --git a/build/docker/agent/Dockerfile b/build/docker/agent/Dockerfile index 5a9c37c0..378da43f 100644 --- a/build/docker/agent/Dockerfile +++ b/build/docker/agent/Dockerfile @@ -162,14 +162,22 @@ RUN source ~/.bashrc && pip install -r /workspace/requirements.txt # Add agent code COPY --chown=$USERNAME:$USERNAME ./code /workspace/code/ +RUN cd /workspace/code/planning/src/local_planner +RUN git clone https://github.com/erdos-project/frenet_optimal_trajectory_planner.git +RUN cd frenet_optimal_trajectory_planner && source ./build.sh # Link code into catkin workspace RUN ln -s /workspace/code /catkin_ws/src +# + +# RUN sudo ./build.sh # re-make the catkin workspace RUN source /opt/ros/noetic/setup.bash && catkin_make ADD ./build/docker/agent/entrypoint.sh /entrypoint.sh + + # set the default working directory to the code WORKDIR /workspace/code diff --git a/code/agent/config/rviz_config.rviz b/code/agent/config/rviz_config.rviz index 5c10eca9..f27c44d8 100644 --- a/code/agent/config/rviz_config.rviz +++ b/code/agent/config/rviz_config.rviz @@ -166,7 +166,7 @@ Visualization Manager: Offset: X: 0 Y: 0 - Z: 39 + Z: 703 Pose Color: 255; 85; 255 Pose Style: None Queue Size: 10 diff --git a/code/planning/src/local_planner/test_traj.py b/code/planning/src/local_planner/test_traj.py new file mode 100644 index 00000000..bd59a273 --- /dev/null +++ b/code/planning/src/local_planner/test_traj.py @@ -0,0 +1,65 @@ +# import sys +import numpy as np +import matplotlib.pyplot as plt +# sys.path.append('/erdos') + +from FrenetOptimalTrajectory.fot_wrapper import run_fot + +wp = wp = np.r_[[np.full((50), 983.5889666959667)], + [np.linspace(5370.016106881272, 5399.016106881272, 50)]].T +initial_conditions = { + 'ps': 0, + 'target_speed': 6, + 'pos': np.array([983.5807552562393, 5370.014637890163]), + 'vel': np.array([5, 1]), + 'wp': wp, + 'obs': np.array([[983.568124548765, 5386.0219828457075, + 983.628124548765, 5386.0219828457075]]) +} + +hyperparameters = { + "max_speed": 25.0, + "max_accel": 15.0, + "max_curvature": 15.0, + "max_road_width_l": 3.0, + "max_road_width_r": 0, + "d_road_w": 0.5, + "dt": 0.2, + "maxt": 20.0, + "mint": 6.0, + "d_t_s": 0.5, + "n_s_sample": 2.0, + "obstacle_clearance": 2, + "kd": 1.0, + "kv": 0.1, + "ka": 0.1, + "kj": 0.1, + "kt": 0.1, + "ko": 0.1, + "klat": 1.0, + "klon": 1.0, + "num_threads": 0, # set 0 to avoid using threaded algorithm +} + +result_x, result_y, speeds, ix, iy, iyaw, d, s, speeds_x, \ + speeds_y, misc, costs, success = run_fot(initial_conditions, + hyperparameters) + +if success: + print("Success!") + print("result_x: ", result_x) + print("result_y: ", result_y) + fig, ax = plt.subplots(1, 2) + + ax[0].scatter(wp[:, 0], wp[:, 1], label="original") + ax[0].scatter([983.568124548765, 983.628124548765], + [5386.0219828457075, 5386.0219828457075], label="object") + ax[0].set_xticks([983.518124548765, 983.598124548765]) + ax[1].scatter(result_x, result_y, label="frenet") + ax[1].scatter([983.568124548765, 983.628124548765], + [5386.0219828457075, 5386.0219828457075], label="object") + ax[1].set_xticks([983.518124548765, 983.598124548765]) + plt.legend() + plt.show() +else: + print("Failure!") From 314738dad32ae8532a9db97750e0881ec9cad7f2 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Fri, 19 Jan 2024 16:07:37 +0100 Subject: [PATCH 04/19] feat: intallation of Frenet tool in docker file --- build/docker-compose.yml | 4 +- build/docker/agent/Dockerfile | 21 +++- .../src/global_plan_distance_publisher.py | 2 +- code/planning/launch/planning.launch | 4 +- .../src/global_planner/global_planner.py | 4 +- .../src/local_planner/motion_planning.py | 119 +++++++++++++++++- code/planning/src/local_planner/test_traj.py | 31 ++--- 7 files changed, 157 insertions(+), 28 deletions(-) diff --git a/build/docker-compose.yml b/build/docker-compose.yml index 7c490c30..10431837 100644 --- a/build/docker-compose.yml +++ b/build/docker-compose.yml @@ -61,8 +61,8 @@ services: tty: true shm_size: 2gb #command: bash -c "sleep 10 && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/opt/leaderboard/leaderboard/autoagents/npc_agent.py --host=carla-simulator --track=SENSORS" - #command: bash -c "sleep 10 && roslaunch agent/launch/dev.launch" - command: bash -c "sleep 10 && sudo chown -R carla:carla ../code/ && sudo chmod -R a+w ../code/ && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/workspace/code/agent/src/agent/agent.py --host=paf23-carla-simulator-1 --track=MAP" + command: bash -c "sleep 10 && roslaunch agent/launch/dev.launch" + # command: bash -c "sleep 10 && sudo chown -R carla:carla ../code/ && sudo chmod -R a+w ../code/ && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/workspace/code/agent/src/agent/agent.py --host=paf23-carla-simulator-1 --track=MAP" logging: driver: "local" diff --git a/build/docker/agent/Dockerfile b/build/docker/agent/Dockerfile index 378da43f..acf7edf7 100644 --- a/build/docker/agent/Dockerfile +++ b/build/docker/agent/Dockerfile @@ -162,15 +162,24 @@ RUN source ~/.bashrc && pip install -r /workspace/requirements.txt # Add agent code COPY --chown=$USERNAME:$USERNAME ./code /workspace/code/ -RUN cd /workspace/code/planning/src/local_planner -RUN git clone https://github.com/erdos-project/frenet_optimal_trajectory_planner.git -RUN cd frenet_optimal_trajectory_planner && source ./build.sh +# Install Frenet Optimal Trajectory Planner +RUN sudo mkdir /erdos +RUN sudo mkdir /erdos/dependencies +RUN sudo mkdir /erdos/dependencies/frenet_optimal_trajectory_planner +# Needed to resolve dependencies correctly inside freent_optimal_trajectory_planner +ENV PYLOT_HOME=/erdos + +ENV FREENET_HOME=/erdos/dependencies/frenet_optimal_trajectory_planner +RUN sudo chown $USERNAME:$USERNAME $PYLOT_HOME +RUN sudo chown $USERNAME:$USERNAME $FREENET_HOME + +RUN git clone https://github.com/erdos-project/frenet_optimal_trajectory_planner.git $FREENET_HOME +RUN cd $FREENET_HOME && source ./build.sh + +ENV PYTHONPATH=$PYTHONPATH:/erdos/dependencies # Link code into catkin workspace RUN ln -s /workspace/code /catkin_ws/src -# - -# RUN sudo ./build.sh # re-make the catkin workspace RUN source /opt/ros/noetic/setup.bash && catkin_make diff --git a/code/perception/src/global_plan_distance_publisher.py b/code/perception/src/global_plan_distance_publisher.py index 40da367c..4e4cf37b 100755 --- a/code/perception/src/global_plan_distance_publisher.py +++ b/code/perception/src/global_plan_distance_publisher.py @@ -47,7 +47,7 @@ def __init__(self): # Change comment for dev_launch self.global_plan_subscriber = self.new_subscription( CarlaRoute, - "/carla/" + self.role_name + "/global_plan", + "/paf/" + self.role_name + "/global_plan", self.update_global_route, qos_profile=1) # self.global_plan_subscriber = self.new_subscription( diff --git a/code/planning/launch/planning.launch b/code/planning/launch/planning.launch index 9dee1352..72c1f88c 100644 --- a/code/planning/launch/planning.launch +++ b/code/planning/launch/planning.launch @@ -16,13 +16,13 @@ - + diff --git a/code/planning/src/global_planner/global_planner.py b/code/planning/src/global_planner/global_planner.py index e39cb4be..14c161ba 100755 --- a/code/planning/src/global_planner/global_planner.py +++ b/code/planning/src/global_planner/global_planner.py @@ -57,7 +57,7 @@ def __init__(self): # uncomment /paf/hero/global_plan and comment /carla/... for dev_launch self.global_plan_sub = self.new_subscription( msg_type=CarlaRoute, - topic='/carla/' + self.role_name + '/global_plan', + topic='/paf/' + self.role_name + '/global_plan', callback=self.global_route_callback, qos_profile=10) # self.global_plan_sub = self.new_subscription( @@ -84,7 +84,7 @@ def __init__(self): self.logdebug('PrePlanner-Node started') # uncomment for self.dev_load_world_info() for dev_launch - # self.dev_load_world_info() + self.dev_load_world_info() def global_route_callback(self, data: CarlaRoute) -> None: """ diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index 952687ab..cb21adb9 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -2,12 +2,21 @@ # import rospy # import tf.transformations import ros_compatibility as roscomp +import rospy +import tf.transformations + from ros_compatibility.node import CompatibleNode from rospy import Publisher, Subscriber from std_msgs.msg import String, Float32, Bool +from nav_msgs.msg import Path +from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion + + import numpy as np # from behavior_agent.msg import BehaviorSpeed +from frenet_optimal_trajectory_planner.FrenetOptimalTrajectory.fot_wrapper \ + import run_fot from perception.msg import Waypoint, LaneChange import behavior_speed as bs @@ -40,8 +49,17 @@ def __init__(self): self.__acc_speed = 0.0 self.__stopline = None # (Distance, isStopline) self.__change_point = None # (Distance, isLaneChange, roadOption) - # Subscriber + self.trajectory_sub = self.new_subscription( + Path, + f"/paf/{self.role_name}/trajectory", + self.__set_trajectory, + qos_profile=1) + self.current_pos_sub = self.new_subscription( + PoseStamped, + f"/paf/{self.role_name}/current_pos", + self.__set_current_pos, + qos_profile=1) self.curr_behavior_sub: Subscriber = self.new_subscription( String, f"/paf/{self.role_name}/curr_behavior", @@ -71,6 +89,10 @@ def __init__(self): qos_profile=1) # Publisher + self.traj_pub: Publisher = self.new_publisher( + Path, + f"/paf/{self.role_name}/trajectory", + qos_profile=1) self.velocity_pub: Publisher = self.new_publisher( Float32, f"/paf/{self.role_name}/target_velocity", @@ -83,6 +105,101 @@ def __init__(self): qos_profile=1) self.logdebug("MotionPlanning started") + self.published = False + self.current_pos = None + + def __set_current_pos(self, data: PoseStamped): + """set current position + + Args: + data (PoseStamped): current position + """ + self.current_pos = np.array([data.pose.position.x, + data.pose.position.y]) + + def __set_trajectory(self, data: Path): + """get current trajectory global planning + + Args: + data (Path): Trajectory waypoints + """ + if len(data.poses) and not self.published > 0: + self.published = True + np_array = np.array(data.poses) + selection = np_array[5:17] + waypoints = self.convert_pose_to_array(selection) + self.logerr(waypoints) + obs = np.array([[waypoints[5][0]-0.5, waypoints[5][1], waypoints[5][0]+0.5, waypoints[5][1]-2]]) + initial_conditions = { + 'ps': 0, + 'target_speed': self.__acc_speed, + 'pos': np.array([983.5807552562393, 5370.014637890163]), + 'vel': np.array([5, 1]), + 'wp': waypoints, + 'obs': obs + } + hyperparameters = { + "max_speed": 25.0, + "max_accel": 15.0, + "max_curvature": 15.0, + "max_road_width_l": 3.0, + "max_road_width_r": 0.5, + "d_road_w": 0.5, + "dt": 0.2, + "maxt": 20.0, + "mint": 6.0, + "d_t_s": 0.5, + "n_s_sample": 2.0, + "obstacle_clearance": 0.1, + "kd": 1.0, + "kv": 0.1, + "ka": 0.1, + "kj": 0.1, + "kt": 0.1, + "ko": 0.1, + "klat": 1.0, + "klon": 1.0, + "num_threads": 0, # set 0 to avoid using threaded algorithm + } + result_x, result_y, speeds, ix, iy, iyaw, d, s, speeds_x, \ + speeds_y, misc, costs, success = run_fot(initial_conditions, + hyperparameters) + if success: + print("Success!") + print("result_x: ", result_x) + print("result_y: ", result_y) + result = [] + for i in range(len(result_x)): + position = Point(result_x[i], result_y[i], 703) + quaternion = tf.transformations.quaternion_from_euler(0, + 0, + iyaw[i]) + orientation = Quaternion(x=quaternion[0], y=quaternion[1], + z=quaternion[2], w=quaternion[3]) + pose = Pose(position, orientation) + pos = PoseStamped() + pos.header.stamp = rospy.Time.now() + pos.header.frame_id = "global" + pos.pose = pose + result.append(PoseStamped) + path = Path() + path.header.stamp = rospy.Time.now() + path.path_backup.header.frame_id = "global" + path.poses = list(np_array[:5]) + result + list(np_array[17:]) + self.traj_pub.publish(path) + + def convert_pose_to_array(self, poses: np.array): + """convert pose array to numpy array + + Args: + poses (np.array): pose array + + Returns: + np.array: numpy array + """ + def get_x_y(pose): + return np.array([pose.pose.position.x, pose.pose.position.y]) + return np.vectorize(get_x_y)(poses) def __check_emergency(self, data: Bool): """If an emergency stop is needed first check if we are diff --git a/code/planning/src/local_planner/test_traj.py b/code/planning/src/local_planner/test_traj.py index bd59a273..05a00c73 100644 --- a/code/planning/src/local_planner/test_traj.py +++ b/code/planning/src/local_planner/test_traj.py @@ -1,26 +1,28 @@ -# import sys import numpy as np + +from frenet_optimal_trajectory_planner.FrenetOptimalTrajectory.fot_wrapper \ + import run_fot + import matplotlib.pyplot as plt -# sys.path.append('/erdos') -from FrenetOptimalTrajectory.fot_wrapper import run_fot -wp = wp = np.r_[[np.full((50), 983.5889666959667)], - [np.linspace(5370.016106881272, 5399.016106881272, 50)]].T +wp = np.r_[[np.full((50), 983.5889666959667)], + [np.linspace(5370.016106881272, 5399.016106881272, 50)]].T +obs = np.array([[983.568124548765, 5384.0219828457075, + 983.628124548765, 5386.0219828457075]]) initial_conditions = { 'ps': 0, 'target_speed': 6, 'pos': np.array([983.5807552562393, 5370.014637890163]), 'vel': np.array([5, 1]), 'wp': wp, - 'obs': np.array([[983.568124548765, 5386.0219828457075, - 983.628124548765, 5386.0219828457075]]) + 'obs': obs } hyperparameters = { "max_speed": 25.0, "max_accel": 15.0, - "max_curvature": 15.0, + "max_curvature": 20.0, "max_road_width_l": 3.0, "max_road_width_r": 0, "d_road_w": 0.5, @@ -49,16 +51,17 @@ print("Success!") print("result_x: ", result_x) print("result_y: ", result_y) + print("yaw!", iyaw) fig, ax = plt.subplots(1, 2) ax[0].scatter(wp[:, 0], wp[:, 1], label="original") - ax[0].scatter([983.568124548765, 983.628124548765], - [5386.0219828457075, 5386.0219828457075], label="object") - ax[0].set_xticks([983.518124548765, 983.598124548765]) + ax[0].scatter([obs[0, 0], obs[0, 2]], + [obs[0, 1], obs[0, 3]], label="object") + ax[0].set_xticks([obs[0, 0], obs[0, 2]]) ax[1].scatter(result_x, result_y, label="frenet") - ax[1].scatter([983.568124548765, 983.628124548765], - [5386.0219828457075, 5386.0219828457075], label="object") - ax[1].set_xticks([983.518124548765, 983.598124548765]) + ax[1].scatter([obs[0, 0], obs[0, 2]], + [obs[0, 1], obs[0, 3]], label="object") + ax[1].set_xticks([obs[0, 0], obs[0, 2]]) plt.legend() plt.show() else: From 933a3ac4a8a5430763ccea59e248f5a005601da3 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Fri, 19 Jan 2024 16:53:33 +0100 Subject: [PATCH 05/19] fix: rivz and behavior tree --- build/docker-compose.yml | 4 +- code/agent/config/rviz_config.rviz | 4 +- .../src/global_plan_distance_publisher.py | 2 +- code/planning/__init__.py | 1 + code/planning/launch/planning.launch | 4 +- .../src/behavior_agent/behaviours/__init__.py | 3 + .../src/global_planner/global_planner.py | 4 +- .../src/local_planner/behavior_speed.py | 69 ------------------- .../src/local_planner/motion_planning.py | 3 +- 9 files changed, 15 insertions(+), 79 deletions(-) delete mode 100755 code/planning/src/local_planner/behavior_speed.py diff --git a/build/docker-compose.yml b/build/docker-compose.yml index 10431837..f3d46bf3 100644 --- a/build/docker-compose.yml +++ b/build/docker-compose.yml @@ -61,8 +61,8 @@ services: tty: true shm_size: 2gb #command: bash -c "sleep 10 && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/opt/leaderboard/leaderboard/autoagents/npc_agent.py --host=carla-simulator --track=SENSORS" - command: bash -c "sleep 10 && roslaunch agent/launch/dev.launch" - # command: bash -c "sleep 10 && sudo chown -R carla:carla ../code/ && sudo chmod -R a+w ../code/ && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/workspace/code/agent/src/agent/agent.py --host=paf23-carla-simulator-1 --track=MAP" + # command: bash -c "sleep 10 && roslaunch agent/launch/dev.launch" + command: bash -c "sleep 10 && sudo chown -R carla:carla ../code/ && sudo chmod -R a+w ../code/ && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/workspace/code/agent/src/agent/agent.py --host=paf23-carla-simulator-1 --track=MAP" logging: driver: "local" diff --git a/code/agent/config/rviz_config.rviz b/code/agent/config/rviz_config.rviz index f27c44d8..42b620d5 100644 --- a/code/agent/config/rviz_config.rviz +++ b/code/agent/config/rviz_config.rviz @@ -65,9 +65,9 @@ Visualization Manager: Visibility: Grid: false Imu: false - Path: false + Path: true PointCloud2: false - Value: false + Value: true Zoom Factor: 1 - Class: rviz/Image Enabled: true diff --git a/code/perception/src/global_plan_distance_publisher.py b/code/perception/src/global_plan_distance_publisher.py index 4e4cf37b..40da367c 100755 --- a/code/perception/src/global_plan_distance_publisher.py +++ b/code/perception/src/global_plan_distance_publisher.py @@ -47,7 +47,7 @@ def __init__(self): # Change comment for dev_launch self.global_plan_subscriber = self.new_subscription( CarlaRoute, - "/paf/" + self.role_name + "/global_plan", + "/carla/" + self.role_name + "/global_plan", self.update_global_route, qos_profile=1) # self.global_plan_subscriber = self.new_subscription( diff --git a/code/planning/__init__.py b/code/planning/__init__.py index e69de29b..191d7b66 100755 --- a/code/planning/__init__.py +++ b/code/planning/__init__.py @@ -0,0 +1 @@ +import behavior_agent \ No newline at end of file diff --git a/code/planning/launch/planning.launch b/code/planning/launch/planning.launch index 72c1f88c..9dee1352 100644 --- a/code/planning/launch/planning.launch +++ b/code/planning/launch/planning.launch @@ -16,13 +16,13 @@ - + diff --git a/code/planning/src/behavior_agent/behaviours/__init__.py b/code/planning/src/behavior_agent/behaviours/__init__.py index e69de29b..93673a12 100755 --- a/code/planning/src/behavior_agent/behaviours/__init__.py +++ b/code/planning/src/behavior_agent/behaviours/__init__.py @@ -0,0 +1,3 @@ +from . import intersection, lane_change, maneuvers, meta, road_features +from . import topics2blackboard, traffic_objects +from . import behavior_speed \ No newline at end of file diff --git a/code/planning/src/global_planner/global_planner.py b/code/planning/src/global_planner/global_planner.py index 14c161ba..e39cb4be 100755 --- a/code/planning/src/global_planner/global_planner.py +++ b/code/planning/src/global_planner/global_planner.py @@ -57,7 +57,7 @@ def __init__(self): # uncomment /paf/hero/global_plan and comment /carla/... for dev_launch self.global_plan_sub = self.new_subscription( msg_type=CarlaRoute, - topic='/paf/' + self.role_name + '/global_plan', + topic='/carla/' + self.role_name + '/global_plan', callback=self.global_route_callback, qos_profile=10) # self.global_plan_sub = self.new_subscription( @@ -84,7 +84,7 @@ def __init__(self): self.logdebug('PrePlanner-Node started') # uncomment for self.dev_load_world_info() for dev_launch - self.dev_load_world_info() + # self.dev_load_world_info() def global_route_callback(self, data: CarlaRoute) -> None: """ diff --git a/code/planning/src/local_planner/behavior_speed.py b/code/planning/src/local_planner/behavior_speed.py deleted file mode 100755 index e67bdad1..00000000 --- a/code/planning/src/local_planner/behavior_speed.py +++ /dev/null @@ -1,69 +0,0 @@ - -from collections import namedtuple - - -def convert_to_ms(speed): - return speed / 3.6 - - -Behavior = namedtuple("Behavior", ("name", "speed")) - -# Changed target_speed_pub to curr_behavior_pub - -# Leave Parking space - -parking = Behavior("parking", convert_to_ms(30.0)) -# Intersection - Behaviors - -# Approach - -int_app_init = Behavior("int_app_init", convert_to_ms(30.0)) - -# No Traffic Light or Sign -> stop dynamically at Stopline -int_app_no_sign = Behavior("int_app_no_sign", -2) - -int_app_green = Behavior("int_app_green", convert_to_ms(30.0)) - -# Wait - -int_wait = Behavior("int_wait", 0) - -# Enter - -int_enter_no_light = Behavior("int_enter_no_light", convert_to_ms(50.0)) - -int_enter_empty_str = Behavior("int_enter_empty_string", convert_to_ms(18.0)) - -int_enter_light = Behavior("int_enter_light", convert_to_ms(50.0)) - -# Exit - -int_exit = Behavior("int_exit", -1) # Get SpeedLimit dynamically - - -# Lane Change - -# Approach - -lc_app_init = Behavior("lc_app_blocked", convert_to_ms(30.0)) - - -# TODO: Find out purpose of v_stop in lane_change (lines: 105 - 128) -lc_app_blocked = Behavior("lc_app_blocked", 0.5) - -# Wait - -# Has a publisher but doesnt publish anything ?? - -# Enter - -lc_enter_init = Behavior("lc_enter_init", convert_to_ms(20.0)) - -# Exit - -lc_exit = Behavior("lc_exit", -1) # Get SpeedLimit dynamically - - -# Cruise - -cruise = Behavior("Cruise", -1) diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index cb21adb9..e9cd1e7f 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -18,7 +18,8 @@ from frenet_optimal_trajectory_planner.FrenetOptimalTrajectory.fot_wrapper \ import run_fot from perception.msg import Waypoint, LaneChange -import behavior_speed as bs +import planning +from behavior_agent.behaviours import behavior_speed as bs # from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion # from carla_msgs.msg import CarlaRoute # , CarlaWorldInfo From 8bbea27ba71b6c635109fa8f883b02442cc9d6bd Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Fri, 19 Jan 2024 19:10:13 +0100 Subject: [PATCH 06/19] feat: Test overtake waypoints are possible and visible --- build/docker-compose.yml | 4 +- code/agent/config/rviz_config.rviz | 2 +- .../src/global_plan_distance_publisher.py | 2 +- code/planning/launch/planning.launch | 4 +- .../src/global_planner/global_planner.py | 6 +- code/planning/src/local_planner/ACC.py | 2 +- .../src/local_planner/motion_planning.py | 153 ++++++++++-------- 7 files changed, 93 insertions(+), 80 deletions(-) diff --git a/build/docker-compose.yml b/build/docker-compose.yml index f3d46bf3..10431837 100644 --- a/build/docker-compose.yml +++ b/build/docker-compose.yml @@ -61,8 +61,8 @@ services: tty: true shm_size: 2gb #command: bash -c "sleep 10 && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/opt/leaderboard/leaderboard/autoagents/npc_agent.py --host=carla-simulator --track=SENSORS" - # command: bash -c "sleep 10 && roslaunch agent/launch/dev.launch" - command: bash -c "sleep 10 && sudo chown -R carla:carla ../code/ && sudo chmod -R a+w ../code/ && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/workspace/code/agent/src/agent/agent.py --host=paf23-carla-simulator-1 --track=MAP" + command: bash -c "sleep 10 && roslaunch agent/launch/dev.launch" + # command: bash -c "sleep 10 && sudo chown -R carla:carla ../code/ && sudo chmod -R a+w ../code/ && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/workspace/code/agent/src/agent/agent.py --host=paf23-carla-simulator-1 --track=MAP" logging: driver: "local" diff --git a/code/agent/config/rviz_config.rviz b/code/agent/config/rviz_config.rviz index 42b620d5..1de772b6 100644 --- a/code/agent/config/rviz_config.rviz +++ b/code/agent/config/rviz_config.rviz @@ -173,7 +173,7 @@ Visualization Manager: Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 - Topic: /paf/hero/trajectory + Topic: /paf/hero/trajectory_enhanced Unreliable: false Value: true - Alpha: 1 diff --git a/code/perception/src/global_plan_distance_publisher.py b/code/perception/src/global_plan_distance_publisher.py index 40da367c..4e4cf37b 100755 --- a/code/perception/src/global_plan_distance_publisher.py +++ b/code/perception/src/global_plan_distance_publisher.py @@ -47,7 +47,7 @@ def __init__(self): # Change comment for dev_launch self.global_plan_subscriber = self.new_subscription( CarlaRoute, - "/carla/" + self.role_name + "/global_plan", + "/paf/" + self.role_name + "/global_plan", self.update_global_route, qos_profile=1) # self.global_plan_subscriber = self.new_subscription( diff --git a/code/planning/launch/planning.launch b/code/planning/launch/planning.launch index 9dee1352..72c1f88c 100644 --- a/code/planning/launch/planning.launch +++ b/code/planning/launch/planning.launch @@ -16,13 +16,13 @@ - + diff --git a/code/planning/src/global_planner/global_planner.py b/code/planning/src/global_planner/global_planner.py index e39cb4be..5e420f47 100755 --- a/code/planning/src/global_planner/global_planner.py +++ b/code/planning/src/global_planner/global_planner.py @@ -57,7 +57,7 @@ def __init__(self): # uncomment /paf/hero/global_plan and comment /carla/... for dev_launch self.global_plan_sub = self.new_subscription( msg_type=CarlaRoute, - topic='/carla/' + self.role_name + '/global_plan', + topic='/paf/' + self.role_name + '/global_plan', callback=self.global_route_callback, qos_profile=10) # self.global_plan_sub = self.new_subscription( @@ -74,7 +74,7 @@ def __init__(self): self.path_pub = self.new_publisher( msg_type=Path, - topic='/paf/' + self.role_name + '/trajectory', + topic='/paf/' + self.role_name + '/trajectory_dummy', qos_profile=1) self.speed_limit_pub = self.new_publisher( @@ -84,7 +84,7 @@ def __init__(self): self.logdebug('PrePlanner-Node started') # uncomment for self.dev_load_world_info() for dev_launch - # self.dev_load_world_info() + self.dev_load_world_info() def global_route_callback(self, data: CarlaRoute) -> None: """ diff --git a/code/planning/src/local_planner/ACC.py b/code/planning/src/local_planner/ACC.py index 4b7c2b9b..a8b0c2dc 100755 --- a/code/planning/src/local_planner/ACC.py +++ b/code/planning/src/local_planner/ACC.py @@ -88,7 +88,7 @@ def _set_distance(self, data: Float32): """Get min distance to object in front from perception Args: - data (MinDistance): Minimum Distance from LIDAR + data (Float32): Minimum Distance from LIDAR """ self.obstacle_distance = data.data diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index e9cd1e7f..1310233c 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -51,9 +51,14 @@ def __init__(self): self.__stopline = None # (Distance, isStopline) self.__change_point = None # (Distance, isLaneChange, roadOption) # Subscriber + self.test_sub = self.new_subscription( + Bool, + f"/paf/{self.role_name}/test", + self.change_trajectory, + qos_profile=1) self.trajectory_sub = self.new_subscription( Path, - f"/paf/{self.role_name}/trajectory", + f"/paf/{self.role_name}/trajectory_dummy", self.__set_trajectory, qos_profile=1) self.current_pos_sub = self.new_subscription( @@ -91,8 +96,8 @@ def __init__(self): # Publisher self.traj_pub: Publisher = self.new_publisher( - Path, - f"/paf/{self.role_name}/trajectory", + msg_type=Path, + topic=f"/paf/{self.role_name}/trajectory_enhanced", qos_profile=1) self.velocity_pub: Publisher = self.new_publisher( Float32, @@ -108,6 +113,7 @@ def __init__(self): self.logdebug("MotionPlanning started") self.published = False self.current_pos = None + self.trajectory = None def __set_current_pos(self, data: PoseStamped): """set current position @@ -118,76 +124,82 @@ def __set_current_pos(self, data: PoseStamped): self.current_pos = np.array([data.pose.position.x, data.pose.position.y]) + def change_trajectory(self, data: Bool): + data = self.trajectory + self.logerr("Trajectory chagen started") + np_array = np.array(data.poses) + selection = np_array[10:26] + waypoints = self.convert_pose_to_array(selection) + self.logerr("waypoints " + str(waypoints)) + obs = np.array([[waypoints[7][0]-0.5, waypoints[7][1], waypoints[7][0]+0.5, waypoints[7][1]-2]]) + self.logerr("obs " + str(obs)) + initial_conditions = { + 'ps': 0, + 'target_speed': self.__acc_speed, + 'pos': waypoints[0], + 'vel': np.array([5, 1]), + 'wp': waypoints, + 'obs': obs + } + hyperparameters = { + "max_speed": 25.0, + "max_accel": 15.0, + "max_curvature": 15.0, + "max_road_width_l": 3.0, + "max_road_width_r": 0.5, + "d_road_w": 0.5, + "dt": 0.2, + "maxt": 20.0, + "mint": 6.0, + "d_t_s": 0.5, + "n_s_sample": 2.0, + "obstacle_clearance": 0.4, + "kd": 1.0, + "kv": 0.1, + "ka": 0.1, + "kj": 0.1, + "kt": 0.1, + "ko": 0.1, + "klat": 1.0, + "klon": 1.0, + "num_threads": 0, # set 0 to avoid using threaded algorithm + } + result_x, result_y, speeds, ix, iy, iyaw, d, s, speeds_x, \ + speeds_y, misc, costs, success = run_fot(initial_conditions, + hyperparameters) + if success: + print("Success!") + print("result_x: ", result_x) + print("result_y: ", result_y) + result = [] + for i in range(len(result_x)): + position = Point(result_x[i], result_y[i], 0) + quaternion = tf.transformations.quaternion_from_euler(0, + 0, + iyaw[i]) + orientation = Quaternion(x=quaternion[0], y=quaternion[1], + z=quaternion[2], w=quaternion[3]) + pose = Pose(position, orientation) + pos = PoseStamped() + pos.header.stamp = rospy.Time.now() + pos.header.frame_id = "global" + pos.pose = pose + self.logerr(pos) + result.append(pos) + path = Path() + path.header.stamp = rospy.Time.now() + path.header.frame_id = "global" + path.poses = list(np_array[:10]) + result + list(np_array[26:]) + self.logerr(path) + self.traj_pub.publish(path) + def __set_trajectory(self, data: Path): """get current trajectory global planning Args: data (Path): Trajectory waypoints """ - if len(data.poses) and not self.published > 0: - self.published = True - np_array = np.array(data.poses) - selection = np_array[5:17] - waypoints = self.convert_pose_to_array(selection) - self.logerr(waypoints) - obs = np.array([[waypoints[5][0]-0.5, waypoints[5][1], waypoints[5][0]+0.5, waypoints[5][1]-2]]) - initial_conditions = { - 'ps': 0, - 'target_speed': self.__acc_speed, - 'pos': np.array([983.5807552562393, 5370.014637890163]), - 'vel': np.array([5, 1]), - 'wp': waypoints, - 'obs': obs - } - hyperparameters = { - "max_speed": 25.0, - "max_accel": 15.0, - "max_curvature": 15.0, - "max_road_width_l": 3.0, - "max_road_width_r": 0.5, - "d_road_w": 0.5, - "dt": 0.2, - "maxt": 20.0, - "mint": 6.0, - "d_t_s": 0.5, - "n_s_sample": 2.0, - "obstacle_clearance": 0.1, - "kd": 1.0, - "kv": 0.1, - "ka": 0.1, - "kj": 0.1, - "kt": 0.1, - "ko": 0.1, - "klat": 1.0, - "klon": 1.0, - "num_threads": 0, # set 0 to avoid using threaded algorithm - } - result_x, result_y, speeds, ix, iy, iyaw, d, s, speeds_x, \ - speeds_y, misc, costs, success = run_fot(initial_conditions, - hyperparameters) - if success: - print("Success!") - print("result_x: ", result_x) - print("result_y: ", result_y) - result = [] - for i in range(len(result_x)): - position = Point(result_x[i], result_y[i], 703) - quaternion = tf.transformations.quaternion_from_euler(0, - 0, - iyaw[i]) - orientation = Quaternion(x=quaternion[0], y=quaternion[1], - z=quaternion[2], w=quaternion[3]) - pose = Pose(position, orientation) - pos = PoseStamped() - pos.header.stamp = rospy.Time.now() - pos.header.frame_id = "global" - pos.pose = pose - result.append(PoseStamped) - path = Path() - path.header.stamp = rospy.Time.now() - path.path_backup.header.frame_id = "global" - path.poses = list(np_array[:5]) + result + list(np_array[17:]) - self.traj_pub.publish(path) + self.trajectory = data def convert_pose_to_array(self, poses: np.array): """convert pose array to numpy array @@ -198,9 +210,10 @@ def convert_pose_to_array(self, poses: np.array): Returns: np.array: numpy array """ - def get_x_y(pose): - return np.array([pose.pose.position.x, pose.pose.position.y]) - return np.vectorize(get_x_y)(poses) + result_array = np.empty((len(poses), 2)) + for pose in range(len(poses)): + result_array[pose] = np.array([poses[pose].pose.position.x, poses[pose].pose.position.y]) + return result_array def __check_emergency(self, data: Bool): """If an emergency stop is needed first check if we are From 05ac84bcb886d18deab0b2a085677966e94b799b Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Sat, 20 Jan 2024 18:08:46 +0100 Subject: [PATCH 07/19] feat: added spawn vehicle --- code/agent/config/rviz_config.rviz | 4 +- .../src/local_planner/motion_planning.py | 87 ++++++++++++++++--- .../src/local_planner/spawn_vehicle.py | 24 +++++ 3 files changed, 100 insertions(+), 15 deletions(-) create mode 100644 code/planning/src/local_planner/spawn_vehicle.py diff --git a/code/agent/config/rviz_config.rviz b/code/agent/config/rviz_config.rviz index 1de772b6..abfef827 100644 --- a/code/agent/config/rviz_config.rviz +++ b/code/agent/config/rviz_config.rviz @@ -63,7 +63,7 @@ Visualization Manager: Unreliable: false Value: true Visibility: - Grid: false + Grid: true Imu: false Path: true PointCloud2: false @@ -173,7 +173,7 @@ Visualization Manager: Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 - Topic: /paf/hero/trajectory_enhanced + Topic: /paf/hero/trajectory Unreliable: false Value: true - Alpha: 1 diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index 1310233c..2272e163 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -11,8 +11,9 @@ from nav_msgs.msg import Path from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion - +import carla import numpy as np +import math # from behavior_agent.msg import BehaviorSpeed from frenet_optimal_trajectory_planner.FrenetOptimalTrajectory.fot_wrapper \ @@ -27,6 +28,29 @@ # from std_msgs.msg import String # from std_msgs.msg import Float32MultiArray +# import numpy as np +# from scipy.spatial.transform import Rotation + +# # Annahme: Ihre eigene Position im globalen Koordinatensystem +# your_position_global = np.array([x, y, z]) + +# # Annahme: Ihre Rotation als Quaternion +# your_rotation_quaternion = np.array([w, x, y, z]) + +# # Annahme: Distanz zum vorausfahrenden Fahrzeug +# distance_to_vehicle = 10.0 # Beispielwert + +# # Annahme: Relative Position des vorausfahrenden Fahrzeugs in Ihrem lokalen Koordinatensystem +# relative_position_local = np.array([0, 0, distance_to_vehicle]) + +# # Schritt 1: Rotation auf die relative Position anwenden +# rotation_matrix = Rotation.from_quat(your_rotation_quaternion).as_matrix() +# absolute_position_local = rotation_matrix.dot(relative_position_local) + +# # Schritt 2: Absolute Position in das globale Koordinatensystem transformieren +# vehicle_position_global = your_position_global + absolute_position_local + +# print("Globale Position des vorausfahrenden Fahrzeugs:", vehicle_position_global) def convert_to_ms(speed): return speed / 3.6 @@ -97,7 +121,7 @@ def __init__(self): # Publisher self.traj_pub: Publisher = self.new_publisher( msg_type=Path, - topic=f"/paf/{self.role_name}/trajectory_enhanced", + topic=f"/paf/{self.role_name}/trajectory", qos_profile=1) self.velocity_pub: Publisher = self.new_publisher( Float32, @@ -115,9 +139,30 @@ def __init__(self): self.current_pos = None self.trajectory = None + def _location_to_gps(self, lat_ref, lon_ref, x, y): + """ + Convert from world coordinates to GPS coordinates + :param lat_ref: latitude reference for the current map + :param lon_ref: longitude reference for the current map + :param location: location to translate + :return: dictionary with lat, lon and height + """ + + EARTH_RADIUS_EQUA = 6378137.0 # pylint: disable=invalid-name + scale = math.cos(lat_ref * math.pi / 180.0) + mx = scale * lon_ref * math.pi * EARTH_RADIUS_EQUA / 180.0 + my = scale * EARTH_RADIUS_EQUA * math.log(math.tan((90.0 + lat_ref) * math.pi / 360.0)) + mx += x + my -= y + + lon = mx * 180.0 / (math.pi * EARTH_RADIUS_EQUA * scale) + lat = 360.0 * math.atan(math.exp(my / (EARTH_RADIUS_EQUA * scale))) / math.pi - 90.0 + z = 703 + + return {'lat': lat, 'lon': lon, 'z': z} + def __set_current_pos(self, data: PoseStamped): """set current position - Args: data (PoseStamped): current position """ @@ -125,17 +170,21 @@ def __set_current_pos(self, data: PoseStamped): data.pose.position.y]) def change_trajectory(self, data: Bool): + index_car = 20 + limit_waypoints = 100 data = self.trajectory self.logerr("Trajectory chagen started") np_array = np.array(data.poses) - selection = np_array[10:26] + selection = np_array[:limit_waypoints] waypoints = self.convert_pose_to_array(selection) self.logerr("waypoints " + str(waypoints)) - obs = np.array([[waypoints[7][0]-0.5, waypoints[7][1], waypoints[7][0]+0.5, waypoints[7][1]-2]]) + obs = np.array([[waypoints[index_car][0]-0.5, waypoints[index_car][1], waypoints[index_car][0]+0.5, waypoints[index_car][1]-2]]) self.logerr("obs " + str(obs)) + pos_lat_lon = self._location_to_gps(0,0, waypoints[0][0], waypoints[0][1]) + initial_conditions = { - 'ps': 0, - 'target_speed': self.__acc_speed, + 'ps': pos_lat_lon["lon"], + 'target_speed': 11, 'pos': waypoints[0], 'vel': np.array([5, 1]), 'wp': waypoints, @@ -145,15 +194,15 @@ def change_trajectory(self, data: Bool): "max_speed": 25.0, "max_accel": 15.0, "max_curvature": 15.0, - "max_road_width_l": 3.0, - "max_road_width_r": 0.5, + "max_road_width_l": 1, + "max_road_width_r": 0, "d_road_w": 0.5, "dt": 0.2, "maxt": 20.0, "mint": 6.0, "d_t_s": 0.5, "n_s_sample": 2.0, - "obstacle_clearance": 0.4, + "obstacle_clearance": 0.1, "kd": 1.0, "kv": 0.1, "ka": 0.1, @@ -168,11 +217,24 @@ def change_trajectory(self, data: Bool): speeds_y, misc, costs, success = run_fot(initial_conditions, hyperparameters) if success: + client = carla.Client("paf23-carla-simulator-1", 2000) + client.set_timeout(2.0) + world = client.get_world() + blueprint_library = world.get_blueprint_library() + bp = blueprint_library.filter("model3")[0] + world = client.get_world() + spawnPoint=carla.Transform(carla.Location(x=waypoints[index_car][0],y=waypoints[index_car][1], z=703),carla.Rotation(pitch=0.0, yaw=0.0, roll=0.000000)) + vehicle = world.spawn_actor(bp, spawnPoint) + spectator = world.get_spectator() + self.logerr("vehicle location: " + str(vehicle.get_location())) + spectator.set_transform(carla.Transform(vehicle.get_location() + carla.Location(z=703), + carla.Rotation(pitch=-90))) print("Success!") print("result_x: ", result_x) print("result_y: ", result_y) + self.logerr("result yaw: " + str(iyaw)) result = [] - for i in range(len(result_x)): + for i in range(len(result_x)-1): position = Point(result_x[i], result_y[i], 0) quaternion = tf.transformations.quaternion_from_euler(0, 0, @@ -184,12 +246,11 @@ def change_trajectory(self, data: Bool): pos.header.stamp = rospy.Time.now() pos.header.frame_id = "global" pos.pose = pose - self.logerr(pos) result.append(pos) path = Path() path.header.stamp = rospy.Time.now() path.header.frame_id = "global" - path.poses = list(np_array[:10]) + result + list(np_array[26:]) + path.poses = result + list(np_array[limit_waypoints:]) self.logerr(path) self.traj_pub.publish(path) diff --git a/code/planning/src/local_planner/spawn_vehicle.py b/code/planning/src/local_planner/spawn_vehicle.py new file mode 100644 index 00000000..abc8d63d --- /dev/null +++ b/code/planning/src/local_planner/spawn_vehicle.py @@ -0,0 +1,24 @@ +import carla +import os + + +CARLA_HOST = os.environ.get('CARLA_HOST', 'paf23-carla-simulator-1') +CARLA_PORT = int(os.environ.get('CARLA_PORT', '2000')) + +client = carla.Client(CARLA_HOST, CARLA_PORT) + +world = client.get_world() +world.wait_for_tick() + + +blueprint_library = world.get_blueprint_library() +bp = blueprint_library.filter('vehicle.*')[0] +vehicle = world.spawn_actor(bp, world.get_map().get_spawn_points()[0]) +vehicle.set_autopilot(False) + +# get spectator +spectator = world.get_spectator() +# set spectator to follow ego vehicle with offset +spectator.set_transform( + carla.Transform(vehicle.get_location() + carla.Location(z=50), + carla.Rotation(pitch=-90))) From 77fd4e0c07f041b8bdb3f7ef204473de6fb88f12 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Mon, 22 Jan 2024 17:12:05 +0100 Subject: [PATCH 08/19] fix: TestCode --- .../src/local_planner/motion_planning.py | 80 ++++++++----------- .../src/local_planner/spawn_vehicle.py | 20 ++++- .../src/local_planner/test_approx_pos.py | 49 ++++++++++++ 3 files changed, 101 insertions(+), 48 deletions(-) create mode 100644 code/planning/src/local_planner/test_approx_pos.py diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index 2272e163..c60f72c9 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -21,40 +21,27 @@ from perception.msg import Waypoint, LaneChange import planning from behavior_agent.behaviours import behavior_speed as bs +from scipy.spatial.transform import Rotation -# from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion -# from carla_msgs.msg import CarlaRoute # , CarlaWorldInfo -# from nav_msgs.msg import Path -# from std_msgs.msg import String -# from std_msgs.msg import Float32MultiArray -# import numpy as np -# from scipy.spatial.transform import Rotation - -# # Annahme: Ihre eigene Position im globalen Koordinatensystem -# your_position_global = np.array([x, y, z]) - -# # Annahme: Ihre Rotation als Quaternion -# your_rotation_quaternion = np.array([w, x, y, z]) - -# # Annahme: Distanz zum vorausfahrenden Fahrzeug -# distance_to_vehicle = 10.0 # Beispielwert - -# # Annahme: Relative Position des vorausfahrenden Fahrzeugs in Ihrem lokalen Koordinatensystem -# relative_position_local = np.array([0, 0, distance_to_vehicle]) +def convert_to_ms(speed): + return speed / 3.6 -# # Schritt 1: Rotation auf die relative Position anwenden -# rotation_matrix = Rotation.from_quat(your_rotation_quaternion).as_matrix() -# absolute_position_local = rotation_matrix.dot(relative_position_local) +def approx_obstacle_pos(distance: float, heading: float, ego_pos: np.array): + """calculate the position of the obstacle in the global coordinate system + based on ego position, heading and distance + """ + rotation_matrix = Rotation.from_euler('z', heading).as_matrix() -# # Schritt 2: Absolute Position in das globale Koordinatensystem transformieren -# vehicle_position_global = your_position_global + absolute_position_local + # Annahme: Relative Position des vorausfahrenden Fahrzeugs in Ihrem lokalen Koordinatensystem + relative_position_local = np.array([0, distance, 0]) -# print("Globale Position des vorausfahrenden Fahrzeugs:", vehicle_position_global) - -def convert_to_ms(speed): - return speed / 3.6 + # Schritt 1: Rotation auf die relative Position anwenden + absolute_position_local = rotation_matrix.dot(relative_position_local) + # Schritt 2: Absolute Position in das globale Koordinatensystem transformieren + vehicle_position_global = ego_pos + absolute_position_local + return vehicle_position_global class MotionPlanning(CompatibleNode): """ @@ -75,6 +62,11 @@ def __init__(self): self.__stopline = None # (Distance, isStopline) self.__change_point = None # (Distance, isLaneChange, roadOption) # Subscriber + self.head_sub = self.new_subscription( + Float32, + f"/paf/{self.role_name}/current_heading", + self.__set_heading, + qos_profile=1) self.test_sub = self.new_subscription( Bool, f"/paf/{self.role_name}/test", @@ -137,8 +129,17 @@ def __init__(self): self.logdebug("MotionPlanning started") self.published = False self.current_pos = None + self.current_heading = None self.trajectory = None + def __set_heading(self, data: Float32): + """Set current Heading + + Args: + data (Float32): Current Heading vom Subscriber + """ + self.current_heading = data.data + def _location_to_gps(self, lat_ref, lon_ref, x, y): """ Convert from world coordinates to GPS coordinates @@ -167,7 +168,8 @@ def __set_current_pos(self, data: PoseStamped): data (PoseStamped): current position """ self.current_pos = np.array([data.pose.position.x, - data.pose.position.y]) + data.pose.position.y, + data.pose.position.z]) def change_trajectory(self, data: Bool): index_car = 20 @@ -181,7 +183,7 @@ def change_trajectory(self, data: Bool): obs = np.array([[waypoints[index_car][0]-0.5, waypoints[index_car][1], waypoints[index_car][0]+0.5, waypoints[index_car][1]-2]]) self.logerr("obs " + str(obs)) pos_lat_lon = self._location_to_gps(0,0, waypoints[0][0], waypoints[0][1]) - + self.logerr("approx position: " + str(self.approx_obstacle_pos())) initial_conditions = { 'ps': pos_lat_lon["lon"], 'target_speed': 11, @@ -193,9 +195,9 @@ def change_trajectory(self, data: Bool): hyperparameters = { "max_speed": 25.0, "max_accel": 15.0, - "max_curvature": 15.0, - "max_road_width_l": 1, - "max_road_width_r": 0, + "max_curvature": 20.0, + "max_road_width_l": 1.5, + "max_road_width_r": 0.5, "d_road_w": 0.5, "dt": 0.2, "maxt": 20.0, @@ -217,18 +219,6 @@ def change_trajectory(self, data: Bool): speeds_y, misc, costs, success = run_fot(initial_conditions, hyperparameters) if success: - client = carla.Client("paf23-carla-simulator-1", 2000) - client.set_timeout(2.0) - world = client.get_world() - blueprint_library = world.get_blueprint_library() - bp = blueprint_library.filter("model3")[0] - world = client.get_world() - spawnPoint=carla.Transform(carla.Location(x=waypoints[index_car][0],y=waypoints[index_car][1], z=703),carla.Rotation(pitch=0.0, yaw=0.0, roll=0.000000)) - vehicle = world.spawn_actor(bp, spawnPoint) - spectator = world.get_spectator() - self.logerr("vehicle location: " + str(vehicle.get_location())) - spectator.set_transform(carla.Transform(vehicle.get_location() + carla.Location(z=703), - carla.Rotation(pitch=-90))) print("Success!") print("result_x: ", result_x) print("result_y: ", result_y) diff --git a/code/planning/src/local_planner/spawn_vehicle.py b/code/planning/src/local_planner/spawn_vehicle.py index abc8d63d..efebd901 100644 --- a/code/planning/src/local_planner/spawn_vehicle.py +++ b/code/planning/src/local_planner/spawn_vehicle.py @@ -1,5 +1,6 @@ import carla import os +import rospy CARLA_HOST = os.environ.get('CARLA_HOST', 'paf23-carla-simulator-1') @@ -12,13 +13,26 @@ blueprint_library = world.get_blueprint_library() -bp = blueprint_library.filter('vehicle.*')[0] -vehicle = world.spawn_actor(bp, world.get_map().get_spawn_points()[0]) +# bp = blueprint_library.filter('vehicle.*')[0] +# vehicle = world.spawn_actor(bp, world.get_map().get_spawn_points()[0]) +bp = blueprint_library.filter("model3")[0] +world = client.get_world() +loc = carla.Location(x=983.59, y=-5450.01, z=703.810118745277) +print("location: " + str(loc)) +spawnPoint = carla.Transform(loc, carla.Rotation(pitch=0.0, yaw=0.0, roll=0.0)) +print("spawnPoint: " + str(spawnPoint)) +vehicle = world.spawn_actor(bp, spawnPoint) + +print("spawned vehicle: " + str(vehicle.get_location())) vehicle.set_autopilot(False) +vehicle.set_location(loc) +print("spawned vehicle: " + str(vehicle.get_location())) # get spectator spectator = world.get_spectator() # set spectator to follow ego vehicle with offset spectator.set_transform( - carla.Transform(vehicle.get_location() + carla.Location(z=50), + carla.Transform(loc + carla.Location(z=50), carla.Rotation(pitch=-90))) +print(spectator.get_transform()) +client.reload_world() diff --git a/code/planning/src/local_planner/test_approx_pos.py b/code/planning/src/local_planner/test_approx_pos.py new file mode 100644 index 00000000..8ffde0b5 --- /dev/null +++ b/code/planning/src/local_planner/test_approx_pos.py @@ -0,0 +1,49 @@ +import unittest +import numpy as np +import matplotlib.pyplot as plt +from scipy.spatial.transform import Rotation + + +def approx_obstacle_pos(distance: float, heading: float, ego_pos: np.array): + """calculate the position of the obstacle in the global coordinate system + based on ego position, heading and distance + """ + rotation_matrix = Rotation.from_euler('z', heading).as_matrix() + + # Annahme: Relative Position des vorausfahrenden Fahrzeugs in Ihrem lokalen Koordinatensystem + relative_position_local = np.array([0, distance, 0]) + + # Schritt 1: Rotation auf die relative Position anwenden + absolute_position_local = rotation_matrix.dot(relative_position_local) + + # Schritt 2: Absolute Position in das globale Koordinatensystem transformieren + vehicle_position_global = ego_pos + absolute_position_local + return vehicle_position_global + + +class TestMotionPlanning(unittest.TestCase): + def test_approx_obstacle_pos(self): + ego_pos = np.array([0, 0, 0]) + target_positions = [np.array([10, 10, 0]), np.array([20, 20, 0]), np.array([30, 30, 0])] + headings = [0, np.pi/2, np.pi, 3*np.pi/2] + + plt.figure() + plt.title('Approximated vs Expected Positions') + plt.xlabel('x') + plt.ylabel('y') + + for target_position in target_positions: + for heading in headings: + distance = np.linalg.norm(target_position - ego_pos) + calculated_position = approx_obstacle_pos(distance, heading, ego_pos) + np.testing.assert_array_almost_equal(calculated_position, target_position, decimal=5) + + plt.scatter(*target_positions[:2], color='blue', label='Expected') + plt.scatter(*calculated_position[:2], color='red', label='Calculated') + + plt.legend() + plt.show() + + +if __name__ == '__main__': + unittest.main() \ No newline at end of file From 9cc15083221e6e4396ddd4117a3169d3815ba310 Mon Sep 17 00:00:00 2001 From: samuelkuehnel <51356601+samuelkuehnel@users.noreply.github.com> Date: Mon, 22 Jan 2024 23:43:21 +0100 Subject: [PATCH 09/19] Update test_approx_pos.py --- .../src/local_planner/test_approx_pos.py | 24 ++++++++++--------- 1 file changed, 13 insertions(+), 11 deletions(-) diff --git a/code/planning/src/local_planner/test_approx_pos.py b/code/planning/src/local_planner/test_approx_pos.py index 8ffde0b5..12b645b7 100644 --- a/code/planning/src/local_planner/test_approx_pos.py +++ b/code/planning/src/local_planner/test_approx_pos.py @@ -2,6 +2,7 @@ import numpy as np import matplotlib.pyplot as plt from scipy.spatial.transform import Rotation +import math def approx_obstacle_pos(distance: float, heading: float, ego_pos: np.array): @@ -24,26 +25,27 @@ def approx_obstacle_pos(distance: float, heading: float, ego_pos: np.array): class TestMotionPlanning(unittest.TestCase): def test_approx_obstacle_pos(self): ego_pos = np.array([0, 0, 0]) - target_positions = [np.array([10, 10, 0]), np.array([20, 20, 0]), np.array([30, 30, 0])] - headings = [0, np.pi/2, np.pi, 3*np.pi/2] + target_positions = np.array([np.array([10, 10, 0]), np.array([20, 10, 0]), np.array([30, 30, 0])]) + headings = [math.radians(315), -0.4636476, math.radians(315)] plt.figure() plt.title('Approximated vs Expected Positions') plt.xlabel('x') plt.ylabel('y') - for target_position in target_positions: - for heading in headings: - distance = np.linalg.norm(target_position - ego_pos) - calculated_position = approx_obstacle_pos(distance, heading, ego_pos) - np.testing.assert_array_almost_equal(calculated_position, target_position, decimal=5) + for index in range(len(target_positions)): + distance = np.linalg.norm(target_positions[index] - ego_pos) + calculated_position = approx_obstacle_pos(distance, headings[index], ego_pos) + print(calculated_position) + # np.testing.assert_array_almost_equal(calculated_position, target_position, decimal=1) - plt.scatter(*target_positions[:2], color='blue', label='Expected') - plt.scatter(*calculated_position[:2], color='red', label='Calculated') + plt.scatter(*target_positions[:2], color='blue', label='Expected') + plt.scatter(*calculated_position[:2], color='red', label='Calculated', marker='x') - plt.legend() + plt.legend(loc='lower left') + plt.grid(visible=True) plt.show() if __name__ == '__main__': - unittest.main() \ No newline at end of file + unittest.main() From 742820400e8d544e1cd25c400ea390809a10dd0e Mon Sep 17 00:00:00 2001 From: samuelkuehnel <51356601+samuelkuehnel@users.noreply.github.com> Date: Mon, 22 Jan 2024 23:48:16 +0100 Subject: [PATCH 10/19] Update test_approx_pos.py number 2 --- code/planning/src/local_planner/test_approx_pos.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/code/planning/src/local_planner/test_approx_pos.py b/code/planning/src/local_planner/test_approx_pos.py index 12b645b7..4bd5e37c 100644 --- a/code/planning/src/local_planner/test_approx_pos.py +++ b/code/planning/src/local_planner/test_approx_pos.py @@ -9,13 +9,13 @@ def approx_obstacle_pos(distance: float, heading: float, ego_pos: np.array): """calculate the position of the obstacle in the global coordinate system based on ego position, heading and distance """ - rotation_matrix = Rotation.from_euler('z', heading).as_matrix() + rotation_matrix = Rotation.from_euler('z', heading) # Annahme: Relative Position des vorausfahrenden Fahrzeugs in Ihrem lokalen Koordinatensystem relative_position_local = np.array([0, distance, 0]) # Schritt 1: Rotation auf die relative Position anwenden - absolute_position_local = rotation_matrix.dot(relative_position_local) + absolute_position_local = rotation_matrix.apply(relative_position_local) # Schritt 2: Absolute Position in das globale Koordinatensystem transformieren vehicle_position_global = ego_pos + absolute_position_local From 44d542e5e98dbc93272698eaa76b7941b72c93f1 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Tue, 23 Jan 2024 14:37:54 +0100 Subject: [PATCH 11/19] feat: obstacle position approx done --- .../src/local_planner/motion_planning.py | 36 +++++++++++++++---- .../src/local_planner/test_approx_pos.py | 8 ++--- .../00_paf23/05_local_trajectory_planning.md | 17 +++++++++ 3 files changed, 50 insertions(+), 11 deletions(-) create mode 100644 doc/03_research/03_planning/00_paf23/05_local_trajectory_planning.md diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index c60f72c9..b0980269 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -27,22 +27,32 @@ def convert_to_ms(speed): return speed / 3.6 + def approx_obstacle_pos(distance: float, heading: float, ego_pos: np.array): """calculate the position of the obstacle in the global coordinate system based on ego position, heading and distance + + Args: + distance (float): Distance to the obstacle + heading (float): Ego vehivle heading + ego_pos (np.array): Position in [x, y, z] + + Returns: + np.array: approximated position of the obstacle """ - rotation_matrix = Rotation.from_euler('z', heading).as_matrix() + rotation_matrix = Rotation.from_euler('z', heading) - # Annahme: Relative Position des vorausfahrenden Fahrzeugs in Ihrem lokalen Koordinatensystem - relative_position_local = np.array([0, distance, 0]) + # Create distance vector with 0 rotation + relative_position_local = np.array([distance, 0, 0]) - # Schritt 1: Rotation auf die relative Position anwenden - absolute_position_local = rotation_matrix.dot(relative_position_local) + # Rotate distance vector to match heading + absolute_position_local = rotation_matrix.apply(relative_position_local) - # Schritt 2: Absolute Position in das globale Koordinatensystem transformieren + # Add egomposition vector with distance vetor to get absolute position vehicle_position_global = ego_pos + absolute_position_local return vehicle_position_global + class MotionPlanning(CompatibleNode): """ This node selects speeds according to the behavior in the Decision Tree @@ -131,6 +141,8 @@ def __init__(self): self.current_pos = None self.current_heading = None self.trajectory = None + self.overtaking = False + self.overtake_start = None def __set_heading(self, data: Float32): """Set current Heading @@ -172,18 +184,21 @@ def __set_current_pos(self, data: PoseStamped): data.pose.position.z]) def change_trajectory(self, data: Bool): + self.overtaking = True + self.overtake_start = rospy.get_rostime() index_car = 20 limit_waypoints = 100 data = self.trajectory self.logerr("Trajectory chagen started") np_array = np.array(data.poses) + obstacle_position = approx_obstacle_pos(20, self.current_heading, self.current_pos) selection = np_array[:limit_waypoints] waypoints = self.convert_pose_to_array(selection) self.logerr("waypoints " + str(waypoints)) obs = np.array([[waypoints[index_car][0]-0.5, waypoints[index_car][1], waypoints[index_car][0]+0.5, waypoints[index_car][1]-2]]) self.logerr("obs " + str(obs)) pos_lat_lon = self._location_to_gps(0,0, waypoints[0][0], waypoints[0][1]) - self.logerr("approx position: " + str(self.approx_obstacle_pos())) + self.logerr("Distance: 40, Heading: " + str(self.current_heading) + ", Position: " + str(self.current_pos) + "\napprox position: " + str(approx_obstacle_pos(40, self.current_heading, self.current_pos))) initial_conditions = { 'ps': pos_lat_lon["lon"], 'target_speed': 11, @@ -251,6 +266,8 @@ def __set_trajectory(self, data: Path): data (Path): Trajectory waypoints """ self.trajectory = data + if not self.overtaking: + self.traj_pub.publish(data) def convert_pose_to_array(self, poses: np.array): """convert pose array to numpy array @@ -397,6 +414,11 @@ def run(self): def loop(timer_event=None): self.update_target_speed(self.__acc_speed, self.__curr_behavior) + if self.overtake_start is None: + return + if rospy.get_rostime() - self.overtake_start < rospy.Duration(10): + self.logerr("overtake finished") + self.overtaking = False self.new_timer(self.control_loop_rate, loop) self.spin() diff --git a/code/planning/src/local_planner/test_approx_pos.py b/code/planning/src/local_planner/test_approx_pos.py index 4bd5e37c..280a7455 100644 --- a/code/planning/src/local_planner/test_approx_pos.py +++ b/code/planning/src/local_planner/test_approx_pos.py @@ -11,13 +11,13 @@ def approx_obstacle_pos(distance: float, heading: float, ego_pos: np.array): """ rotation_matrix = Rotation.from_euler('z', heading) - # Annahme: Relative Position des vorausfahrenden Fahrzeugs in Ihrem lokalen Koordinatensystem - relative_position_local = np.array([0, distance, 0]) + # Create distance vector with 0 rotation + relative_position_local = np.array([distance, 0, 0]) - # Schritt 1: Rotation auf die relative Position anwenden + # Rotate distance vector to match heading absolute_position_local = rotation_matrix.apply(relative_position_local) - # Schritt 2: Absolute Position in das globale Koordinatensystem transformieren + # Add egomposition vector with distance vetor to get absolute position vehicle_position_global = ego_pos + absolute_position_local return vehicle_position_global diff --git a/doc/03_research/03_planning/00_paf23/05_local_trajectory_planning.md b/doc/03_research/03_planning/00_paf23/05_local_trajectory_planning.md new file mode 100644 index 00000000..8e998d3b --- /dev/null +++ b/doc/03_research/03_planning/00_paf23/05_local_trajectory_planning.md @@ -0,0 +1,17 @@ +# Local trajectory planning + +**Summary:** Explanation on how the frenet trajectory planner is used inside the motion planning component. + +--- + +## Author + +Samuel Kühnel + +## Date + +23.01.2024 + +## Position calculation from Obstacle + +The position from a possible obstacle that we need to overtake is calculated via the current heading from the ego vehicle, the current position and the measured distance. The \ No newline at end of file From 6c035f4ffd053ec2c93415a00c0bbbb10b8270b0 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Fri, 26 Jan 2024 19:57:18 +0100 Subject: [PATCH 12/19] feat: testing setup --- code/Default.perspective | 229 ++++++++++++++++++ .../src/global_planner/global_planner.py | 16 +- code/planning/src/local_planner/ACC.py | 12 +- .../src/local_planner/motion_planning.py | 202 +++++++++++---- code/planning/src/local_planner/objects.json | 11 + .../src/local_planner/spawn_vehicle.py | 22 +- 6 files changed, 419 insertions(+), 73 deletions(-) create mode 100644 code/Default.perspective create mode 100644 code/planning/src/local_planner/objects.json diff --git a/code/Default.perspective b/code/Default.perspective new file mode 100644 index 00000000..13ed1903 --- /dev/null +++ b/code/Default.perspective @@ -0,0 +1,229 @@ +{ + "keys": {}, + "groups": { + "mainwindow": { + "keys": { + "geometry": { + "repr(QByteArray.hex)": "QtCore.QByteArray(b'01d9d0cb00030000000000900000003600000eff0000086f000000900000008000000eff0000086f00000000000000000f00000000900000008000000eff0000086f')", + "type": "repr(QByteArray.hex)", + "pretty-print": " 6 o o o" + }, + "state": { + "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff00000000fd000000010000000300000e70000007acfc0100000003fb00000058007200710074005f007000750062006c00690073006800650072005f005f005000750062006c00690073006800650072005f005f0031005f005f005000750062006c0069007300680065007200570069006400670065007401000000000000075e0000034100fffffffb0000004c007200710074005f0063006f006e0073006f006c0065005f005f0043006f006e0073006f006c0065005f005f0031005f005f0043006f006e0073006f006c0065005700690064006700650074010000076a000004cd000002ca00fffffffb0000004c007200710074005f0074006f007000690063005f005f0054006f0070006900630050006c007500670069006e005f005f0031005f005f0054006f0070006900630057006900640067006500740100000c430000022d0000011c00ffffff00000e700000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')", + "type": "repr(QByteArray.hex)", + "pretty-print": " Xrqt_publisher__Publisher__1__PublisherWidget Lrqt_console__Console__1__ConsoleWidget Lrqt_topic__TopicPlugin__1__TopicWidget 6MinimizedDockWidgetsToolbar " + } + }, + "groups": { + "toolbar_areas": { + "keys": { + "MinimizedDockWidgetsToolbar": { + "repr": "8", + "type": "repr" + } + }, + "groups": {} + } + } + }, + "pluginmanager": { + "keys": { + "running-plugins": { + "repr": "{'rqt_console/Console': [1], 'rqt_publisher/Publisher': [1], 'rqt_topic/TopicPlugin': [1]}", + "type": "repr" + } + }, + "groups": { + "plugin__rqt_console__Console__1": { + "keys": {}, + "groups": { + "dock_widget__ConsoleWidget": { + "keys": { + "dock_widget_title": { + "repr": "'Console'", + "type": "repr" + }, + "dockable": { + "repr": "True", + "type": "repr" + }, + "parent": { + "repr": "None", + "type": "repr" + } + }, + "groups": {} + }, + "plugin": { + "keys": { + "exclude_filters": { + "repr": "['severity', 'message', 'message']", + "type": "repr" + }, + "filter_splitter": { + "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff0000000100000002000000d4000000d40100000009010000000200')", + "type": "repr(QByteArray.hex)", + "pretty-print": " " + }, + "highlight_filters": { + "repr": "'message'", + "type": "repr" + }, + "message_limit": { + "repr": "20000", + "type": "repr" + }, + "paused": { + "repr": "False", + "type": "repr" + }, + "settings_exist": { + "repr": "True", + "type": "repr" + }, + "show_highlighted_only": { + "repr": "False", + "type": "repr" + }, + "table_splitter": { + "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff00000001000000020000008a000001b10100000009010000000200')", + "type": "repr(QByteArray.hex)", + "pretty-print": " " + } + }, + "groups": { + "exclude_filter_0": { + "keys": { + "enabled": { + "repr": "True", + "type": "repr" + }, + "itemlist": { + "repr": "['Warn', 'Info']", + "type": "repr" + } + }, + "groups": {} + }, + "exclude_filter_1": { + "keys": { + "enabled": { + "repr": "True", + "type": "repr" + }, + "regex": { + "repr": "False", + "type": "repr" + }, + "text": { + "repr": "'Lidar'", + "type": "repr" + } + }, + "groups": {} + }, + "exclude_filter_2": { + "keys": { + "enabled": { + "repr": "True", + "type": "repr" + }, + "regex": { + "repr": "False", + "type": "repr" + }, + "text": { + "repr": "'Vision'", + "type": "repr" + } + }, + "groups": {} + }, + "highlight_filter_0": { + "keys": { + "enabled": { + "repr": "True", + "type": "repr" + }, + "regex": { + "repr": "False", + "type": "repr" + }, + "text": { + "repr": "''", + "type": "repr" + } + }, + "groups": {} + } + } + } + } + }, + "plugin__rqt_publisher__Publisher__1": { + "keys": {}, + "groups": { + "dock_widget__PublisherWidget": { + "keys": { + "dock_widget_title": { + "repr": "'Message Publisher'", + "type": "repr" + }, + "dockable": { + "repr": "True", + "type": "repr" + }, + "parent": { + "repr": "None", + "type": "repr" + } + }, + "groups": {} + }, + "plugin": { + "keys": { + "publishers": { + "repr": "\"[{'topic_name': '/paf/hero/test', 'type_name': 'std_msgs/Bool', 'rate': 1.0, 'enabled': False, 'publisher_id': 0, 'counter': 0, 'expressions': {}}]\"", + "type": "repr" + } + }, + "groups": {} + } + } + }, + "plugin__rqt_topic__TopicPlugin__1": { + "keys": {}, + "groups": { + "dock_widget__TopicWidget": { + "keys": { + "dock_widget_title": { + "repr": "'Topic Monitor'", + "type": "repr" + }, + "dockable": { + "repr": "True", + "type": "repr" + }, + "parent": { + "repr": "None", + "type": "repr" + } + }, + "groups": {} + }, + "plugin": { + "keys": { + "tree_widget_header_state": { + "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff0000000000000001000000000000000001000000000000000000000000000000000000064c0000000501010001000000000000000005000000c8ffffffff0000008100000003000000050000023d0000000100000003000002310000000100000003000000c20000000100000003000000540000000100000003000000c80000000100000003000003e800000000c8')", + "type": "repr(QByteArray.hex)", + "pretty-print": " = 1 T " + } + }, + "groups": {} + } + } + } + } + } + } +} \ No newline at end of file diff --git a/code/planning/src/global_planner/global_planner.py b/code/planning/src/global_planner/global_planner.py index 5e420f47..bfed5f32 100755 --- a/code/planning/src/global_planner/global_planner.py +++ b/code/planning/src/global_planner/global_planner.py @@ -257,16 +257,16 @@ def run(self): :return: """ - def loop(timer_event=None): - if len(self.path_backup.poses) < 1: - return + # def loop(timer_event=None): + # if len(self.path_backup.poses) < 1: + # return - # Continuously update paths time to update car position in rviz - # TODO: remove next lines when local planner exists - self.path_backup.header.stamp = rospy.Time.now() - self.path_pub.publish(self.path_backup) + # # # Continuously update paths time to update car position in rviz + # # # TODO: remove next lines when local planner exists + # self.path_backup.header.stamp = rospy.Time.now() + # self.path_pub.publish(self.path_backup) - self.new_timer(self.control_loop_rate, loop) + # self.new_timer(self.control_loop_rate, loop) self.spin() diff --git a/code/planning/src/local_planner/ACC.py b/code/planning/src/local_planner/ACC.py index a8b0c2dc..ed44ab4c 100755 --- a/code/planning/src/local_planner/ACC.py +++ b/code/planning/src/local_planner/ACC.py @@ -47,7 +47,7 @@ def __init__(self): # Get trajectory to determine current speed limit self.trajectory_sub: Subscriber = self.new_subscription( Path, - f"/paf/{self.role_name}/trajectory", + f"/paf/{self.role_name}/trajectory_dummy", self.__set_trajectory, qos_profile=1) @@ -66,6 +66,14 @@ def __init__(self): Float32, f"/paf/{self.role_name}/acc_velocity", qos_profile=1) + self.wp_publisher: Publisher = self.new_publisher( + Float32, + f"/paf/{self.role_name}/current_wp", + qos_profile=1) + self.speed_limit_publisher: Publisher = self.new_publisher( + Float32, + f"/paf/{self.role_name}/speed_limit", + qos_profile=1) # List of all speed limits, sorted by waypoint index self.__speed_limits_OD: [float] = [] @@ -148,8 +156,10 @@ def __current_position_callback(self, data: PoseStamped): if d_new < d_old: # update current waypoint and corresponding speed limit self.__current_wp_index += 1 + self.wp_publisher.publish(self.__current_wp_index) self.speed_limit = \ self.__speed_limits_OD[self.__current_wp_index] + self.speed_limit_publisher.publish(self.speed_limit) def run(self): """ diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index b0980269..821367df 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -1,5 +1,4 @@ #!/usr/bin/env python -# import rospy # import tf.transformations import ros_compatibility as roscomp import rospy @@ -10,8 +9,7 @@ from std_msgs.msg import String, Float32, Bool from nav_msgs.msg import Path from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion - -import carla +from carla_msgs.msg import CarlaSpeedometer import numpy as np import math @@ -24,15 +22,17 @@ from scipy.spatial.transform import Rotation + def convert_to_ms(speed): return speed / 3.6 -def approx_obstacle_pos(distance: float, heading: float, ego_pos: np.array): +def approx_obstacle_pos(distance: float, heading: float, ego_pos: np.array, speed: float): """calculate the position of the obstacle in the global coordinate system based on ego position, heading and distance Args: + speed (float): Speed of the ego vehicle distance (float): Distance to the obstacle heading (float): Ego vehivle heading ego_pos (np.array): Position in [x, y, z] @@ -44,13 +44,20 @@ def approx_obstacle_pos(distance: float, heading: float, ego_pos: np.array): # Create distance vector with 0 rotation relative_position_local = np.array([distance, 0, 0]) - + + # speed vector + speed_vector = rotation_matrix.apply(np.array([speed, 0, 0])) # Rotate distance vector to match heading absolute_position_local = rotation_matrix.apply(relative_position_local) # Add egomposition vector with distance vetor to get absolute position - vehicle_position_global = ego_pos + absolute_position_local - return vehicle_position_global + vehicle_position_global_start = ego_pos + absolute_position_local + + length = np.array([3, 0, 0]) + length_vector = rotation_matrix.apply(length) + vehicle_position_global_end = vehicle_position_global_start + length_vector + + return vehicle_position_global_start, vehicle_position_global_end, speed_vector class MotionPlanning(CompatibleNode): @@ -64,14 +71,34 @@ class MotionPlanning(CompatibleNode): def __init__(self): super(MotionPlanning, self).__init__('MotionPlanning') self.role_name = self.get_param("role_name", "hero") - self.control_loop_rate = self.get_param("control_loop_rate", 0.5) + self.control_loop_rate = self.get_param("control_loop_rate", 0.3) self.target_speed = 0.0 self.__curr_behavior = None self.__acc_speed = 0.0 self.__stopline = None # (Distance, isStopline) self.__change_point = None # (Distance, isLaneChange, roadOption) + self.published = False + self.current_pos = None + self.current_heading = None + self.trajectory = None + self.overtaking = False + self.overtake_start = rospy.get_rostime() + self.current_wp = None + self.enhanced_path = None + self.current_speed = None + self.speed_limit = None # Subscriber + self.speed_limit_sub = self.new_subscription( + Float32, + f"/paf/{self.role_name}/speed_limit", + self.__set_speed_limit, + qos_profile=1) + self.velocity_sub: Subscriber = self.new_subscription( + CarlaSpeedometer, + f"/carla/{self.role_name}/Speed", + self.__get_current_velocity, + qos_profile=1) self.head_sub = self.new_subscription( Float32, f"/paf/{self.role_name}/current_heading", @@ -130,6 +157,11 @@ def __init__(self): f"/paf/{self.role_name}/target_velocity", qos_profile=1) + self.wp_subs = self.new_subscription( + Float32, + f"/paf/{self.role_name}/current_wp", + self.__set_wp, + qos_profile=1) # Publisher for emergency stop self.emergency_pub = self.new_publisher( Bool, @@ -137,12 +169,30 @@ def __init__(self): qos_profile=1) self.logdebug("MotionPlanning started") - self.published = False - self.current_pos = None - self.current_heading = None - self.trajectory = None - self.overtaking = False - self.overtake_start = None + + def __set_speed_limit(self, data: Float32): + """Set current speed limit + + Args: + data (Float32): Current speed limit + """ + self.speed_limit = data.data + + def __get_current_velocity(self, data: CarlaSpeedometer): + """Get current velocity from CarlaSpeedometer + + Args: + data (CarlaSpeedometer): Current velocity + """ + self.current_speed = float(data.speed) + + def __set_wp(self, data: Float32): + """Recieve current waypoint index from ACC + + Args: + data (Float32): Waypoint index + """ + self.current_wp = data.data def __set_heading(self, data: Float32): """Set current Heading @@ -184,42 +234,79 @@ def __set_current_pos(self, data: PoseStamped): data.pose.position.z]) def change_trajectory(self, data: Bool): + import carla + import os + CARLA_HOST = os.environ.get('CARLA_HOST', 'paf23-carla-simulator-1') + CARLA_PORT = int(os.environ.get('CARLA_PORT', '2000')) + + + client = carla.Client(CARLA_HOST, CARLA_PORT) + + world = client.get_world() + world.wait_for_tick() + + + blueprint_library = world.get_blueprint_library() + # bp = blueprint_library.filter('vehicle.*')[0] + # vehicle = world.spawn_actor(bp, world.get_map().get_spawn_points()[0]) + bp = blueprint_library.filter("model3")[0] + for actor in world.get_actors(): + if actor.attributes.get('role_name') == "hero": + ego_vehicle = actor + break + + spawnPoint = carla.Transform(ego_vehicle.get_location() + carla.Location(y=25), ego_vehicle.get_transform().rotation) + vehicle = world.spawn_actor(bp, spawnPoint) + + vehicle.set_autopilot(False) + # vehicle.set_location(loc) + self.logerr("spawned vehicle: " + str(vehicle.get_location())) + # coords = vehicle.get_location() + # get spectator + spectator = world.get_spectator() + # set spectator to follow ego vehicle with offset + spectator.set_transform( + carla.Transform(ego_vehicle.get_location() + carla.Location(z=50), + carla.Rotation(pitch=-90))) + print(spectator.get_location()) + self.overtaking = True self.overtake_start = rospy.get_rostime() - index_car = 20 - limit_waypoints = 100 + # index_car = 20 + limit_waypoints = 60 data = self.trajectory self.logerr("Trajectory chagen started") np_array = np.array(data.poses) - obstacle_position = approx_obstacle_pos(20, self.current_heading, self.current_pos) - selection = np_array[:limit_waypoints] + + obstacle_position = approx_obstacle_pos(20, self.current_heading, self.current_pos, self.current_speed-5) + self.logerr("obstacle position " + str(obstacle_position)) + selection = np_array[int(self.current_wp):int(self.current_wp + limit_waypoints)] waypoints = self.convert_pose_to_array(selection) self.logerr("waypoints " + str(waypoints)) - obs = np.array([[waypoints[index_car][0]-0.5, waypoints[index_car][1], waypoints[index_car][0]+0.5, waypoints[index_car][1]-2]]) - self.logerr("obs " + str(obs)) - pos_lat_lon = self._location_to_gps(0,0, waypoints[0][0], waypoints[0][1]) - self.logerr("Distance: 40, Heading: " + str(self.current_heading) + ", Position: " + str(self.current_pos) + "\napprox position: " + str(approx_obstacle_pos(40, self.current_heading, self.current_pos))) + # obs = np.array([[coords.x, coords.y, coords.x, coords.y-3]]) + # self.logerr("obs " + str(obs)) + pos_lat_lon = self._location_to_gps(0,0, self.current_pos[0], self.current_pos[1]) initial_conditions = { - 'ps': pos_lat_lon["lon"], - 'target_speed': 11, - 'pos': waypoints[0], - 'vel': np.array([5, 1]), + 'ps': pos_lat_lon['lon'], + 'target_speed': self.target_speed, + 'pos': np.array([self.current_pos[0], self.current_pos[1]]), + 'vel': obstacle_position[2][:2], 'wp': waypoints, - 'obs': obs + 'obs': np.array([[obstacle_position[0][0], obstacle_position[0][1], obstacle_position[1][0], obstacle_position[1][1]]]) } hyperparameters = { - "max_speed": 25.0, - "max_accel": 15.0, - "max_curvature": 20.0, - "max_road_width_l": 1.5, - "max_road_width_r": 0.5, - "d_road_w": 0.5, + "max_speed": self.speed_limit, + "max_accel": 4.0, + "max_curvature": 30.0, + "max_road_width_l": 0.1, + "max_road_width_r": 3.5, + "d_road_w": 0.2, "dt": 0.2, - "maxt": 20.0, + "maxt": 17.0, "mint": 6.0, "d_t_s": 0.5, "n_s_sample": 2.0, - "obstacle_clearance": 0.1, + "obstacle_clearance": 1.5, "kd": 1.0, "kv": 0.1, "ka": 0.1, @@ -228,16 +315,14 @@ def change_trajectory(self, data: Bool): "ko": 0.1, "klat": 1.0, "klon": 1.0, - "num_threads": 0, # set 0 to avoid using threaded algorithm + "num_threads": 1, # set 0 to avoid using threaded algorithm } result_x, result_y, speeds, ix, iy, iyaw, d, s, speeds_x, \ speeds_y, misc, costs, success = run_fot(initial_conditions, - hyperparameters) + hyperparameters) + self.logerr("Success: " + str(success)) if success: - print("Success!") - print("result_x: ", result_x) - print("result_y: ", result_y) - self.logerr("result yaw: " + str(iyaw)) + self.logerr("Success") result = [] for i in range(len(result_x)-1): position = Point(result_x[i], result_y[i], 0) @@ -248,16 +333,15 @@ def change_trajectory(self, data: Bool): z=quaternion[2], w=quaternion[3]) pose = Pose(position, orientation) pos = PoseStamped() - pos.header.stamp = rospy.Time.now() pos.header.frame_id = "global" pos.pose = pose result.append(pos) path = Path() path.header.stamp = rospy.Time.now() path.header.frame_id = "global" - path.poses = result + list(np_array[limit_waypoints:]) - self.logerr(path) - self.traj_pub.publish(path) + path.poses = list(np_array[:int(self.current_wp)]) + result + list(np_array[int(self.current_wp + limit_waypoints):]) + self.enhanced_path = path + self.logerr("Trajectory change finished") def __set_trajectory(self, data: Path): """get current trajectory global planning @@ -266,8 +350,7 @@ def __set_trajectory(self, data: Path): data (Path): Trajectory waypoints """ self.trajectory = data - if not self.overtaking: - self.traj_pub.publish(data) + self.logerr("Trajectory recieved: " + str(self.trajectory)) def convert_pose_to_array(self, poses: np.array): """convert pose array to numpy array @@ -413,12 +496,26 @@ def run(self): """ def loop(timer_event=None): - self.update_target_speed(self.__acc_speed, self.__curr_behavior) - if self.overtake_start is None: + if self.trajectory is None or self.__acc_speed is None or \ + self.__curr_behavior is None: return - if rospy.get_rostime() - self.overtake_start < rospy.Duration(10): - self.logerr("overtake finished") - self.overtaking = False + + # self.update_target_speed(self.__acc_speed, self.__curr_behavior) + # self.velocity_pub.publish(10) + + # if self.overtaking and rospy.get_rostime() - self.overtake_start < rospy.Duration(30): + # self.logerr("overtake finished") + # self.overtaking = False + + # if len(self.trajectory.poses) < 1: + # self.loginfo("No trajectory recieved") + # return + if self.overtaking and self.enhanced_path is not None: + self.enhanced_path.header.stamp = rospy.Time.now() + self.traj_pub.publish(self.enhanced_path) + else: + self.trajectory.header.stamp = rospy.Time.now() + self.traj_pub.publish(self.trajectory) self.new_timer(self.control_loop_rate, loop) self.spin() @@ -430,7 +527,6 @@ def loop(timer_event=None): :param args: """ roscomp.init('MotionPlanning') - try: node = MotionPlanning() node.run() diff --git a/code/planning/src/local_planner/objects.json b/code/planning/src/local_planner/objects.json new file mode 100644 index 00000000..bdc96273 --- /dev/null +++ b/code/planning/src/local_planner/objects.json @@ -0,0 +1,11 @@ +{ + "objects": + [ + { + "type": "vehicle.*", + "id": "dummy", + "spawn_point": {"x": 983.59, "y": -5450.01, "z": 703.810118745277, "roll": 0.0, "pitch": 0.0, "yaw": -1.4} + } + ] + } + \ No newline at end of file diff --git a/code/planning/src/local_planner/spawn_vehicle.py b/code/planning/src/local_planner/spawn_vehicle.py index efebd901..552145be 100644 --- a/code/planning/src/local_planner/spawn_vehicle.py +++ b/code/planning/src/local_planner/spawn_vehicle.py @@ -6,6 +6,7 @@ CARLA_HOST = os.environ.get('CARLA_HOST', 'paf23-carla-simulator-1') CARLA_PORT = int(os.environ.get('CARLA_PORT', '2000')) + client = carla.Client(CARLA_HOST, CARLA_PORT) world = client.get_world() @@ -16,23 +17,22 @@ # bp = blueprint_library.filter('vehicle.*')[0] # vehicle = world.spawn_actor(bp, world.get_map().get_spawn_points()[0]) bp = blueprint_library.filter("model3")[0] -world = client.get_world() -loc = carla.Location(x=983.59, y=-5450.01, z=703.810118745277) -print("location: " + str(loc)) -spawnPoint = carla.Transform(loc, carla.Rotation(pitch=0.0, yaw=0.0, roll=0.0)) -print("spawnPoint: " + str(spawnPoint)) +for actor in world.get_actors(): + if actor.attributes.get('role_name') == "hero": + ego_vehicle = actor + break + +spawnPoint = carla.Transform(ego_vehicle.get_location() + carla.Location(y=25) + carla.Location(x=-1), ego_vehicle.get_transform().rotation) vehicle = world.spawn_actor(bp, spawnPoint) -print("spawned vehicle: " + str(vehicle.get_location())) + vehicle.set_autopilot(False) -vehicle.set_location(loc) +# vehicle.set_location(loc) print("spawned vehicle: " + str(vehicle.get_location())) - # get spectator spectator = world.get_spectator() # set spectator to follow ego vehicle with offset spectator.set_transform( - carla.Transform(loc + carla.Location(z=50), + carla.Transform(ego_vehicle.get_location() + carla.Location(z=50), carla.Rotation(pitch=-90))) -print(spectator.get_transform()) -client.reload_world() +print(spectator.get_location()) \ No newline at end of file From 4db52c09150cb815fd857140752d61ad7c501781 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Sat, 27 Jan 2024 17:34:38 +0100 Subject: [PATCH 13/19] feat: test data --- .../src/local_planner/motion_planning.py | 191 ++++++------------ code/planning/src/local_planner/test.txt | 68 +++++++ code/planning/src/local_planner/utils.py | 99 +++++++++ 3 files changed, 233 insertions(+), 125 deletions(-) create mode 100644 code/planning/src/local_planner/test.txt create mode 100644 code/planning/src/local_planner/utils.py diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index 821367df..9332072e 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -19,45 +19,11 @@ from perception.msg import Waypoint, LaneChange import planning from behavior_agent.behaviours import behavior_speed as bs -from scipy.spatial.transform import Rotation +from utils import convert_to_ms, approx_obstacle_pos, \ + hyperparameters, location_to_gps - -def convert_to_ms(speed): - return speed / 3.6 - - -def approx_obstacle_pos(distance: float, heading: float, ego_pos: np.array, speed: float): - """calculate the position of the obstacle in the global coordinate system - based on ego position, heading and distance - - Args: - speed (float): Speed of the ego vehicle - distance (float): Distance to the obstacle - heading (float): Ego vehivle heading - ego_pos (np.array): Position in [x, y, z] - - Returns: - np.array: approximated position of the obstacle - """ - rotation_matrix = Rotation.from_euler('z', heading) - - # Create distance vector with 0 rotation - relative_position_local = np.array([distance, 0, 0]) - - # speed vector - speed_vector = rotation_matrix.apply(np.array([speed, 0, 0])) - # Rotate distance vector to match heading - absolute_position_local = rotation_matrix.apply(relative_position_local) - - # Add egomposition vector with distance vetor to get absolute position - vehicle_position_global_start = ego_pos + absolute_position_local - - length = np.array([3, 0, 0]) - length_vector = rotation_matrix.apply(length) - vehicle_position_global_end = vehicle_position_global_start + length_vector - - return vehicle_position_global_start, vehicle_position_global_end, speed_vector +import matplotlib.pyplot as plt class MotionPlanning(CompatibleNode): @@ -202,28 +168,6 @@ def __set_heading(self, data: Float32): """ self.current_heading = data.data - def _location_to_gps(self, lat_ref, lon_ref, x, y): - """ - Convert from world coordinates to GPS coordinates - :param lat_ref: latitude reference for the current map - :param lon_ref: longitude reference for the current map - :param location: location to translate - :return: dictionary with lat, lon and height - """ - - EARTH_RADIUS_EQUA = 6378137.0 # pylint: disable=invalid-name - scale = math.cos(lat_ref * math.pi / 180.0) - mx = scale * lon_ref * math.pi * EARTH_RADIUS_EQUA / 180.0 - my = scale * EARTH_RADIUS_EQUA * math.log(math.tan((90.0 + lat_ref) * math.pi / 360.0)) - mx += x - my -= y - - lon = mx * 180.0 / (math.pi * EARTH_RADIUS_EQUA * scale) - lat = 360.0 * math.atan(math.exp(my / (EARTH_RADIUS_EQUA * scale))) / math.pi - 90.0 - z = 703 - - return {'lat': lat, 'lon': lon, 'z': z} - def __set_current_pos(self, data: PoseStamped): """set current position Args: @@ -234,97 +178,92 @@ def __set_current_pos(self, data: PoseStamped): data.pose.position.z]) def change_trajectory(self, data: Bool): - import carla - import os - CARLA_HOST = os.environ.get('CARLA_HOST', 'paf23-carla-simulator-1') - CARLA_PORT = int(os.environ.get('CARLA_PORT', '2000')) - - - client = carla.Client(CARLA_HOST, CARLA_PORT) - - world = client.get_world() - world.wait_for_tick() - - - blueprint_library = world.get_blueprint_library() - # bp = blueprint_library.filter('vehicle.*')[0] - # vehicle = world.spawn_actor(bp, world.get_map().get_spawn_points()[0]) - bp = blueprint_library.filter("model3")[0] - for actor in world.get_actors(): - if actor.attributes.get('role_name') == "hero": - ego_vehicle = actor - break - - spawnPoint = carla.Transform(ego_vehicle.get_location() + carla.Location(y=25), ego_vehicle.get_transform().rotation) - vehicle = world.spawn_actor(bp, spawnPoint) - - vehicle.set_autopilot(False) - # vehicle.set_location(loc) - self.logerr("spawned vehicle: " + str(vehicle.get_location())) - # coords = vehicle.get_location() - # get spectator - spectator = world.get_spectator() - # set spectator to follow ego vehicle with offset - spectator.set_transform( - carla.Transform(ego_vehicle.get_location() + carla.Location(z=50), - carla.Rotation(pitch=-90))) - print(spectator.get_location()) + # import carla + # import os + # CARLA_HOST = os.environ.get('CARLA_HOST', 'paf23-carla-simulator-1') + # CARLA_PORT = int(os.environ.get('CARLA_PORT', '2000')) + + + # client = carla.Client(CARLA_HOST, CARLA_PORT) + + # world = client.get_world() + # world.wait_for_tick() + + + # blueprint_library = world.get_blueprint_library() + # # bp = blueprint_library.filter('vehicle.*')[0] + # # vehicle = world.spawn_actor(bp, world.get_map().get_spawn_points()[0]) + # bp = blueprint_library.filter("model3")[0] + # for actor in world.get_actors(): + # if actor.attributes.get('role_name') == "hero": + # ego_vehicle = actor + # break + + # spawnPoint = carla.Transform(ego_vehicle.get_location() + carla.Location(y=25), ego_vehicle.get_transform().rotation) + # vehicle = world.spawn_actor(bp, spawnPoint) + + # vehicle.set_autopilot(False) + # # vehicle.set_location(loc) + # self.logerr("spawned vehicle: " + str(vehicle.get_location())) + # # coords = vehicle.get_location() + # # get spectator + # spectator = world.get_spectator() + # # set spectator to follow ego vehicle with offset + # spectator.set_transform( + # carla.Transform(ego_vehicle.get_location() + carla.Location(z=50), + # carla.Rotation(pitch=-90))) + # print(spectator.get_location()) self.overtaking = True self.overtake_start = rospy.get_rostime() # index_car = 20 - limit_waypoints = 60 + limit_waypoints = 100 data = self.trajectory self.logerr("Trajectory chagen started") np_array = np.array(data.poses) - - obstacle_position = approx_obstacle_pos(20, self.current_heading, self.current_pos, self.current_speed-5) + + obstacle_position = approx_obstacle_pos(20, self.current_heading, self.current_pos, self.current_speed) self.logerr("obstacle position " + str(obstacle_position)) + self.logerr("current position " + str(self.current_pos)) + self.logerr("current heading " + str(self.current_heading)) + self.logerr("current waypoint " + str(self.current_wp)) selection = np_array[int(self.current_wp):int(self.current_wp + limit_waypoints)] waypoints = self.convert_pose_to_array(selection) + trajectory_np = self.convert_pose_to_array(np_array) + np.save("trajectory.npy", trajectory_np) self.logerr("waypoints " + str(waypoints)) # obs = np.array([[coords.x, coords.y, coords.x, coords.y-3]]) # self.logerr("obs " + str(obs)) - pos_lat_lon = self._location_to_gps(0,0, self.current_pos[0], self.current_pos[1]) + pos_lat_lon = location_to_gps(0,0, self.current_pos[0], self.current_pos[1]) initial_conditions = { 'ps': pos_lat_lon['lon'], - 'target_speed': self.target_speed, + 'target_speed': self.current_speed, 'pos': np.array([self.current_pos[0], self.current_pos[1]]), 'vel': obstacle_position[2][:2], 'wp': waypoints, 'obs': np.array([[obstacle_position[0][0], obstacle_position[0][1], obstacle_position[1][0], obstacle_position[1][1]]]) } - hyperparameters = { - "max_speed": self.speed_limit, - "max_accel": 4.0, - "max_curvature": 30.0, - "max_road_width_l": 0.1, - "max_road_width_r": 3.5, - "d_road_w": 0.2, - "dt": 0.2, - "maxt": 17.0, - "mint": 6.0, - "d_t_s": 0.5, - "n_s_sample": 2.0, - "obstacle_clearance": 1.5, - "kd": 1.0, - "kv": 0.1, - "ka": 0.1, - "kj": 0.1, - "kt": 0.1, - "ko": 0.1, - "klat": 1.0, - "klon": 1.0, - "num_threads": 1, # set 0 to avoid using threaded algorithm - } + result_x, result_y, speeds, ix, iy, iyaw, d, s, speeds_x, \ speeds_y, misc, costs, success = run_fot(initial_conditions, hyperparameters) - self.logerr("Success: " + str(success)) + self.logerr("success: " + str(success)) + self.logerr("result_x: " + str(result_x)) + self.logerr("result_y: " + str(result_y)) + fig, ax = plt.subplots(1, 2) + + ax[0].scatter(waypoints[:, 0], waypoints[:, 1], label="original", color = "blue") + ax[0].scatter([obstacle_position[0][ 0], obstacle_position[0][1]], + [obstacle_position[1][0], obstacle_position[1][1]], label="object", color="red") + ax[1].scatter(result_x, result_y, label="frenet", color="green") + ax[1].scatter([obstacle_position[0][0], obstacle_position[0][1]], + [obstacle_position[1][0], obstacle_position[1][1]], label="object", color="red") + plt.legend() + plt.show() if success: self.logerr("Success") result = [] - for i in range(len(result_x)-1): + for i in range(len(result_x)): position = Point(result_x[i], result_y[i], 0) quaternion = tf.transformations.quaternion_from_euler(0, 0, @@ -339,7 +278,9 @@ def change_trajectory(self, data: Bool): path = Path() path.header.stamp = rospy.Time.now() path.header.frame_id = "global" + self.logerr("result: " + str(result)) path.poses = list(np_array[:int(self.current_wp)]) + result + list(np_array[int(self.current_wp + limit_waypoints):]) + self.logerr("Changed traj: " + str(path.poses)) self.enhanced_path = path self.logerr("Trajectory change finished") @@ -499,7 +440,7 @@ def loop(timer_event=None): if self.trajectory is None or self.__acc_speed is None or \ self.__curr_behavior is None: return - + self.velocity_pub.publish(8) # self.update_target_speed(self.__acc_speed, self.__curr_behavior) # self.velocity_pub.publish(10) diff --git a/code/planning/src/local_planner/test.txt b/code/planning/src/local_planner/test.txt new file mode 100644 index 00000000..42d24e4c --- /dev/null +++ b/code/planning/src/local_planner/test.txt @@ -0,0 +1,68 @@ +obstacle position (array([ 980.96230118, -5476.51898298, 703.77912124]), array([ 979.05757394, -5479.27978253, 703.77912124]), array([-0.23368553, -1.65490565, 0. ])) + +current position [ 983.71518498 -5451.65969637 703.77912124] + +current heading -1.7110766172409058 + +distance: 20 + +[[ 983.4673446 -5452.11599828] + [ 983.46555604 -5453.11746727] + [ 983.46376748 -5454.11893626] + [ 983.46197892 -5455.12040525] + [ 983.46019036 -5456.12187424] + [ 983.4584018 -5457.12334323] + [ 983.45661324 -5458.12481222] + [ 983.45482468 -5459.12628121] + [ 983.45303612 -5460.12775021] + [ 983.45124755 -5461.1292192 ] + [ 983.44945899 -5462.13068819] + [ 983.44767043 -5463.13215718] + [ 983.44588187 -5464.13362617] + [ 983.44409331 -5465.13509516] + [ 983.44230475 -5466.13656415] + [ 983.44051619 -5467.13803314] + [ 983.43872763 -5468.13950213] + [ 983.43693907 -5469.14097113] + [ 983.43515051 -5470.14244012] + [ 983.43336195 -5471.14390911] + [ 983.43157339 -5472.1453781 ] + [ 983.42978483 -5473.14684709] + [ 983.42799627 -5474.14831608] + [ 983.42620771 -5475.14978507] + [ 983.42441915 -5476.15125406] + [ 983.42263059 -5477.15272305] + [ 983.42084203 -5478.15419205] + [ 983.41905347 -5479.15566104] + [ 983.41726491 -5480.15713003] + [ 983.41547635 -5481.15859902] + [ 983.41368779 -5482.16006801] + [ 983.41189923 -5483.161537 ] + [ 983.41011067 -5484.16300599] + [ 983.40832211 -5485.16447498] + [ 983.40653355 -5486.16594397] + [ 983.40474499 -5487.16741297] + [ 983.40295643 -5488.16888196] + [ 983.40116787 -5489.17035095] + [ 983.39937931 -5490.17181994] + [ 983.39759075 -5491.17328893] + [ 983.39580219 -5492.17475792] + [ 983.39401363 -5493.17622691] + [ 983.39222507 -5494.1776959 ] + [ 983.39043651 -5495.17916489] + [ 983.38864795 -5496.18063389] + [ 983.38685939 -5497.18210288] + [ 983.38507082 -5498.18357187] + [ 983.38328226 -5499.18504086] + [ 983.3814937 -5500.18650985] + [ 983.37970514 -5501.18797884] + [ 983.37791658 -5502.18944783] + [ 983.37612802 -5503.19091682] + [ 983.37433946 -5504.19238581] + [ 983.3725509 -5505.19385481] + [ 983.37076234 -5506.1953238 ] + [ 983.36897378 -5507.19679279] + [ 983.36718522 -5508.19826178] + [ 983.36539666 -5509.19973077] + [ 983.3636081 -5510.20119976] + [ 983.36181954 -5511.20266875]] \ No newline at end of file diff --git a/code/planning/src/local_planner/utils.py b/code/planning/src/local_planner/utils.py new file mode 100644 index 00000000..e9de4e6d --- /dev/null +++ b/code/planning/src/local_planner/utils.py @@ -0,0 +1,99 @@ +from scipy.spatial.transform import Rotation +import numpy as np +import math + +hyperparameters = { + "max_speed": 15, + "max_accel": 4.0, + "max_curvature": 30.0, + "max_road_width_l": 0.0, + "max_road_width_r": 3.5, + "d_road_w": 0.2, + "dt": 0.2, + "maxt": 17.0, + "mint": 6.0, + "d_t_s": 0.5, + "n_s_sample": 2.0, + "obstacle_clearance": 1, + "kd": 1.0, + "kv": 0.1, + "ka": 0.1, + "kj": 0.1, + "kt": 0.1, + "ko": 0.1, + "klat": 1.0, + "klon": 1.0, + "num_threads": 1, # set 0 to avoid using threaded algorithm +} + + +def location_to_gps(lat_ref, lon_ref, x, y): + """ + Convert from world coordinates to GPS coordinates + :param lat_ref: latitude reference for the current map + :param lon_ref: longitude reference for the current map + :param location: location to translate + :return: dictionary with lat, lon and height + """ + + EARTH_RADIUS_EQUA = 6378137.0 # pylint: disable=invalid-name + scale = math.cos(lat_ref * math.pi / 180.0) + mx = scale * lon_ref * math.pi * EARTH_RADIUS_EQUA / 180.0 + my = scale * EARTH_RADIUS_EQUA * math.log(math.tan((90.0 + lat_ref) * + math.pi / 360.0)) + mx += x + my -= y + + lon = mx * 180.0 / (math.pi * EARTH_RADIUS_EQUA * scale) + lat = 360.0 * math.atan(math.exp(my / (EARTH_RADIUS_EQUA * scale))) / math.pi - 90.0 + z = 703 + + return {'lat': lat, 'lon': lon, 'z': z} + + +def approx_obstacle_pos(distance: float, heading: float, + ego_pos: np.array, speed: float): + """calculate the position of the obstacle in the global coordinate system + based on ego position, heading and distance + + Args: + speed (float): Speed of the ego vehicle + distance (float): Distance to the obstacle + heading (float): Ego vehivle heading + ego_pos (np.array): Position in [x, y, z] + + Returns: + np.array: approximated position of the obstacle + """ + rotation_matrix = Rotation.from_euler('z', heading) + + # Create distance vector with 0 rotation + relative_position_local = np.array([distance, 0, 0]) + + # speed vector + speed_vector = rotation_matrix.apply(np.array([speed, 0, 0])) + # Rotate distance vector to match heading + absolute_position_local = rotation_matrix.apply(relative_position_local) + + # Add egomposition vector with distance vetor to get absolute position + vehicle_position_global_start = ego_pos + absolute_position_local + + length = np.array([3, 0, 0]) + length_vector = rotation_matrix.apply(length) + + offset = np.array([0.75, 0, 0]) + rotation_adjusted = Rotation.from_euler('z', heading + math.radians(90)) + offset_front = rotation_adjusted.apply(offset) + + rotation_adjusted = Rotation.from_euler('z', heading + math.radians(270)) + offset_back = rotation_adjusted.apply(offset) + + vehicle_position_global_end = vehicle_position_global_start + \ + length_vector + offset_back + + return vehicle_position_global_start + offset_front, \ + vehicle_position_global_end, speed_vector + + +def convert_to_ms(speed): + return speed / 3.6 From c65ca59e45a150877eab8f8719d508f9cb0d7045 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Sun, 28 Jan 2024 22:58:52 +0100 Subject: [PATCH 14/19] fix: create test video --- code/Default.perspective | 229 ------------------ .../src/local_planner/motion_planning.py | 110 ++++----- code/planning/src/local_planner/objects.json | 11 - .../src/local_planner/spawn_vehicle.py | 38 --- code/planning/src/local_planner/test.txt | 68 ------ .../src/local_planner/test_approx_pos.py | 51 ---- code/planning/src/local_planner/test_traj.py | 68 ------ code/planning/src/local_planner/utils.py | 10 +- 8 files changed, 58 insertions(+), 527 deletions(-) delete mode 100644 code/Default.perspective delete mode 100644 code/planning/src/local_planner/objects.json delete mode 100644 code/planning/src/local_planner/spawn_vehicle.py delete mode 100644 code/planning/src/local_planner/test.txt delete mode 100644 code/planning/src/local_planner/test_approx_pos.py delete mode 100644 code/planning/src/local_planner/test_traj.py diff --git a/code/Default.perspective b/code/Default.perspective deleted file mode 100644 index 13ed1903..00000000 --- a/code/Default.perspective +++ /dev/null @@ -1,229 +0,0 @@ -{ - "keys": {}, - "groups": { - "mainwindow": { - "keys": { - "geometry": { - "repr(QByteArray.hex)": "QtCore.QByteArray(b'01d9d0cb00030000000000900000003600000eff0000086f000000900000008000000eff0000086f00000000000000000f00000000900000008000000eff0000086f')", - "type": "repr(QByteArray.hex)", - "pretty-print": " 6 o o o" - }, - "state": { - "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff00000000fd000000010000000300000e70000007acfc0100000003fb00000058007200710074005f007000750062006c00690073006800650072005f005f005000750062006c00690073006800650072005f005f0031005f005f005000750062006c0069007300680065007200570069006400670065007401000000000000075e0000034100fffffffb0000004c007200710074005f0063006f006e0073006f006c0065005f005f0043006f006e0073006f006c0065005f005f0031005f005f0043006f006e0073006f006c0065005700690064006700650074010000076a000004cd000002ca00fffffffb0000004c007200710074005f0074006f007000690063005f005f0054006f0070006900630050006c007500670069006e005f005f0031005f005f0054006f0070006900630057006900640067006500740100000c430000022d0000011c00ffffff00000e700000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')", - "type": "repr(QByteArray.hex)", - "pretty-print": " Xrqt_publisher__Publisher__1__PublisherWidget Lrqt_console__Console__1__ConsoleWidget Lrqt_topic__TopicPlugin__1__TopicWidget 6MinimizedDockWidgetsToolbar " - } - }, - "groups": { - "toolbar_areas": { - "keys": { - "MinimizedDockWidgetsToolbar": { - "repr": "8", - "type": "repr" - } - }, - "groups": {} - } - } - }, - "pluginmanager": { - "keys": { - "running-plugins": { - "repr": "{'rqt_console/Console': [1], 'rqt_publisher/Publisher': [1], 'rqt_topic/TopicPlugin': [1]}", - "type": "repr" - } - }, - "groups": { - "plugin__rqt_console__Console__1": { - "keys": {}, - "groups": { - "dock_widget__ConsoleWidget": { - "keys": { - "dock_widget_title": { - "repr": "'Console'", - "type": "repr" - }, - "dockable": { - "repr": "True", - "type": "repr" - }, - "parent": { - "repr": "None", - "type": "repr" - } - }, - "groups": {} - }, - "plugin": { - "keys": { - "exclude_filters": { - "repr": "['severity', 'message', 'message']", - "type": "repr" - }, - "filter_splitter": { - "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff0000000100000002000000d4000000d40100000009010000000200')", - "type": "repr(QByteArray.hex)", - "pretty-print": " " - }, - "highlight_filters": { - "repr": "'message'", - "type": "repr" - }, - "message_limit": { - "repr": "20000", - "type": "repr" - }, - "paused": { - "repr": "False", - "type": "repr" - }, - "settings_exist": { - "repr": "True", - "type": "repr" - }, - "show_highlighted_only": { - "repr": "False", - "type": "repr" - }, - "table_splitter": { - "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff00000001000000020000008a000001b10100000009010000000200')", - "type": "repr(QByteArray.hex)", - "pretty-print": " " - } - }, - "groups": { - "exclude_filter_0": { - "keys": { - "enabled": { - "repr": "True", - "type": "repr" - }, - "itemlist": { - "repr": "['Warn', 'Info']", - "type": "repr" - } - }, - "groups": {} - }, - "exclude_filter_1": { - "keys": { - "enabled": { - "repr": "True", - "type": "repr" - }, - "regex": { - "repr": "False", - "type": "repr" - }, - "text": { - "repr": "'Lidar'", - "type": "repr" - } - }, - "groups": {} - }, - "exclude_filter_2": { - "keys": { - "enabled": { - "repr": "True", - "type": "repr" - }, - "regex": { - "repr": "False", - "type": "repr" - }, - "text": { - "repr": "'Vision'", - "type": "repr" - } - }, - "groups": {} - }, - "highlight_filter_0": { - "keys": { - "enabled": { - "repr": "True", - "type": "repr" - }, - "regex": { - "repr": "False", - "type": "repr" - }, - "text": { - "repr": "''", - "type": "repr" - } - }, - "groups": {} - } - } - } - } - }, - "plugin__rqt_publisher__Publisher__1": { - "keys": {}, - "groups": { - "dock_widget__PublisherWidget": { - "keys": { - "dock_widget_title": { - "repr": "'Message Publisher'", - "type": "repr" - }, - "dockable": { - "repr": "True", - "type": "repr" - }, - "parent": { - "repr": "None", - "type": "repr" - } - }, - "groups": {} - }, - "plugin": { - "keys": { - "publishers": { - "repr": "\"[{'topic_name': '/paf/hero/test', 'type_name': 'std_msgs/Bool', 'rate': 1.0, 'enabled': False, 'publisher_id': 0, 'counter': 0, 'expressions': {}}]\"", - "type": "repr" - } - }, - "groups": {} - } - } - }, - "plugin__rqt_topic__TopicPlugin__1": { - "keys": {}, - "groups": { - "dock_widget__TopicWidget": { - "keys": { - "dock_widget_title": { - "repr": "'Topic Monitor'", - "type": "repr" - }, - "dockable": { - "repr": "True", - "type": "repr" - }, - "parent": { - "repr": "None", - "type": "repr" - } - }, - "groups": {} - }, - "plugin": { - "keys": { - "tree_widget_header_state": { - "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff0000000000000001000000000000000001000000000000000000000000000000000000064c0000000501010001000000000000000005000000c8ffffffff0000008100000003000000050000023d0000000100000003000002310000000100000003000000c20000000100000003000000540000000100000003000000c80000000100000003000003e800000000c8')", - "type": "repr(QByteArray.hex)", - "pretty-print": " = 1 T " - } - }, - "groups": {} - } - } - } - } - } - } -} \ No newline at end of file diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index 9332072e..528543be 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -24,7 +24,7 @@ hyperparameters, location_to_gps import matplotlib.pyplot as plt - +from scipy.spatial._kdtree import KDTree class MotionPlanning(CompatibleNode): """ @@ -135,6 +135,7 @@ def __init__(self): qos_profile=1) self.logdebug("MotionPlanning started") + self.counter = 0 def __set_speed_limit(self, data: Float32): """Set current speed limit @@ -178,68 +179,71 @@ def __set_current_pos(self, data: PoseStamped): data.pose.position.z]) def change_trajectory(self, data: Bool): - # import carla - # import os - # CARLA_HOST = os.environ.get('CARLA_HOST', 'paf23-carla-simulator-1') - # CARLA_PORT = int(os.environ.get('CARLA_PORT', '2000')) - - - # client = carla.Client(CARLA_HOST, CARLA_PORT) - - # world = client.get_world() - # world.wait_for_tick() - - - # blueprint_library = world.get_blueprint_library() - # # bp = blueprint_library.filter('vehicle.*')[0] - # # vehicle = world.spawn_actor(bp, world.get_map().get_spawn_points()[0]) - # bp = blueprint_library.filter("model3")[0] - # for actor in world.get_actors(): - # if actor.attributes.get('role_name') == "hero": - # ego_vehicle = actor - # break - - # spawnPoint = carla.Transform(ego_vehicle.get_location() + carla.Location(y=25), ego_vehicle.get_transform().rotation) - # vehicle = world.spawn_actor(bp, spawnPoint) - - # vehicle.set_autopilot(False) - # # vehicle.set_location(loc) - # self.logerr("spawned vehicle: " + str(vehicle.get_location())) - # # coords = vehicle.get_location() - # # get spectator - # spectator = world.get_spectator() - # # set spectator to follow ego vehicle with offset - # spectator.set_transform( - # carla.Transform(ego_vehicle.get_location() + carla.Location(z=50), - # carla.Rotation(pitch=-90))) - # print(spectator.get_location()) - + import carla + import os + CARLA_HOST = os.environ.get('CARLA_HOST', 'paf23-carla-simulator-1') + CARLA_PORT = int(os.environ.get('CARLA_PORT', '2000')) + + + client = carla.Client(CARLA_HOST, CARLA_PORT) + + world = client.get_world() + world.wait_for_tick() + + + blueprint_library = world.get_blueprint_library() + # bp = blueprint_library.filter('vehicle.*')[0] + # vehicle = world.spawn_actor(bp, world.get_map().get_spawn_points()[0]) + bp = blueprint_library.filter("model3")[0] + for actor in world.get_actors(): + if actor.attributes.get('role_name') == "hero": + ego_vehicle = actor + break + + spawnPoint = carla.Transform(ego_vehicle.get_location() + carla.Location(y=25), ego_vehicle.get_transform().rotation) + vehicle = world.spawn_actor(bp, spawnPoint) + + vehicle.set_autopilot(False) + # vehicle.set_location(loc) + self.logerr("spawned vehicle: " + str(vehicle.get_location())) + # coords = vehicle.get_location() + # get spectator + spectator = world.get_spectator() + # set spectator to follow ego vehicle with offset + spectator.set_transform( + carla.Transform(ego_vehicle.get_location() + carla.Location(z=50), + carla.Rotation(pitch=-90))) + print(spectator.get_location()) + self.counter += 1 self.overtaking = True self.overtake_start = rospy.get_rostime() # index_car = 20 - limit_waypoints = 100 + limit_waypoints = 30 data = self.trajectory self.logerr("Trajectory chagen started") np_array = np.array(data.poses) - obstacle_position = approx_obstacle_pos(20, self.current_heading, self.current_pos, self.current_speed) + obstacle_position = approx_obstacle_pos(25, self.current_heading, self.current_pos, self.current_speed) + trajectory_np = self.convert_pose_to_array(np_array) + wp = KDTree(trajectory_np[:, :2]).query(obstacle_position[0][:2])[1] + self.logerr("waypoint index obs " + str(wp)) self.logerr("obstacle position " + str(obstacle_position)) self.logerr("current position " + str(self.current_pos)) self.logerr("current heading " + str(self.current_heading)) self.logerr("current waypoint " + str(self.current_wp)) - selection = np_array[int(self.current_wp):int(self.current_wp + limit_waypoints)] + selection = np_array[int(self.current_wp):int(self.current_wp) + 25 + 30] waypoints = self.convert_pose_to_array(selection) - trajectory_np = self.convert_pose_to_array(np_array) - np.save("trajectory.npy", trajectory_np) + + # np.save("/workspace/code/trajectory.npy", trajectory_np) self.logerr("waypoints " + str(waypoints)) - # obs = np.array([[coords.x, coords.y, coords.x, coords.y-3]]) + # obs = np.array([[coords.x, coords.y, coords.x, coords.y-3]]) [self.current_speed * math.cos(self.current_heading), self.current_speed * math.sin(self.current_heading)] # self.logerr("obs " + str(obs)) pos_lat_lon = location_to_gps(0,0, self.current_pos[0], self.current_pos[1]) initial_conditions = { - 'ps': pos_lat_lon['lon'], + 'ps': 0, 'target_speed': self.current_speed, 'pos': np.array([self.current_pos[0], self.current_pos[1]]), - 'vel': obstacle_position[2][:2], + 'vel': np.array([obstacle_position[2][0], obstacle_position[2][1]]), 'wp': waypoints, 'obs': np.array([[obstacle_position[0][0], obstacle_position[0][1], obstacle_position[1][0], obstacle_position[1][1]]]) } @@ -250,16 +254,8 @@ def change_trajectory(self, data: Bool): self.logerr("success: " + str(success)) self.logerr("result_x: " + str(result_x)) self.logerr("result_y: " + str(result_y)) - fig, ax = plt.subplots(1, 2) - - ax[0].scatter(waypoints[:, 0], waypoints[:, 1], label="original", color = "blue") - ax[0].scatter([obstacle_position[0][ 0], obstacle_position[0][1]], - [obstacle_position[1][0], obstacle_position[1][1]], label="object", color="red") - ax[1].scatter(result_x, result_y, label="frenet", color="green") - ax[1].scatter([obstacle_position[0][0], obstacle_position[0][1]], - [obstacle_position[1][0], obstacle_position[1][1]], label="object", color="red") - plt.legend() - plt.show() + # fig.savefig("/workspace/code/fot.png") + # plt.show() if success: self.logerr("Success") result = [] @@ -279,7 +275,7 @@ def change_trajectory(self, data: Bool): path.header.stamp = rospy.Time.now() path.header.frame_id = "global" self.logerr("result: " + str(result)) - path.poses = list(np_array[:int(self.current_wp)]) + result + list(np_array[int(self.current_wp + limit_waypoints):]) + path.poses = list(np_array[:int(self.current_wp)]) + result + list(np_array[int(self.current_wp + 25 + 30):]) self.logerr("Changed traj: " + str(path.poses)) self.enhanced_path = path self.logerr("Trajectory change finished") @@ -440,7 +436,7 @@ def loop(timer_event=None): if self.trajectory is None or self.__acc_speed is None or \ self.__curr_behavior is None: return - self.velocity_pub.publish(8) + self.velocity_pub.publish(5) # self.update_target_speed(self.__acc_speed, self.__curr_behavior) # self.velocity_pub.publish(10) diff --git a/code/planning/src/local_planner/objects.json b/code/planning/src/local_planner/objects.json deleted file mode 100644 index bdc96273..00000000 --- a/code/planning/src/local_planner/objects.json +++ /dev/null @@ -1,11 +0,0 @@ -{ - "objects": - [ - { - "type": "vehicle.*", - "id": "dummy", - "spawn_point": {"x": 983.59, "y": -5450.01, "z": 703.810118745277, "roll": 0.0, "pitch": 0.0, "yaw": -1.4} - } - ] - } - \ No newline at end of file diff --git a/code/planning/src/local_planner/spawn_vehicle.py b/code/planning/src/local_planner/spawn_vehicle.py deleted file mode 100644 index 552145be..00000000 --- a/code/planning/src/local_planner/spawn_vehicle.py +++ /dev/null @@ -1,38 +0,0 @@ -import carla -import os -import rospy - - -CARLA_HOST = os.environ.get('CARLA_HOST', 'paf23-carla-simulator-1') -CARLA_PORT = int(os.environ.get('CARLA_PORT', '2000')) - - -client = carla.Client(CARLA_HOST, CARLA_PORT) - -world = client.get_world() -world.wait_for_tick() - - -blueprint_library = world.get_blueprint_library() -# bp = blueprint_library.filter('vehicle.*')[0] -# vehicle = world.spawn_actor(bp, world.get_map().get_spawn_points()[0]) -bp = blueprint_library.filter("model3")[0] -for actor in world.get_actors(): - if actor.attributes.get('role_name') == "hero": - ego_vehicle = actor - break - -spawnPoint = carla.Transform(ego_vehicle.get_location() + carla.Location(y=25) + carla.Location(x=-1), ego_vehicle.get_transform().rotation) -vehicle = world.spawn_actor(bp, spawnPoint) - - -vehicle.set_autopilot(False) -# vehicle.set_location(loc) -print("spawned vehicle: " + str(vehicle.get_location())) -# get spectator -spectator = world.get_spectator() -# set spectator to follow ego vehicle with offset -spectator.set_transform( - carla.Transform(ego_vehicle.get_location() + carla.Location(z=50), - carla.Rotation(pitch=-90))) -print(spectator.get_location()) \ No newline at end of file diff --git a/code/planning/src/local_planner/test.txt b/code/planning/src/local_planner/test.txt deleted file mode 100644 index 42d24e4c..00000000 --- a/code/planning/src/local_planner/test.txt +++ /dev/null @@ -1,68 +0,0 @@ -obstacle position (array([ 980.96230118, -5476.51898298, 703.77912124]), array([ 979.05757394, -5479.27978253, 703.77912124]), array([-0.23368553, -1.65490565, 0. ])) - -current position [ 983.71518498 -5451.65969637 703.77912124] - -current heading -1.7110766172409058 - -distance: 20 - -[[ 983.4673446 -5452.11599828] - [ 983.46555604 -5453.11746727] - [ 983.46376748 -5454.11893626] - [ 983.46197892 -5455.12040525] - [ 983.46019036 -5456.12187424] - [ 983.4584018 -5457.12334323] - [ 983.45661324 -5458.12481222] - [ 983.45482468 -5459.12628121] - [ 983.45303612 -5460.12775021] - [ 983.45124755 -5461.1292192 ] - [ 983.44945899 -5462.13068819] - [ 983.44767043 -5463.13215718] - [ 983.44588187 -5464.13362617] - [ 983.44409331 -5465.13509516] - [ 983.44230475 -5466.13656415] - [ 983.44051619 -5467.13803314] - [ 983.43872763 -5468.13950213] - [ 983.43693907 -5469.14097113] - [ 983.43515051 -5470.14244012] - [ 983.43336195 -5471.14390911] - [ 983.43157339 -5472.1453781 ] - [ 983.42978483 -5473.14684709] - [ 983.42799627 -5474.14831608] - [ 983.42620771 -5475.14978507] - [ 983.42441915 -5476.15125406] - [ 983.42263059 -5477.15272305] - [ 983.42084203 -5478.15419205] - [ 983.41905347 -5479.15566104] - [ 983.41726491 -5480.15713003] - [ 983.41547635 -5481.15859902] - [ 983.41368779 -5482.16006801] - [ 983.41189923 -5483.161537 ] - [ 983.41011067 -5484.16300599] - [ 983.40832211 -5485.16447498] - [ 983.40653355 -5486.16594397] - [ 983.40474499 -5487.16741297] - [ 983.40295643 -5488.16888196] - [ 983.40116787 -5489.17035095] - [ 983.39937931 -5490.17181994] - [ 983.39759075 -5491.17328893] - [ 983.39580219 -5492.17475792] - [ 983.39401363 -5493.17622691] - [ 983.39222507 -5494.1776959 ] - [ 983.39043651 -5495.17916489] - [ 983.38864795 -5496.18063389] - [ 983.38685939 -5497.18210288] - [ 983.38507082 -5498.18357187] - [ 983.38328226 -5499.18504086] - [ 983.3814937 -5500.18650985] - [ 983.37970514 -5501.18797884] - [ 983.37791658 -5502.18944783] - [ 983.37612802 -5503.19091682] - [ 983.37433946 -5504.19238581] - [ 983.3725509 -5505.19385481] - [ 983.37076234 -5506.1953238 ] - [ 983.36897378 -5507.19679279] - [ 983.36718522 -5508.19826178] - [ 983.36539666 -5509.19973077] - [ 983.3636081 -5510.20119976] - [ 983.36181954 -5511.20266875]] \ No newline at end of file diff --git a/code/planning/src/local_planner/test_approx_pos.py b/code/planning/src/local_planner/test_approx_pos.py deleted file mode 100644 index 280a7455..00000000 --- a/code/planning/src/local_planner/test_approx_pos.py +++ /dev/null @@ -1,51 +0,0 @@ -import unittest -import numpy as np -import matplotlib.pyplot as plt -from scipy.spatial.transform import Rotation -import math - - -def approx_obstacle_pos(distance: float, heading: float, ego_pos: np.array): - """calculate the position of the obstacle in the global coordinate system - based on ego position, heading and distance - """ - rotation_matrix = Rotation.from_euler('z', heading) - - # Create distance vector with 0 rotation - relative_position_local = np.array([distance, 0, 0]) - - # Rotate distance vector to match heading - absolute_position_local = rotation_matrix.apply(relative_position_local) - - # Add egomposition vector with distance vetor to get absolute position - vehicle_position_global = ego_pos + absolute_position_local - return vehicle_position_global - - -class TestMotionPlanning(unittest.TestCase): - def test_approx_obstacle_pos(self): - ego_pos = np.array([0, 0, 0]) - target_positions = np.array([np.array([10, 10, 0]), np.array([20, 10, 0]), np.array([30, 30, 0])]) - headings = [math.radians(315), -0.4636476, math.radians(315)] - - plt.figure() - plt.title('Approximated vs Expected Positions') - plt.xlabel('x') - plt.ylabel('y') - - for index in range(len(target_positions)): - distance = np.linalg.norm(target_positions[index] - ego_pos) - calculated_position = approx_obstacle_pos(distance, headings[index], ego_pos) - print(calculated_position) - # np.testing.assert_array_almost_equal(calculated_position, target_position, decimal=1) - - plt.scatter(*target_positions[:2], color='blue', label='Expected') - plt.scatter(*calculated_position[:2], color='red', label='Calculated', marker='x') - - plt.legend(loc='lower left') - plt.grid(visible=True) - plt.show() - - -if __name__ == '__main__': - unittest.main() diff --git a/code/planning/src/local_planner/test_traj.py b/code/planning/src/local_planner/test_traj.py deleted file mode 100644 index 05a00c73..00000000 --- a/code/planning/src/local_planner/test_traj.py +++ /dev/null @@ -1,68 +0,0 @@ -import numpy as np - -from frenet_optimal_trajectory_planner.FrenetOptimalTrajectory.fot_wrapper \ - import run_fot - -import matplotlib.pyplot as plt - - -wp = np.r_[[np.full((50), 983.5889666959667)], - [np.linspace(5370.016106881272, 5399.016106881272, 50)]].T -obs = np.array([[983.568124548765, 5384.0219828457075, - 983.628124548765, 5386.0219828457075]]) -initial_conditions = { - 'ps': 0, - 'target_speed': 6, - 'pos': np.array([983.5807552562393, 5370.014637890163]), - 'vel': np.array([5, 1]), - 'wp': wp, - 'obs': obs -} - -hyperparameters = { - "max_speed": 25.0, - "max_accel": 15.0, - "max_curvature": 20.0, - "max_road_width_l": 3.0, - "max_road_width_r": 0, - "d_road_w": 0.5, - "dt": 0.2, - "maxt": 20.0, - "mint": 6.0, - "d_t_s": 0.5, - "n_s_sample": 2.0, - "obstacle_clearance": 2, - "kd": 1.0, - "kv": 0.1, - "ka": 0.1, - "kj": 0.1, - "kt": 0.1, - "ko": 0.1, - "klat": 1.0, - "klon": 1.0, - "num_threads": 0, # set 0 to avoid using threaded algorithm -} - -result_x, result_y, speeds, ix, iy, iyaw, d, s, speeds_x, \ - speeds_y, misc, costs, success = run_fot(initial_conditions, - hyperparameters) - -if success: - print("Success!") - print("result_x: ", result_x) - print("result_y: ", result_y) - print("yaw!", iyaw) - fig, ax = plt.subplots(1, 2) - - ax[0].scatter(wp[:, 0], wp[:, 1], label="original") - ax[0].scatter([obs[0, 0], obs[0, 2]], - [obs[0, 1], obs[0, 3]], label="object") - ax[0].set_xticks([obs[0, 0], obs[0, 2]]) - ax[1].scatter(result_x, result_y, label="frenet") - ax[1].scatter([obs[0, 0], obs[0, 2]], - [obs[0, 1], obs[0, 3]], label="object") - ax[1].set_xticks([obs[0, 0], obs[0, 2]]) - plt.legend() - plt.show() -else: - print("Failure!") diff --git a/code/planning/src/local_planner/utils.py b/code/planning/src/local_planner/utils.py index e9de4e6d..6483b16e 100644 --- a/code/planning/src/local_planner/utils.py +++ b/code/planning/src/local_planner/utils.py @@ -6,15 +6,15 @@ "max_speed": 15, "max_accel": 4.0, "max_curvature": 30.0, - "max_road_width_l": 0.0, - "max_road_width_r": 3.5, + "max_road_width_l": 0.1, + "max_road_width_r": 4, "d_road_w": 0.2, "dt": 0.2, - "maxt": 17.0, + "maxt": 30, "mint": 6.0, "d_t_s": 0.5, "n_s_sample": 2.0, - "obstacle_clearance": 1, + "obstacle_clearance": 2, "kd": 1.0, "kv": 0.1, "ka": 0.1, @@ -81,7 +81,7 @@ def approx_obstacle_pos(distance: float, heading: float, length = np.array([3, 0, 0]) length_vector = rotation_matrix.apply(length) - offset = np.array([0.75, 0, 0]) + offset = np.array([1, 0, 0]) rotation_adjusted = Rotation.from_euler('z', heading + math.radians(90)) offset_front = rotation_adjusted.apply(offset) From a1a1b3b8704edce7a81e139e13ee17caa001803b Mon Sep 17 00:00:00 2001 From: samuelkuehnel <51356601+samuelkuehnel@users.noreply.github.com> Date: Mon, 29 Jan 2024 00:08:46 +0100 Subject: [PATCH 15/19] fix: Linter and remove test code --- .../src/global_plan_distance_publisher.py | 2 +- code/planning/launch/planning.launch | 4 +- .../src/global_planner/global_planner.py | 4 +- .../src/local_planner/motion_planning.py | 126 +++++------------- code/planning/src/local_planner/utils.py | 35 +++-- 5 files changed, 60 insertions(+), 111 deletions(-) diff --git a/code/perception/src/global_plan_distance_publisher.py b/code/perception/src/global_plan_distance_publisher.py index 4e4cf37b..40da367c 100755 --- a/code/perception/src/global_plan_distance_publisher.py +++ b/code/perception/src/global_plan_distance_publisher.py @@ -47,7 +47,7 @@ def __init__(self): # Change comment for dev_launch self.global_plan_subscriber = self.new_subscription( CarlaRoute, - "/paf/" + self.role_name + "/global_plan", + "/carla/" + self.role_name + "/global_plan", self.update_global_route, qos_profile=1) # self.global_plan_subscriber = self.new_subscription( diff --git a/code/planning/launch/planning.launch b/code/planning/launch/planning.launch index 72c1f88c..9dee1352 100644 --- a/code/planning/launch/planning.launch +++ b/code/planning/launch/planning.launch @@ -16,13 +16,13 @@ - + diff --git a/code/planning/src/global_planner/global_planner.py b/code/planning/src/global_planner/global_planner.py index bfed5f32..a32936ab 100755 --- a/code/planning/src/global_planner/global_planner.py +++ b/code/planning/src/global_planner/global_planner.py @@ -57,7 +57,7 @@ def __init__(self): # uncomment /paf/hero/global_plan and comment /carla/... for dev_launch self.global_plan_sub = self.new_subscription( msg_type=CarlaRoute, - topic='/paf/' + self.role_name + '/global_plan', + topic='/carla/' + self.role_name + '/global_plan', callback=self.global_route_callback, qos_profile=10) # self.global_plan_sub = self.new_subscription( @@ -84,7 +84,7 @@ def __init__(self): self.logdebug('PrePlanner-Node started') # uncomment for self.dev_load_world_info() for dev_launch - self.dev_load_world_info() + # self.dev_load_world_info() def global_route_callback(self, data: CarlaRoute) -> None: """ diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index 528543be..44019360 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -11,20 +11,18 @@ from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion from carla_msgs.msg import CarlaSpeedometer import numpy as np -import math -# from behavior_agent.msg import BehaviorSpeed from frenet_optimal_trajectory_planner.FrenetOptimalTrajectory.fot_wrapper \ import run_fot from perception.msg import Waypoint, LaneChange -import planning +import planning # noqa: F401 from behavior_agent.behaviours import behavior_speed as bs from utils import convert_to_ms, approx_obstacle_pos, \ - hyperparameters, location_to_gps + hyperparameters + +# from scipy.spatial._kdtree import KDTree -import matplotlib.pyplot as plt -from scipy.spatial._kdtree import KDTree class MotionPlanning(CompatibleNode): """ @@ -178,94 +176,45 @@ def __set_current_pos(self, data: PoseStamped): data.pose.position.y, data.pose.position.z]) - def change_trajectory(self, data: Bool): - import carla - import os - CARLA_HOST = os.environ.get('CARLA_HOST', 'paf23-carla-simulator-1') - CARLA_PORT = int(os.environ.get('CARLA_PORT', '2000')) - - - client = carla.Client(CARLA_HOST, CARLA_PORT) - - world = client.get_world() - world.wait_for_tick() - - - blueprint_library = world.get_blueprint_library() - # bp = blueprint_library.filter('vehicle.*')[0] - # vehicle = world.spawn_actor(bp, world.get_map().get_spawn_points()[0]) - bp = blueprint_library.filter("model3")[0] - for actor in world.get_actors(): - if actor.attributes.get('role_name') == "hero": - ego_vehicle = actor - break - - spawnPoint = carla.Transform(ego_vehicle.get_location() + carla.Location(y=25), ego_vehicle.get_transform().rotation) - vehicle = world.spawn_actor(bp, spawnPoint) - - vehicle.set_autopilot(False) - # vehicle.set_location(loc) - self.logerr("spawned vehicle: " + str(vehicle.get_location())) - # coords = vehicle.get_location() - # get spectator - spectator = world.get_spectator() - # set spectator to follow ego vehicle with offset - spectator.set_transform( - carla.Transform(ego_vehicle.get_location() + carla.Location(z=50), - carla.Rotation(pitch=-90))) - print(spectator.get_location()) - self.counter += 1 - self.overtaking = True + def change_trajectory(self, distance: float): self.overtake_start = rospy.get_rostime() - # index_car = 20 limit_waypoints = 30 data = self.trajectory - self.logerr("Trajectory chagen started") np_array = np.array(data.poses) - - obstacle_position = approx_obstacle_pos(25, self.current_heading, self.current_pos, self.current_speed) - trajectory_np = self.convert_pose_to_array(np_array) - wp = KDTree(trajectory_np[:, :2]).query(obstacle_position[0][:2])[1] - self.logerr("waypoint index obs " + str(wp)) - self.logerr("obstacle position " + str(obstacle_position)) - self.logerr("current position " + str(self.current_pos)) - self.logerr("current heading " + str(self.current_heading)) - self.logerr("current waypoint " + str(self.current_wp)) - selection = np_array[int(self.current_wp):int(self.current_wp) + 25 + 30] + obstacle_position = approx_obstacle_pos(distance, + self.current_heading, + self.current_pos, + self.current_speed) + # trajectory_np = self.convert_pose_to_array(np_array) + # wp = KDTree(trajectory_np[:, :2]).query(obstacle_position[0][:2])[1] + selection = np_array[int(self.current_wp):int(self.current_wp) + + int(distance + limit_waypoints)] waypoints = self.convert_pose_to_array(selection) - # np.save("/workspace/code/trajectory.npy", trajectory_np) - self.logerr("waypoints " + str(waypoints)) - # obs = np.array([[coords.x, coords.y, coords.x, coords.y-3]]) [self.current_speed * math.cos(self.current_heading), self.current_speed * math.sin(self.current_heading)] - # self.logerr("obs " + str(obs)) - pos_lat_lon = location_to_gps(0,0, self.current_pos[0], self.current_pos[1]) initial_conditions = { 'ps': 0, 'target_speed': self.current_speed, 'pos': np.array([self.current_pos[0], self.current_pos[1]]), - 'vel': np.array([obstacle_position[2][0], obstacle_position[2][1]]), + 'vel': np.array([obstacle_position[2][0], + obstacle_position[2][1]]), 'wp': waypoints, - 'obs': np.array([[obstacle_position[0][0], obstacle_position[0][1], obstacle_position[1][0], obstacle_position[1][1]]]) + 'obs': np.array([[obstacle_position[0][0], + obstacle_position[0][1], + obstacle_position[1][0], + obstacle_position[1][1]]]) } - result_x, result_y, speeds, ix, iy, iyaw, d, s, speeds_x, \ speeds_y, misc, costs, success = run_fot(initial_conditions, hyperparameters) - self.logerr("success: " + str(success)) - self.logerr("result_x: " + str(result_x)) - self.logerr("result_y: " + str(result_y)) - # fig.savefig("/workspace/code/fot.png") - # plt.show() if success: - self.logerr("Success") result = [] for i in range(len(result_x)): position = Point(result_x[i], result_y[i], 0) quaternion = tf.transformations.quaternion_from_euler(0, - 0, - iyaw[i]) + 0, + iyaw[i]) orientation = Quaternion(x=quaternion[0], y=quaternion[1], - z=quaternion[2], w=quaternion[3]) + z=quaternion[2], w=quaternion[3]) pose = Pose(position, orientation) pos = PoseStamped() pos.header.frame_id = "global" @@ -274,11 +223,9 @@ def change_trajectory(self, data: Bool): path = Path() path.header.stamp = rospy.Time.now() path.header.frame_id = "global" - self.logerr("result: " + str(result)) - path.poses = list(np_array[:int(self.current_wp)]) + result + list(np_array[int(self.current_wp + 25 + 30):]) - self.logerr("Changed traj: " + str(path.poses)) - self.enhanced_path = path - self.logerr("Trajectory change finished") + path.poses = list(np_array[:int(self.current_wp)]) + \ + result + list(np_array[int(self.current_wp + 25 + 30):]) + self.trajectory = path def __set_trajectory(self, data: Path): """get current trajectory global planning @@ -300,7 +247,8 @@ def convert_pose_to_array(self, poses: np.array): """ result_array = np.empty((len(poses), 2)) for pose in range(len(poses)): - result_array[pose] = np.array([poses[pose].pose.position.x, poses[pose].pose.position.y]) + result_array[pose] = np.array([poses[pose].pose.position.x, + poses[pose].pose.position.y]) return result_array def __check_emergency(self, data: Bool): @@ -436,23 +384,9 @@ def loop(timer_event=None): if self.trajectory is None or self.__acc_speed is None or \ self.__curr_behavior is None: return - self.velocity_pub.publish(5) - # self.update_target_speed(self.__acc_speed, self.__curr_behavior) - # self.velocity_pub.publish(10) - - # if self.overtaking and rospy.get_rostime() - self.overtake_start < rospy.Duration(30): - # self.logerr("overtake finished") - # self.overtaking = False - - # if len(self.trajectory.poses) < 1: - # self.loginfo("No trajectory recieved") - # return - if self.overtaking and self.enhanced_path is not None: - self.enhanced_path.header.stamp = rospy.Time.now() - self.traj_pub.publish(self.enhanced_path) - else: - self.trajectory.header.stamp = rospy.Time.now() - self.traj_pub.publish(self.trajectory) + self.update_target_speed(self.__acc_speed, self.__curr_behavior) + self.trajectory.header.stamp = rospy.Time.now() + self.traj_pub.publish(self.trajectory) self.new_timer(self.control_loop_rate, loop) self.spin() diff --git a/code/planning/src/local_planner/utils.py b/code/planning/src/local_planner/utils.py index 6483b16e..dee2fc25 100644 --- a/code/planning/src/local_planner/utils.py +++ b/code/planning/src/local_planner/utils.py @@ -27,13 +27,19 @@ } -def location_to_gps(lat_ref, lon_ref, x, y): - """ - Convert from world coordinates to GPS coordinates - :param lat_ref: latitude reference for the current map - :param lon_ref: longitude reference for the current map - :param location: location to translate - :return: dictionary with lat, lon and height +def location_to_gps(lat_ref: float, lon_ref: float, x: float, y: float): + """Convert world coordinates to (lat,lon,z) coordinates + Copied from: + https://github.com/carla-simulator/scenario_runner/blob/master/srunner/tools/route_manipulation.py + + Args: + lat_ref (float): reference lat value + lon_ref (float): reference lat value + x (float): x-Coordinate value + y (float): y-Coordinate value + + Returns: + dict: Dictionary with (lat,lon,z) coordinates """ EARTH_RADIUS_EQUA = 6378137.0 # pylint: disable=invalid-name @@ -45,7 +51,8 @@ def location_to_gps(lat_ref, lon_ref, x, y): my -= y lon = mx * 180.0 / (math.pi * EARTH_RADIUS_EQUA * scale) - lat = 360.0 * math.atan(math.exp(my / (EARTH_RADIUS_EQUA * scale))) / math.pi - 90.0 + lat = 360.0 * math.atan(math.exp(my / (EARTH_RADIUS_EQUA * scale))) /\ + math.pi - 90.0 z = 703 return {'lat': lat, 'lon': lon, 'z': z} @@ -69,7 +76,7 @@ def approx_obstacle_pos(distance: float, heading: float, # Create distance vector with 0 rotation relative_position_local = np.array([distance, 0, 0]) - + # speed vector speed_vector = rotation_matrix.apply(np.array([speed, 0, 0])) # Rotate distance vector to match heading @@ -95,5 +102,13 @@ def approx_obstacle_pos(distance: float, heading: float, vehicle_position_global_end, speed_vector -def convert_to_ms(speed): +def convert_to_ms(speed: float): + """Convert km/h to m/s + + Args: + speed (float): speed in km/h + + Returns: + float: speed in m/s + """ return speed / 3.6 From f0ca432b08397d0e25938f34b273d4d85a0b1ce0 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Mon, 29 Jan 2024 12:56:56 +0100 Subject: [PATCH 16/19] fix: update docker compose + Linter --- build/docker-compose.yml | 4 ++-- code/planning/__init__.py | 1 - 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/build/docker-compose.yml b/build/docker-compose.yml index 10431837..f3d46bf3 100644 --- a/build/docker-compose.yml +++ b/build/docker-compose.yml @@ -61,8 +61,8 @@ services: tty: true shm_size: 2gb #command: bash -c "sleep 10 && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/opt/leaderboard/leaderboard/autoagents/npc_agent.py --host=carla-simulator --track=SENSORS" - command: bash -c "sleep 10 && roslaunch agent/launch/dev.launch" - # command: bash -c "sleep 10 && sudo chown -R carla:carla ../code/ && sudo chmod -R a+w ../code/ && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/workspace/code/agent/src/agent/agent.py --host=paf23-carla-simulator-1 --track=MAP" + # command: bash -c "sleep 10 && roslaunch agent/launch/dev.launch" + command: bash -c "sleep 10 && sudo chown -R carla:carla ../code/ && sudo chmod -R a+w ../code/ && python3 /opt/leaderboard/leaderboard/leaderboard_evaluator.py --debug=0 --routes=/opt/leaderboard/data/routes_devtest.xml --agent=/workspace/code/agent/src/agent/agent.py --host=paf23-carla-simulator-1 --track=MAP" logging: driver: "local" diff --git a/code/planning/__init__.py b/code/planning/__init__.py index 191d7b66..e69de29b 100755 --- a/code/planning/__init__.py +++ b/code/planning/__init__.py @@ -1 +0,0 @@ -import behavior_agent \ No newline at end of file From 5d32713bfa736c345429605360c23f5acea750ab Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Mon, 29 Jan 2024 13:04:51 +0100 Subject: [PATCH 17/19] fix: Dockerfile fix --- build/docker/agent/Dockerfile | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/build/docker/agent/Dockerfile b/build/docker/agent/Dockerfile index acf7edf7..2c6ce5ac 100644 --- a/build/docker/agent/Dockerfile +++ b/build/docker/agent/Dockerfile @@ -163,9 +163,7 @@ RUN source ~/.bashrc && pip install -r /workspace/requirements.txt COPY --chown=$USERNAME:$USERNAME ./code /workspace/code/ # Install Frenet Optimal Trajectory Planner -RUN sudo mkdir /erdos -RUN sudo mkdir /erdos/dependencies -RUN sudo mkdir /erdos/dependencies/frenet_optimal_trajectory_planner +RUN sudo mkdir -p /erdos/dependencies/frenet_optimal_trajectory_planner # Needed to resolve dependencies correctly inside freent_optimal_trajectory_planner ENV PYLOT_HOME=/erdos From 20eb060a7bf6b6b82df11b6b02dbc8fcafa37c10 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Mon, 29 Jan 2024 13:20:24 +0100 Subject: [PATCH 18/19] fix: md update --- .../03_planning/00_paf23/05_local_trajectory_planning.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/03_research/03_planning/00_paf23/05_local_trajectory_planning.md b/doc/03_research/03_planning/00_paf23/05_local_trajectory_planning.md index 8e998d3b..9d9f2bc1 100644 --- a/doc/03_research/03_planning/00_paf23/05_local_trajectory_planning.md +++ b/doc/03_research/03_planning/00_paf23/05_local_trajectory_planning.md @@ -14,4 +14,4 @@ Samuel Kühnel ## Position calculation from Obstacle -The position from a possible obstacle that we need to overtake is calculated via the current heading from the ego vehicle, the current position and the measured distance. The \ No newline at end of file +The position from a possible obstacle that we need to overtake is calculated via the current heading from the ego vehicle, the current position and the measured distance. The From a07910313722bb4a4a56c43b9c83d3bcd4c22cb9 Mon Sep 17 00:00:00 2001 From: samuelkuehnel Date: Tue, 30 Jan 2024 11:35:00 +0100 Subject: [PATCH 19/19] fix: trajectory topic --- code/agent/config/rviz_config.rviz | 4 ++-- code/planning/src/global_planner/global_planner.py | 2 +- code/planning/src/local_planner/ACC.py | 2 +- code/planning/src/local_planner/motion_planning.py | 12 +++--------- 4 files changed, 7 insertions(+), 13 deletions(-) diff --git a/code/agent/config/rviz_config.rviz b/code/agent/config/rviz_config.rviz index abfef827..2e419cc2 100644 --- a/code/agent/config/rviz_config.rviz +++ b/code/agent/config/rviz_config.rviz @@ -63,9 +63,9 @@ Visualization Manager: Unreliable: false Value: true Visibility: - Grid: true + Grid: false Imu: false - Path: true + Path: false PointCloud2: false Value: true Zoom Factor: 1 diff --git a/code/planning/src/global_planner/global_planner.py b/code/planning/src/global_planner/global_planner.py index a32936ab..41ee798c 100755 --- a/code/planning/src/global_planner/global_planner.py +++ b/code/planning/src/global_planner/global_planner.py @@ -74,7 +74,7 @@ def __init__(self): self.path_pub = self.new_publisher( msg_type=Path, - topic='/paf/' + self.role_name + '/trajectory_dummy', + topic='/paf/' + self.role_name + '/trajectory_global', qos_profile=1) self.speed_limit_pub = self.new_publisher( diff --git a/code/planning/src/local_planner/ACC.py b/code/planning/src/local_planner/ACC.py index ed44ab4c..e09bc575 100755 --- a/code/planning/src/local_planner/ACC.py +++ b/code/planning/src/local_planner/ACC.py @@ -47,7 +47,7 @@ def __init__(self): # Get trajectory to determine current speed limit self.trajectory_sub: Subscriber = self.new_subscription( Path, - f"/paf/{self.role_name}/trajectory_dummy", + f"/paf/{self.role_name}/trajectory_global", self.__set_trajectory, qos_profile=1) diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index 44019360..19d46953 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -68,14 +68,10 @@ def __init__(self): f"/paf/{self.role_name}/current_heading", self.__set_heading, qos_profile=1) - self.test_sub = self.new_subscription( - Bool, - f"/paf/{self.role_name}/test", - self.change_trajectory, - qos_profile=1) + self.trajectory_sub = self.new_subscription( Path, - f"/paf/{self.role_name}/trajectory_dummy", + f"/paf/{self.role_name}/trajectory_global", self.__set_trajectory, qos_profile=1) self.current_pos_sub = self.new_subscription( @@ -179,8 +175,7 @@ def __set_current_pos(self, data: PoseStamped): def change_trajectory(self, distance: float): self.overtake_start = rospy.get_rostime() limit_waypoints = 30 - data = self.trajectory - np_array = np.array(data.poses) + np_array = np.array(self.trajectory.poses) obstacle_position = approx_obstacle_pos(distance, self.current_heading, self.current_pos, @@ -234,7 +229,6 @@ def __set_trajectory(self, data: Path): data (Path): Trajectory waypoints """ self.trajectory = data - self.logerr("Trajectory recieved: " + str(self.trajectory)) def convert_pose_to_array(self, poses: np.array): """convert pose array to numpy array