Mentions légales du service

Skip to content
Snippets Groups Projects
Commit 93227343 authored by AUTERNAUD Alex's avatar AUTERNAUD Alex
Browse files

local_static_cost_map and local_social_cost_map move out of this project

parent 27bc59e8
No related branches found
No related tags found
No related merge requests found
...@@ -17,7 +17,6 @@ import time ...@@ -17,7 +17,6 @@ import time
import numpy as np import numpy as np
import yaml import yaml
import pkg_resources import pkg_resources
import skfmm
import cv2 import cv2
import os import os
...@@ -464,73 +463,10 @@ class RobotController(): ...@@ -464,73 +463,10 @@ class RobotController():
self.flags, self.flags,
self.lock)) self.lock))
self.goal_finder_process.start() self.goal_finder_process.start()
# atexit.register(lambda x: self.on_subprocess_exit("Goal finder"), self.goal_finder_process)
def get_static_cost_map(self, state):
tic = time.time()
if len(state.local_map.shape) == 3:
obstacles = state.local_map[:, :, 0]
elif len(state.local_map.shape) == 2:
obstacles = state.local_map
else:
raise Exception("Wrong size for local map")
if obstacles.max() == 0:
return np.zeros(state.local_map.shape[:2])
scale = state.config.global_map_scale
narrow = state.robot_config.base_footprint_radius * scale
try:
dst = narrow - skfmm.distance(1-obstacles)
dst[dst > narrow] = narrow
dst[dst < 0] = 0
except:
rospy.logwarn("Could not compute signed distance, returning constant cost.")
return np.zeros(state.local_map.shape[:2])
rospy.logdebug("Static cost map time : {0}".format(time.time() - tic))
return dst / narrow
def get_social_cost_map(self, state):
tic = time.time()
groups = None
ids = np.where(state.humans_visible == 0)[0]
# ids = ids[ids != state.humans_id.tolist().index(self.human_target_id)]
n_human = len(ids)
# (x, y, direction, velocity, is_sitting)
humans = np.zeros((n_human, 5))
for j in range(n_human):
i = ids[j]
global_position = state.humans_pose[i, :2]
local_position = global_to_local(state.robot_pose,
global_position)
local_angle = state.humans_pose[i, -
1] - state.robot_pose[-1]
humans[j, :] = * \
local_position, local_angle, state.humans_velocity[i, 0], 0
if n_human > 1:
groups = []
for idx, group_id in enumerate(state.groups_id):
if group_id != -1:
center = global_to_local(state.robot_pose, state.groups_pose[idx])
groups.append(Group(center=center, person_ids=np.where(state.humans_group_id == group_id)[0]))
else:
break
f_dsz = self.ssn.calc_dsz(humans, groups=groups)
rospy.logdebug("Social cost map time : {0}".format(time.time() - tic))
return f_dsz
def compute_cost_map(self, state): def compute_cost_map(self, state):
dst = self.get_static_cost_map(state) self.local_map = state.local_map
if len(dst.shape) == 3:
shape = dst.shape[:2]
dst = dst[:,:,0]
else:
shape = dst.shape
self.local_map = np.zeros((*shape, 3))
# self.local_map[:, :, 0] = dst/dst.max()
self.local_map[:, :, 0] = dst
dsz = self.get_social_cost_map(state)
dsz = cv2.resize(dsz, dst.shape[:2])
self.local_map[:, :, 1] = dsz
def position_to_px_coord(self, state, pos): def position_to_px_coord(self, state, pos):
scale = state.config.local_map_scale scale = state.config.local_map_scale
...@@ -714,11 +650,6 @@ class RobotController(): ...@@ -714,11 +650,6 @@ class RobotController():
self.set_targets(state=state) self.set_targets(state=state)
self.set_mode() self.set_mode()
self.set_weights() self.set_weights()
world_size = state.config.local_map_size
self.ssn = SocialSpaces(
bbox=world_size,
map_shape=(state.config.local_map_height,
state.config.local_map_width))
self.compute_cost_map(state) self.compute_cost_map(state)
if self.update_goals_enabled or self.path_planner_enabled: if self.update_goals_enabled or self.path_planner_enabled:
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment