Skip to content

Commit

Permalink
markers back for foxglove - version control is nice
Browse files Browse the repository at this point in the history
  • Loading branch information
@b1n-ch1kn
b1n-ch1kn committed May 11, 2024
1 parent 654ff8a commit 23ef802
Show file tree
Hide file tree
Showing 2 changed files with 61 additions and 81 deletions.
38 changes: 18 additions & 20 deletions src/common/driverless_common/driverless_common/marker.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@


def marker_array_from_map(detection: ConeDetectionStamped, ground_truth: bool = False) -> MarkerArray:
cones: List[ConeWithCovariance] = detection.cones
cones: List[ConeWithCovariance] = detection.cones_with_cov

if ground_truth: # ground truth markers out of the sim are translucent
alpha: float = 1.0
Expand All @@ -29,8 +29,7 @@ def marker_array_from_map(detection: ConeDetectionStamped, ground_truth: bool =
z_scale = SMALL_HEIGHT
markers.append(
marker_msg(
x=cone.cone.location.x,
y=cone.cone.location.y,
position=cone.cone.location,
z_scale=z_scale,
id_=i,
header=detection.header,
Expand All @@ -41,12 +40,12 @@ def marker_array_from_map(detection: ConeDetectionStamped, ground_truth: bool =
)
markers.append(
cov_marker_msg(
x=cone.cone.location.x,
y=cone.cone.location.y,
position=cone.cone.location,
id_=i,
x_scale=3 * sqrt(abs(cone.covariance[0])),
y_scale=3 * sqrt(abs(cone.covariance[3])),
lifetime=Duration(sec=10, nanosec=100000),
header=detection.header,
)
)

Expand All @@ -72,8 +71,7 @@ def marker_array_from_cone_detection(detection: ConeDetectionStamped, covariance
z_scale = SMALL_HEIGHT
markers.append(
marker_msg(
x=cones[i].location.x,
y=cones[i].location.y,
position=cones[i].location,
z_scale=z_scale,
id_=i,
header=detection.header,
Expand All @@ -83,8 +81,7 @@ def marker_array_from_cone_detection(detection: ConeDetectionStamped, covariance
if covariance:
markers.append(
cov_marker_msg(
x=cones[i].location.x,
y=cones[i].location.y,
position=cones[i].location,
id_=i,
x_scale=3 * sqrt(abs(covs[i][0])),
y_scale=3 * sqrt(abs(covs[i][3])),
Expand Down Expand Up @@ -119,8 +116,7 @@ def marker_array_from_cone_detection(detection: ConeDetectionStamped, covariance


def marker_msg(
x: float,
y: float,
position: Point,
z_scale: float,
id_: int,
header: Header,
Expand All @@ -132,12 +128,14 @@ def marker_msg(
header=header,
ns="cones",
id=id_,
type=Marker.MESH_RESOURCE,
mesh_resource="package://driverless_common/meshes/cone.dae",
mesh_use_embedded_materials=False,
# type=Marker.MESH_RESOURCE,
type=Marker.CYLINDER,
# mesh_resource="package://driverless_common/meshes/cone.dae",
# mesh_use_embedded_materials=False,
action=Marker.ADD,
pose=Pose(position=Point(x=x, y=y, z=0.0), orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)),
scale=Vector3(x=1.0, y=1.0, z=z_scale),
pose=Pose(position=position, orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)),
# scale=Vector3(x=1.0, y=1.0, z=z_scale),
scale=Vector3(x=0.2, y=0.2, z=z_scale * 0.3),
color=CONE_TO_RGB_MAP.get(cone_colour, ColorRGBA(r=0.0, g=0.0, b=0.0, a=1.0)),
lifetime=lifetime,
)
Expand All @@ -146,23 +144,23 @@ def marker_msg(


def cov_marker_msg(
x: float,
y: float,
position: Point,
id_: int,
x_scale: float, # x sigma
y_scale: float, # y sigma
lifetime=Duration(sec=1, nanosec=0),
header=Header(frame_id="track"),
z=0,
) -> Marker:
return Marker(
header=header,
ns="cone_covs",
id=id_,
type=Marker.CYLINDER,
action=Marker.ADD,
pose=Pose(position=Point(x=x, y=y, z=0.0), orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)),
pose=Pose(position=position, orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)),
scale=Vector3(x=x_scale, y=y_scale, z=0.05),
color=ColorRGBA(r=0.3, g=0.1, b=0.1, a=0.1),
color=ColorRGBA(r=1.0, g=0.0, b=0.0, a=0.3),
lifetime=lifetime,
)

Expand Down
104 changes: 43 additions & 61 deletions src/common/driverless_common/driverless_common/node_display.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,15 +6,15 @@
from rclpy.publisher import Publisher

from ackermann_msgs.msg import AckermannDriveStamped
from driverless_msgs.msg import ConeDetectionStamped, PathStamped
from driverless_msgs.msg import ConeDetectionStamped
from geometry_msgs.msg import Point
from sensor_msgs.msg import Image
from std_msgs.msg import ColorRGBA
from visualization_msgs.msg import Marker
from visualization_msgs.msg import MarkerArray

from driverless_common.common import QOS_LATEST
from driverless_common.draw import draw_map, draw_markers, draw_steering
from driverless_common.marker import path_marker_msg
from driverless_common.marker import marker_array_from_cone_detection, marker_array_from_map, path_marker_msg

from typing import List

Expand Down Expand Up @@ -43,17 +43,16 @@ def __init__(self):
# steering angle target sub
self.create_subscription(AckermannDriveStamped, "/control/driving_command", self.steering_callback, QOS_LATEST)

# path spline sub
self.create_subscription(PathStamped, "/planner/path", self.path_callback, QOS_LATEST)

# cv2 rosboard image pubs
self.vision_img_publisher: Publisher = self.create_publisher(Image, "/debug_imgs/vision_det_img", 1)
self.lidar_img_publisher: Publisher = self.create_publisher(Image, "/debug_imgs/lidar_det_img", 1)
# self.vision_img_publisher: Publisher = self.create_publisher(Image, "/debug_imgs/vision_det_img", 1)
# self.lidar_img_publisher: Publisher = self.create_publisher(Image, "/debug_imgs/lidar_det_img", 1)
self.slam_img_publisher: Publisher = self.create_publisher(Image, "/debug_imgs/slam_image", 1)
self.local_img_publisher: Publisher = self.create_publisher(Image, "/debug_imgs/local_image", 1)

# path marker pubs
self.path_marker_publisher: Publisher = self.create_publisher(Marker, "/markers/path_line", 1)
# marker pubs
self.vision_mkr_publisher: Publisher = self.create_publisher(MarkerArray, "/debug_markers/vision_markers", 1)
self.lidar_mkr_publisher: Publisher = self.create_publisher(MarkerArray, "/debug_markers/lidar_markers", 1)
self.slam_mkr_publisher: Publisher = self.create_publisher(MarkerArray, "/debug_markers/slam_with_cov", 1)

self.get_logger().info("---Common visualisation node initialised---")

Expand All @@ -63,61 +62,44 @@ def steering_callback(self, msg: AckermannDriveStamped):
self.velocity = msg.drive.speed

def vision_callback(self, msg: ConeDetectionStamped):
if self.vision_img_publisher.get_subscription_count() == 0:
return
# subscribed to vision cone detections
cones = []
for cone in msg.cones:
new = cone
new.location.x = cone.location.x - 0.37
cones.append(new)
debug_img = draw_markers(msg.cones) # draw cones on image
debug_img = draw_steering(
debug_img, self.steering_angle, self.velocity
) # draw steering angle and vel data on image
self.vision_img_publisher.publish(cv_bridge.cv2_to_imgmsg(debug_img, encoding="bgr8"))
if self.vision_mkr_publisher.get_subscription_count() != 0:
self.vision_mkr_publisher.publish(marker_array_from_cone_detection(msg))

# if self.vision_img_publisher.get_subscription_count() == 0:
# return
# # subscribed to vision cone detections
# cones = []
# for cone in msg.cones:
# new = cone
# new.location.x = cone.location.x - 0.37
# cones.append(new)
# debug_img = draw_markers(msg.cones) # draw cones on image
# debug_img = draw_steering(
# debug_img, self.steering_angle, self.velocity
# ) # draw steering angle and vel data on image
# self.vision_img_publisher.publish(cv_bridge.cv2_to_imgmsg(debug_img, encoding="bgr8"))

def lidar_callback(self, msg: ConeDetectionStamped):
if self.lidar_img_publisher.get_subscription_count() == 0:
return
# subscribed to lidar cone detections
cones = []
for cone in msg.cones:
new = cone
new.location.x = cone.location.x + 1.59
cones.append(new)

debug_img = draw_markers(cones)
debug_img = draw_steering(debug_img, self.steering_angle, self.velocity)
self.lidar_img_publisher.publish(cv_bridge.cv2_to_imgmsg(debug_img, encoding="bgr8"))

def path_callback(self, msg: PathStamped):
if self.path_marker_publisher.get_subscription_count() == 0:
return
# subscribed to a desired path
path_markers: List[Point] = []
path_colours: List[ColorRGBA] = []

for point in msg.path:
# set colour proportional to angle
try:
col = col_range[round(point.turn_intensity)].get_rgb()

line_point = point.location
line_colour = ColorRGBA()
line_colour.a = 1.0 # alpha
line_colour.r = col[0]
line_colour.g = col[1]
line_colour.b = col[2]
path_markers.append(line_point)
path_colours.append(line_colour)

except:
continue

self.path_marker_publisher.publish(path_marker_msg(path_markers, path_colours))
if self.lidar_mkr_publisher.get_subscription_count() != 0:
self.lidar_mkr_publisher.publish(marker_array_from_cone_detection(msg))

# if self.lidar_img_publisher.get_subscription_count() == 0:
# return
# # subscribed to lidar cone detections
# cones = []
# for cone in msg.cones:
# new = cone
# new.location.x = cone.location.x + 1.59
# cones.append(new)

# debug_img = draw_markers(cones)
# debug_img = draw_steering(debug_img, self.steering_angle, self.velocity)
# self.lidar_img_publisher.publish(cv_bridge.cv2_to_imgmsg(debug_img, encoding="bgr8"))

def slam_callback(self, msg: ConeDetectionStamped):
if self.slam_mkr_publisher.get_subscription_count() != 0:
self.slam_mkr_publisher.publish(marker_array_from_map(msg))

if self.slam_img_publisher.get_subscription_count() == 0:
return
# subscribed to slam map
Expand Down

0 comments on commit 23ef802

Please sign in to comment.