feat(#195): add a distance publisher for the stop line and next waypoint

build/Taskfile

@@ -32,7 +32,7 @@ task:shell() {
 ##########################################
 task:install() {
  task:install:git_hooks
- task:gitconfig:copy
+ #task:gitconfig:copy
  install:gpu-support
  docker:install
 }

build/docker/agent/Dockerfile

@@ -73,7 +73,7 @@ RUN apt-get update && apt-get install -y \
  ros-noetic-ackermann-msgs ros-noetic-derived-object-msgs \
  ros-noetic-carla-msgs ros-noetic-pcl-conversions \
  ros-noetic-rviz ros-noetic-rqt ros-noetic-pcl-ros ros-noetic-rosbridge-suite ros-noetic-rosbridge-server \
- ros-noetic-robot-pose-ekf \
+ ros-noetic-robot-pose-ekf ros-noetic-ros-numpy \
  ros-noetic-py-trees-ros ros-noetic-rqt-py-trees ros-noetic-rqt-reconfigure
 
 SHELL ["/bin/bash", "-c"]

code/acting/launch/acting.launch

@@ -21,7 +21,7 @@
  <node pkg="acting" type="velocity_publisher_dummy.py" name="velocity_publisher_dummy" output="screen">
  <param name="role_name" value="$(arg role_name)" />
  <param name="control_loop_rate" value="0.2" />
- <param name="enabled" value="True" /> <!-- set to True to publish dummy velocities for testing-->
+ <param name="enabled" value="False" /> <!-- set to True to publish dummy velocities for testing-->
  </node>
 
  <node pkg="acting" type="vehicle_controller.py" name="vehicle_controller" output="screen">

code/acting/src/acting/velocity_controller.py

@@ -8,7 +8,8 @@
 from std_msgs.msg import Float32, Float32MultiArray
 from nav_msgs.msg import Path
 
-SPEED_LIMIT_DEFAULT: float = 36.0
+# TODO put back to 36 when controller can handle it
+SPEED_LIMIT_DEFAULT: float = 6 # 36.0
 
 
 class VelocityController(CompatibleNode):
@@ -38,10 +39,9 @@ def __init__(self):
  self.__get_current_velocity,
  qos_profile=1)
 
- self.speed_limit_sub: Subscriber = self.new_subscription(
+ self.speed_limit_pub: Publisher = self.new_publisher(
  Float32,
  f"/paf/{self.role_name}/speed_limit",
- self.__get_speed_limit,
  qos_profile=1)
 
  self.max_tree_v_sub: Subscriber = self.new_subscription(
@@ -109,9 +109,10 @@ def loop(timer_event=None):
  """
  if self.__max_velocity is None:
  self.logdebug("VehicleController hasn't received max_velocity"
- " yet and can therefore not publish a "
- "throttle value")
- return
+ " yet. max_velocity has been set to"
+ f"default value {SPEED_LIMIT_DEFAULT}")
+ # return
+ self.__max_velocity = SPEED_LIMIT_DEFAULT
 
  if self.__current_velocity is None:
  self.logdebug("VehicleController hasn't received "
@@ -141,7 +142,6 @@ def loop(timer_event=None):
  self.logerr("Velocity controller doesn't support backward "
  "driving yet.")
  return
-
  v = min(self.__max_velocity, self.__max_tree_v)
  v = min(v, self.__speed_limit)
 
@@ -191,6 +191,7 @@ def __current_position_callback(self, data: PoseStamped):
  self.__current_wp_index += 1
  self.__speed_limit = \
  self.__speed_limits_OD[self.__current_wp_index]
+ self.speed_limit_pub.publish(self.__speed_limit)
 
 
 def main(args=None):

code/agent/config/dev_objects.json

@@ -109,7 +109,7 @@
  "z": 1.60,
  "roll": 0.0,
  "pitch": 0.0,
- "yaw": -45.0
+ "yaw": 0.0
  },
  "range": 85,
  "rotation_frequency": 10,

code/agent/config/rviz_config.rviz

@@ -4,9 +4,11 @@ Panels:
  Name: Displays
  Property Tree Widget:
  Expanded:
- - /Path1
+ - /PointCloud23
+ - /PointCloud24
+ - /PointCloud25
  Splitter Ratio: 0.5
- Tree Height: 266
+ Tree Height: 308
  - Class: rviz/Selection
  Name: Selection
  - Class: rviz/Tool Properties
@@ -70,13 +72,13 @@ Visualization Manager:
  - Alpha: 1
  Class: rviz_plugin_tutorials/Imu
  Color: 204; 51; 204
- Enabled: true
+ Enabled: false
  History Length: 1
  Name: Imu
  Queue Size: 10
  Topic: /carla/hero/IMU
  Unreliable: false
- Value: true
+ Value: false
  - Alpha: 1
  Autocompute Intensity Bounds: true
  Autocompute Value Bounds:
@@ -117,7 +119,7 @@ Visualization Manager:
  Color: 255; 255; 255
  Color Transformer: Intensity
  Decay Time: 0
- Enabled: true
+ Enabled: false
  Invert Rainbow: false
  Max Color: 255; 255; 255
  Min Color: 0; 0; 0
@@ -132,7 +134,7 @@ Visualization Manager:
  Unreliable: false
  Use Fixed Frame: true
  Use rainbow: true
- Value: true
+ Value: false
  - Alpha: 1
  Buffer Length: 1
  Class: rviz/Path
@@ -157,6 +159,90 @@ Visualization Manager:
  Topic: /paf/hero/trajectory
  Unreliable: false
  Value: true
+ - Alpha: 1
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 10
+ Min Value: -10
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz/PointCloud2
+ Color: 255; 255; 255
+ Color Transformer: Intensity
+ Decay Time: 0
+ Enabled: true
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Min Color: 0; 0; 0
+ Name: PointCloud2
+ Position Transformer: XYZ
+ Queue Size: 10
+ Selectable: true
+ Size (Pixels): 3
+ Size (m): 0.20000000298023224
+ Style: Flat Squares
+ Topic: /carla/hero/LIDAR_filtered
+ Unreliable: false
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: true
+ - Alpha: 1
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 10
+ Min Value: -10
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz/PointCloud2
+ Color: 255; 255; 255
+ Color Transformer: Intensity
+ Decay Time: 0
+ Enabled: true
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Min Color: 0; 0; 0
+ Name: PointCloud2
+ Position Transformer: XYZ
+ Queue Size: 10
+ Selectable: true
+ Size (Pixels): 3
+ Size (m): 0.20000000298023224
+ Style: Flat Squares
+ Topic: /carla/hero/LIDAR_filtered_rear_left
+ Unreliable: false
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: true
+ - Alpha: 1
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 10
+ Min Value: -10
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz/PointCloud2
+ Color: 255; 255; 255
+ Color Transformer: Intensity
+ Decay Time: 0
+ Enabled: true
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Min Color: 0; 0; 0
+ Name: PointCloud2
+ Position Transformer: XYZ
+ Queue Size: 10
+ Selectable: true
+ Size (Pixels): 3
+ Size (m): 0.20000000298023224
+ Style: Flat Squares
+ Topic: /carla/hero/LIDAR_filtered_rear_right
+ Unreliable: false
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: true
  Enabled: true
  Global Options:
  Background Color: 48; 48; 48
@@ -185,7 +271,7 @@ Visualization Manager:
  Views:
  Current:
  Class: rviz/Orbit
- Distance: 49.498538970947266
+ Distance: 34.785499572753906
  Enable Stereo Rendering:
  Stereo Eye Separation: 0.05999999865889549
  Stereo Focal Distance: 1
@@ -201,19 +287,19 @@ Visualization Manager:
  Invert Z Axis: false
  Name: Current View
  Near Clip Distance: 0.009999999776482582
- Pitch: 0.33039820194244385
+ Pitch: 0.19039836525917053
  Target Frame: <Fixed Frame>
- Yaw: 1.720420479774475
+ Yaw: 4.520427227020264
  Saved: ~
 Window Geometry:
  Camera:
  collapsed: false
  Displays:
  collapsed: false
- Height: 1043
+ Height: 1376
  Hide Left Dock: false
  Hide Right Dock: false
- QMainWindow State: 000000ff00000000fd00000004000000000000030400000379fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000193000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000001d4000001e00000001600ffffff000000010000010f00000379fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b00000379000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000030700fffffffb0000000800540069006d00650100000000000004500000000000000000000003610000037900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ QMainWindow State: 000000ff00000000fd000000040000000000000304000004c6fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000022a000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d006500720061010000026b000002960000001600ffffff000000010000010f000004c6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000004c6000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009b80000003efc0100000002fb0000000800540069006d00650100000000000009b80000030700fffffffb0000000800540069006d0065010000000000000450000000000000000000000599000004c600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
  Selection:
  collapsed: false
  Time:
@@ -222,6 +308,6 @@ Window Geometry:
  collapsed: false
  Views:
  collapsed: false
- Width: 1920
- X: 0
- Y: 0
+ Width: 2488
+ X: 1992
+ Y: 27

code/agent/src/agent/agent.py

@@ -27,15 +27,15 @@ def sensors(self):
  'yaw': 0.0, 'width': 300, 'height': 200, 'fov': 100},
  {'type': 'sensor.lidar.ray_cast', 'id': 'LIDAR',
  'x': 0.7, 'y': -0.4, 'z': 1.60, 'roll': 0.0, 'pitch': 0.0,
- 'yaw': -45.0},
+ 'yaw': 0.0},
  {'type': 'sensor.other.radar', 'id': 'RADAR',
  'x': 0.7, 'y': -0.4, 'z': 1.60, 'roll': 0.0, 'pitch': 0.0,
  'yaw': -45.0, 'horizontal_fov': 30, 'vertical_fov': 30},
  {'type': 'sensor.other.gnss', 'id': 'GPS',
  'x': 0.7, 'y': -0.4, 'z': 1.60},
  {'type': 'sensor.other.imu', 'id': 'IMU',
  'x': 0.7, 'y': -0.4, 'z': 1.60, 'roll': 0.0, 'pitch': 0.0,
- 'yaw': -45.0},
+ 'yaw': 0.0},
  {'type': 'sensor.opendrive_map', 'id': 'OpenDRIVE',
  'reading_frequency': 1},
  {'type': 'sensor.speedometer', 'id': 'Speed'},

code/mock/launch/mock.launch

@@ -7,14 +7,14 @@
 <!-- <param name="role_name" value="$(arg role_name)" /> -->
 <!-- </node> -->
 
- <node pkg="mock" type="mock_stop_sign.py" name="mock_stop_sign" output="screen">
-  <param name="control_loop_rate" value="$(arg control_loop_rate)" />
+<!--  <node pkg="mock" type="mock_stop_sign.py" name="mock_stop_sign" output="screen">
+ <param name="control_loop_rate" value="$(arg control_loop_rate)" />
  <param name="role_name" value="$(arg role_name)" />
- </node>
+ </node> -->
 
- <node pkg="mock" type="mock_intersection_clear.py" name="mock_intersection_clear" output="screen">
+<!-- <node pkg="mock" type="mock_intersection_clear.py" name="mock_intersection_clear" output="screen">
  <param name="control_loop_rate" value="$(arg control_loop_rate)" />
  <param name="role_name" value="$(arg role_name)" />
- </node>
+ </node> -->
 
 </launch>

code/perception/CMakeLists.txt

@@ -10,13 +10,18 @@ project(perception)
 find_package(catkin REQUIRED)
 
 ## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
+find_package(catkin REQUIRED COMPONENTS
+ rosout
+ rospy
+ std_msgs
+ message_generation
+)
 
 
 ## Uncomment this if the package has a setup.py. This macro ensures
 ## modules and global scripts declared therein get installed
 ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-# catkin_python_setup()
+catkin_python_setup()
 
 ################################################
 ## Declare ROS messages, services and actions ##
@@ -43,11 +48,11 @@ find_package(catkin REQUIRED)
 ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
 
 ## Generate messages in the 'msg' folder
-# add_message_files(
-#  FILES
-# Message1.msg
-# Message2.msg
-# )
+add_message_files(
+ FILES
+ Waypoint.msg
+ LaneChange.msg
+)
 
 ## Generate services in the 'srv' folder
 # add_service_files(
@@ -64,10 +69,10 @@ find_package(catkin REQUIRED)
 # )
 
 ## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#  DEPENDENCIES
-# std_msgs # Or other packages containing msgs
-# )
+generate_messages(
+ DEPENDENCIES
+ std_msgs
+)
 
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##

code/perception/launch/perception.launch

@@ -32,5 +32,38 @@
  <param name="self_diagnose" value="true"/>
  </node>
 
+ <node pkg="perception" type="global_plan_distance_publisher.py" name="GlobalPlanDistance" output="screen">
+ <param name="control_loop_rate" value="0.1" />
+ <param name="role_name" value="$(arg role_name)" />
+ </node>
+
+ <node pkg="perception" type="lidar_distance.py" name="lidar_distance" output="screen">
+ <param name="max_y" value="1.5"/>
+ <param name="min_y" value="-1.5"/>
+ <param name="min_x" value="3"/>
+ <param name="min_z" value="-1.5"/>
+ <param name="max_z" value="0"/>
+ <param name="point_cloud_topic" value="/carla/hero/LIDAR_filtered"/>
+ <param name="range_topic" value="/carla/hero/LIDAR_range"/>
+ </node>
+
+ <node pkg="perception" type="lidar_distance.py" name="lidar_distance_rear_right" output="screen">
+ <param name="min_y" value="-5"/>
+ <param name="max_y" value="-2.5"/>
+ <param name="max_x" value="0"/>
+ <param name="min_z" value="-1.5"/>
+ <param name="max_z" value="0"/>
+ <param name="point_cloud_topic" value="/carla/hero/LIDAR_filtered_rear_right"/>
+ <param name="range_topic" value="/carla/hero/LIDAR_range_rear_right"/>
+ </node>
 
+ <node pkg="perception" type="lidar_distance.py" name="lidar_distance_rear_left" output="screen">
+ <param name="min_y" value="2.5"/>
+ <param name="max_y" value="5"/>
+ <param name="max_x" value="0"/>
+ <param name="min_z" value="-1.5"/>
+ <param name="max_z" value="0"/>
+ <param name="point_cloud_topic" value="/carla/hero/LIDAR_filtered_rear_left"/>
+ <param name="range_topic" value="/carla/hero/LIDAR_range_rear_left"/>
+ </node>
 </launch>