From 93227343ffb2e8a91da485029527862d7d76815f Mon Sep 17 00:00:00 2001
From: Alex AUTERNAUD <alex.auternaud@inria.fr>
Date: Fri, 9 Jun 2023 13:28:54 +0200
Subject: [PATCH] local_static_cost_map and local_social_cost_map move out of
this project
---
social_mpc/controller.py | 71 +---------------------------------------
1 file changed, 1 insertion(+), 70 deletions(-)
diff --git a/social_mpc/controller.py b/social_mpc/controller.py
index 9bbea86..9a7d77b 100644
--- a/social_mpc/controller.py
+++ b/social_mpc/controller.py
@@ -17,7 +17,6 @@ import time
import numpy as np
import yaml
import pkg_resources
-import skfmm
import cv2
import os
@@ -464,73 +463,10 @@ class RobotController():
self.flags,
self.lock))
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):
- dst = self.get_static_cost_map(state)
- 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
+ self.local_map = state.local_map
def position_to_px_coord(self, state, pos):
scale = state.config.local_map_scale
@@ -714,11 +650,6 @@ class RobotController():
self.set_targets(state=state)
self.set_mode()
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)
if self.update_goals_enabled or self.path_planner_enabled:
--
GitLab