Merge pull request #537 from danil-tolkachev/ros-bridge-map
Add OccupancyGrid to ros-bridge
This commit is contained in:
Néstor Subirón
GitHub
commit
7e6ee3795c
|
|
@ -8,7 +8,7 @@ Panels:
|
|||
- /Status1
|
||||
- /DepthCloud1/Auto Size1
|
||||
Splitter Ratio: 0.5
|
||||
Tree Height: 334
|
||||
Tree Height: 332
|
||||
- Class: rviz/Selection
|
||||
Name: Selection
|
||||
- Class: rviz/Tool Properties
|
||||
|
|
@ -17,7 +17,7 @@ Panels:
|
|||
- /2D Nav Goal1
|
||||
- /Publish Point1
|
||||
Name: Tool Properties
|
||||
Splitter Ratio: 0.5886790156364441
|
||||
Splitter Ratio: 0.588679016
|
||||
- Class: rviz/Views
|
||||
Expanded:
|
||||
- /Current View1
|
||||
|
|
@ -36,18 +36,18 @@ Panels:
|
|||
- /Global Options1
|
||||
- /non_player_vehicles1
|
||||
- /DepthCloud1/Auto Size1
|
||||
Splitter Ratio: 0.4943566620349884
|
||||
Tree Height: 170
|
||||
Splitter Ratio: 0.494356662
|
||||
Tree Height: 181
|
||||
Visualization Manager:
|
||||
Class: ""
|
||||
Displays:
|
||||
- Alpha: 0.10000000149011612
|
||||
- Alpha: 0.100000001
|
||||
Cell Size: 1
|
||||
Class: rviz/Grid
|
||||
Color: 170; 164; 162
|
||||
Enabled: true
|
||||
Line Style:
|
||||
Line Width: 0.029999999329447746
|
||||
Line Width: 0.0299999993
|
||||
Value: Lines
|
||||
Name: Grid
|
||||
Normal Cell Count: 0
|
||||
|
|
@ -88,7 +88,7 @@ Visualization Manager:
|
|||
Marker Topic: /vehicles
|
||||
Name: non_player_vehicles
|
||||
Namespaces:
|
||||
{}
|
||||
"": true
|
||||
Queue Size: 100
|
||||
Value: true
|
||||
- Class: rviz/Marker
|
||||
|
|
@ -122,7 +122,7 @@ Visualization Manager:
|
|||
Queue Size: 10
|
||||
Selectable: true
|
||||
Size (Pixels): 3
|
||||
Size (m): 0.009999999776482582
|
||||
Size (m): 0.00999999978
|
||||
Style: Flat Squares
|
||||
Topic: /lidar_0
|
||||
Unreliable: false
|
||||
|
|
@ -167,6 +167,16 @@ Visualization Manager:
|
|||
Use Fixed Frame: true
|
||||
Use rainbow: true
|
||||
Value: true
|
||||
- Alpha: 0.699999988
|
||||
Class: rviz/Map
|
||||
Color Scheme: map
|
||||
Draw Behind: false
|
||||
Enabled: true
|
||||
Name: Map
|
||||
Topic: /map
|
||||
Unreliable: false
|
||||
Use Timestamp: false
|
||||
Value: true
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 48; 48; 48
|
||||
|
|
@ -192,64 +202,62 @@ Visualization Manager:
|
|||
Views:
|
||||
Current:
|
||||
Class: rviz/ThirdPersonFollower
|
||||
Distance: 31.861072540283203
|
||||
Distance: 31.8610725
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.05999999865889549
|
||||
Stereo Eye Separation: 0.0599999987
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Focal Point:
|
||||
X: 6.999748229980469
|
||||
Y: 0.7988636493682861
|
||||
Z: 2.896774276450742e-6
|
||||
X: 6.99974823
|
||||
Y: 0.798863649
|
||||
Z: 2.89677428e-06
|
||||
Focal Shape Fixed Size: false
|
||||
Focal Shape Size: 0.05000000074505806
|
||||
Focal Shape Size: 0.0500000007
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Pitch: 0.18520352244377136
|
||||
Near Clip Distance: 0.00999999978
|
||||
Pitch: 0.185203522
|
||||
Target Frame: base_link
|
||||
Value: ThirdPersonFollower (rviz)
|
||||
Yaw: 3.5035617351531982
|
||||
Yaw: 3.50356174
|
||||
Saved:
|
||||
- Class: rviz/FPS
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.05999999865889549
|
||||
Stereo Eye Separation: 0.0599999987
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Invert Z Axis: false
|
||||
Name: FPS
|
||||
Near Clip Distance: 0.009999999776482582
|
||||
Pitch: 0.984796941280365
|
||||
Near Clip Distance: 0.00999999978
|
||||
Pitch: 0.984796941
|
||||
Position:
|
||||
X: 48.14033508300781
|
||||
Y: -258.651123046875
|
||||
Z: 284.4742431640625
|
||||
X: 48.1403351
|
||||
Y: -258.651123
|
||||
Z: 284.474243
|
||||
Target Frame: base_link
|
||||
Value: FPS (rviz)
|
||||
Yaw: 1.521981120109558
|
||||
Yaw: 1.52198112
|
||||
Window Geometry:
|
||||
"&Displays":
|
||||
collapsed: false
|
||||
"&Tool Properties":
|
||||
collapsed: false
|
||||
"&Views":
|
||||
collapsed: false
|
||||
Depth:
|
||||
collapsed: false
|
||||
Displays:
|
||||
collapsed: false
|
||||
Front_&cam:
|
||||
Front_cam:
|
||||
collapsed: false
|
||||
Height: 994
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: false
|
||||
QMainWindow State: 000000ff00000000fd00000004000000000000019a0000032bfc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007601000003fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007300000000430000032d000000f601000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000100044006900730070006c0061007900730100000043000000f6000000f601000003fb0000001200460072006f006e0074005f00630061006d010000013a0000011d0000002201000003fb0000000a00440065007000740068010000025800000116000000220100000300000001000001810000032bfc0200000009fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000043000001950000007601000003fb0000000a004400650070007400680000000043000001e80000000000000000fb00000018005300650067006d0065006e0074006100740069006f006e000000017f0000011f0000000000000000fb0000000a0049006d0061006700650000000043000000f60000000000000000fb0000000a0049006d0061006700650000000043000001630000000000000000fb0000001a00630061006d005f00660072006f006e0074005f00720067006200000001a7000001c70000000000000000fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007301000001d900000195000000ce01000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000690000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006900000004dfc0100000002fb0000000800540069006d0065010000000000000690000002bb01000003fb0000000800540069006d00650100000000000004500000000000000000000003730000032b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
QMainWindow State: 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
|
||||
Selection:
|
||||
collapsed: false
|
||||
T&ime:
|
||||
Time:
|
||||
collapsed: false
|
||||
Tool Properties:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: false
|
||||
Width: 1680
|
||||
X: 0
|
||||
X: 49
|
||||
Y: 30
|
||||
|
|
|
|||
|
|
@ -14,6 +14,7 @@ from carla.settings import CarlaSettings
|
|||
from carla_ros_bridge.control import InputController
|
||||
from carla_ros_bridge.markers import PlayerAgentHandler, NonPlayerAgentsHandler
|
||||
from carla_ros_bridge.sensors import CameraHandler, LidarHandler
|
||||
from carla_ros_bridge.map import MapHandler
|
||||
|
||||
|
||||
class CarlaRosBridge(object):
|
||||
|
|
@ -138,6 +139,10 @@ class CarlaRosBridge(object):
|
|||
number_of_player_starts = len(scene.player_start_spots)
|
||||
player_start = random.randint(0, max(0, number_of_player_starts - 1))
|
||||
|
||||
# Send occupancy grid to rivz
|
||||
map_handler = MapHandler(scene.map_name)
|
||||
map_handler.send_map()
|
||||
|
||||
self.client.start_episode(player_start)
|
||||
|
||||
for frame in count():
|
||||
|
|
|
|||
|
|
@ -0,0 +1,58 @@
|
|||
"""
|
||||
Class to handle occupancy grid
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
import rospy
|
||||
import tf
|
||||
from nav_msgs.msg import OccupancyGrid
|
||||
|
||||
from carla.planner.map import CarlaMap
|
||||
|
||||
|
||||
class MapHandler(object):
|
||||
"""
|
||||
Convert CarlaMap lane image to ROS OccupancyGrid message
|
||||
"""
|
||||
|
||||
def __init__(self, map_name, topic='/map'):
|
||||
self.map_name = map_name
|
||||
self.carla_map = CarlaMap(map_name)
|
||||
self.map_pub = rospy.Publisher(
|
||||
topic, OccupancyGrid, queue_size=10, latch=True)
|
||||
self.build_map_message()
|
||||
|
||||
def build_map_message(self):
|
||||
self.map_msg = map_msg = OccupancyGrid()
|
||||
|
||||
# form array for map
|
||||
map_img = self.carla_map.get_map_lanes()
|
||||
# extract green channel, invert, scale to range 0..100, convert to int8
|
||||
map_img = (100 - map_img[..., 1] * 100.0 / 255).astype(np.int8)
|
||||
map_msg.data = map_img.ravel().tolist()
|
||||
|
||||
# set up general info
|
||||
map_msg.info.resolution = self.carla_map._pixel_density
|
||||
map_msg.info.width = map_img.shape[1]
|
||||
map_msg.info.height = map_img.shape[0]
|
||||
|
||||
# set up origin orientation
|
||||
quat = tf.transformations.quaternion_from_euler(0, 0, np.pi)
|
||||
map_msg.info.origin.orientation.x = quat[0]
|
||||
map_msg.info.origin.orientation.y = quat[1]
|
||||
map_msg.info.origin.orientation.z = quat[2]
|
||||
map_msg.info.origin.orientation.w = quat[3]
|
||||
|
||||
# set up origin position
|
||||
top_right_corner = float(map_img.shape[1]), 0.0
|
||||
to_world = self.carla_map.convert_to_world(top_right_corner)
|
||||
map_msg.info.origin.position.x = to_world[0]
|
||||
map_msg.info.origin.position.y = -to_world[1]
|
||||
map_msg.info.origin.position.z = to_world[2]
|
||||
|
||||
# FIXME: height for Town01 is still in centimeters (issue #541)
|
||||
if self.map_name == 'Town01':
|
||||
map_msg.info.origin.position.z *= 100
|
||||
|
||||
def send_map(self):
|
||||
self.map_pub.publish(self.map_msg)
|
||||
Loading…
Reference in New Issue