fix: trajectory topic

una-auxme · Jan 30, 2024 · a079103 · a079103
1 parent 20eb060
commit a079103
Show file tree

Hide file tree

Showing 4 changed files with 7 additions and 13 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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