def _update_color(self, visuals, car_color):
link_names = []
visual_names = []
ambients, diffuses, speculars, emissives = [], [], [], []
for visual_name, link_name in zip(visuals.visual_names, visuals.link_names):
if "car_body_link" in visual_name:
visual_names.append(visual_name)
link_names.append(link_name)
ambient = ColorRGBA(const.COLOR_MAP[car_color].r * 0.1,
const.COLOR_MAP[car_color].g * 0.1,
const.COLOR_MAP[car_color].b * 0.1,
const.COLOR_MAP[car_color].a)
diffuse = ColorRGBA(const.COLOR_MAP[car_color].r * 0.35,
const.COLOR_MAP[car_color].g * 0.35,
const.COLOR_MAP[car_color].b * 0.35,
const.COLOR_MAP[car_color].a)
ambients.append(ambient)
diffuses.append(diffuse)
speculars.append(const.DEFAULT_COLOR)
emissives.append(const.DEFAULT_COLOR)
if len(visual_names) > 0:
req = SetVisualColorsRequest()
req.visual_names = visual_names
req.link_names = link_names
req.ambients = ambients
req.diffuses = diffuses
req.speculars = speculars
req.emissives = emissives
self.set_visual_colors(req)
def update_tracker(self, delta_time, sim_time):
"""
Update all color materials tracking to gazebo
Args:
delta_time (float): delta time
sim_time (Clock): simulation time
"""
with self.lock:
if self.visual_name_map.values():
req = SetVisualColorsRequest()
req.visual_names = self.visual_name_map.values()
req.link_names = self.link_name_map.values()
req.ambients = self.ambient_map.values()
req.diffuses = self.diffuse_map.values()
req.speculars = self.specular_map.values()
req.emissives = self.emissive_map.values()
self.set_visual_colors(req)
self.visual_name_map = OrderedDict()
self.link_name_map = OrderedDict()
self.ambient_map = OrderedDict()
self.diffuse_map = OrderedDict()
self.specular_map = OrderedDict()
self.emissive_map = OrderedDict()
def _randomize(self):
link_names = self.link_visuals_map.keys()
# Unroll all visual names
visual_names = [visual_name for visual_names in self.link_visuals_map.values()
for visual_name in visual_names]
if self.model_randomizer_type == ModelRandomizerType.LINK and self.num_selection > 0:
# Select links to randomize if model_randomizer_type is ModelRandomizerType.LINK
link_names = np.random.choice(self.link_visuals_map.keys(),
size=self.num_selection,
replace=False)
elif self.model_randomizer_type == ModelRandomizerType.VISUAL and self.num_selection > 0:
# Select visuals to randomize if model_randomizer_type is ModelRandomizerType.VISUAL
visual_names = np.random.choice(visual_names,
size=self.num_selection,
replace=False)
# Convert to set
visual_names = set(visual_names)
# Model-level random color
color = self._get_random_color()
ambient = color
diffuse = color
specular = ColorRGBA(0.0, 0.0, 0.0, 1.0)
emissive = ColorRGBA(0.0, 0.0, 0.0, 1.0)
req = SetVisualColorsRequest()
req.visual_names = []
req.link_names = []
req.ambients = []
req.diffuses = []
req.speculars = []
req.emissives = []
for link_name in link_names:
for idx, visual_name in enumerate(self.link_visuals_map[link_name]):
if visual_name not in visual_names:
continue
req.visual_names.append(visual_name)
req.link_names.append(link_name)
req.ambients.append(ambient)
req.diffuses.append(diffuse)
req.speculars.append(specular)
req.emissives.append(emissive)
if self.model_randomizer_type == ModelRandomizerType.VISUAL:
# Visual-level random color
color = self._get_random_color()
ambient = color
diffuse = color
if self.model_randomizer_type == ModelRandomizerType.LINK:
# Link-level random color
color = self._get_random_color()
ambient = color
diffuse = color
self.set_visual_colors(req)
def set_visual_color(self,
visual_name,
link_name,
ambient,
diffuse,
specular,
emissive,
blocking=False):
"""
Set Material that will be updated in next update call
Args:
visual_name (str): name of visual
link_name (str): name of the link holding visual
ambient (ColorRBGA): ambient color
diffuse (ColorRBGA): diffuse color
specular (ColorRBGA): specular color
emissive (ColorRBGA): emissive color
blocking (bool): flag to block or not
Returns:
msg (SetVisualColorResponse)
"""
msg = SetVisualColorResponse()
msg.success = True
key = (visual_name, link_name)
with self.lock:
if blocking:
if key in self.visual_name_map:
del self.visual_name_map[key]
del self.link_name_map[key]
del self.ambient_map[key]
del self.diffuse_map[key]
del self.specular_map[key]
del self.emissive_map[key]
req = SetVisualColorsRequest()
req.visual_names = [visual_name]
req.link_names = [link_name]
req.ambients = [ambient]
req.diffuses = [diffuse]
req.speculars = [specular]
req.emissives = [emissive]
res = self.set_visual_colors(req)
msg.success = res.success and res.status[0]
msg.status_message = res.messages[
0] if res.success else res.status_message
else:
self.visual_name_map[key] = visual_name
self.link_name_map[key] = link_name
self.ambient_map[key] = ambient
self.diffuse_map[key] = diffuse
self.specular_map[key] = specular
self.emissive_map[key] = emissive
return msg
def update_color(self, visuals, car_color):
"""Update the model's color using it's visuals
Args:
visuals (Visuals): The visuals of the current model
car_color (str): The car color we want to update the visuals to.
e.g. Purple, Orange, White, Black, etc.
"""
link_names = []
visual_names = []
ambients, diffuses, speculars, emissives = [], [], [], []
for visual_name, link_name in zip(visuals.visual_names, visuals.link_names):
if "car_body_link" in visual_name:
visual_names.append(visual_name)
link_names.append(link_name)
ambient = ColorRGBA(
const.COLOR_MAP[car_color].r * 0.1,
const.COLOR_MAP[car_color].g * 0.1,
const.COLOR_MAP[car_color].b * 0.1,
const.COLOR_MAP[car_color].a,
)
diffuse = ColorRGBA(
const.COLOR_MAP[car_color].r * 0.35,
const.COLOR_MAP[car_color].g * 0.35,
const.COLOR_MAP[car_color].b * 0.35,
const.COLOR_MAP[car_color].a,
)
ambients.append(ambient)
diffuses.append(diffuse)
speculars.append(const.DEFAULT_COLOR)
emissives.append(const.DEFAULT_COLOR)
if len(visual_names) > 0:
req = SetVisualColorsRequest()
req.visual_names = visual_names
req.link_names = link_names
req.ambients = ambients
req.diffuses = diffuses
req.speculars = speculars
req.emissives = emissives
self._set_visual_colors(req)