diff --git a/src/robot_behavior/launch/behavior_local_path_planner_action_server.launch b/src/robot_behavior/launch/behavior_local_path_planner_action_server.launch
index 0cb724b41f5747c702a50786f8d869fa8fd60374..155f609d0a0e1c8b3094825b94bfa0fab512d1d9 100644
--- a/src/robot_behavior/launch/behavior_local_path_planner_action_server.launch
+++ b/src/robot_behavior/launch/behavior_local_path_planner_action_server.launch
@@ -14,6 +14,7 @@
<arg name="final_goal_frame" default="final_goal"/>
<arg name="intermediate_waypoint_goal_frame" default="intermediate_waypoint_goal"/>
<arg name="cmd_vel_topic" default="/nav_vel"/>
+ <arg name="rl_publishing_rate" default="10"/>
<!-- <group if="$(arg social_mpc)"> -->
@@ -41,6 +42,8 @@
<param name="final_goal_frame" type="str" value="$(arg final_goal_frame)"/>
<param name="intermediate_waypoint_goal_frame" type="str" value="$(arg intermediate_waypoint_goal_frame)"/>
<param name="cmd_vel_topic" type="str" value="$(arg cmd_vel_topic)"/>
+ <param name="namespace_slam" type="str" value="$(arg namespace_slam)"/>
+ <param name="rl_publishing_rate" type="int" value="$(arg rl_publishing_rate)"/>
</node>
<!-- </group> -->
</launch>
diff --git a/src/robot_behavior/launch/new_main.launch b/src/robot_behavior/launch/new_main.launch
index 5a4af0b6128e85c6e240c1793604bc295b75b043..4070116653aa6fba88c6827b0004d86a7ee95504 100644
--- a/src/robot_behavior/launch/new_main.launch
+++ b/src/robot_behavior/launch/new_main.launch
@@ -56,14 +56,14 @@
<arg name="go_to_position_action" default="go_to_position_action"/>
<arg name="controller_status_topic" default="controller_status"/>
<arg name="default_timeout" default="200"/>
- <arg name="planning_manager_loop_time" default="2"/>
- <arg name="global_path_loop_time" default="2"/>
+ <arg name="planning_manager_loop_time" default="1"/>
+ <arg name="global_path_loop_time" default="1"/>
<arg name="goal_finder_loop_time" default="2"/>
<arg name="final_goal_frame" default="final_goal"/>
<arg name="intermediate_waypoint_goal_frame" default="intermediate_waypoint_goal"/>
<arg name="pan_goal_frame" default="pan_goal"/>
<arg name="human_goal_frame" default="human_goal"/>
- <arg name="local_path_planner_name" default="social_mpc"/>
+ <arg name="local_path_planner_name" default="social_rl"/>
<arg name="cmd_vel_topic" default="/nav_vel"/>
diff --git a/src/robot_behavior/src/robot_behavior/__init__.py b/src/robot_behavior/src/robot_behavior/__init__.py
index ab5d4dcd56107484ba667ecec229bd64f460dd2a..5a2eb02dfd9ba82ae69dcca14667f0d79fce2f8e 100755
--- a/src/robot_behavior/src/robot_behavior/__init__.py
+++ b/src/robot_behavior/src/robot_behavior/__init__.py
@@ -1,6 +1,6 @@
-from .behavior_global_path_planner import GlobalPathPlanner
-from .behavior_goal_finder import GoalFinder
-from .behavior_local_path_planner_mpc import LocalPathPlannerMPC
+from .utils.behavior_global_path_planner import GlobalPathPlanner
+from .utils.behavior_goal_finder import GoalFinder
+from .utils.behavior_local_path_planner_mpc import LocalPathPlannerMPC
from .behavior_generator_node import BehaviorGenerator
from .behavior_planning_manager_node import PlanningManagerNode
from .behavior_social_cost_map_node import SocialCostMapNode
@@ -23,4 +23,5 @@ from .behavior_look_at_group_action_server import LookAtGroupActionServer
from .behavior_look_at_person_action_client import LookAtPersonActionClient
from .behavior_look_at_person_action_server import LookAtPersonActionServer
from .behavior_look_at_position_action_client import LookAtPositionActionClient
-from .behavior_look_at_position_action_server import LookAtPositionActionServer
\ No newline at end of file
+from .behavior_look_at_position_action_server import LookAtPositionActionServer
+from .utils.drl_local_path_planner import LocalPathPlannerDRL
\ No newline at end of file
diff --git a/src/robot_behavior/src/robot_behavior/behavior_generator_node.py b/src/robot_behavior/src/robot_behavior/behavior_generator_node.py
index 3f2d0cbe2667dc5decb0f2adb810d31cf89d3ea2..8989aca616d27955d598a019bbfc86badb838dff 100755
--- a/src/robot_behavior/src/robot_behavior/behavior_generator_node.py
+++ b/src/robot_behavior/src/robot_behavior/behavior_generator_node.py
@@ -22,9 +22,9 @@ from nav_msgs.msg import Odometry, OccupancyGrid, MapMetaData
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
-from robot_behavior.utils import constraint_angle, meter_to_px
+from robot_behavior.utils.utils import constraint_angle, meter_to_px
from spring_msgs.msg import TrackedPersons2d, ControllerStatus, GoToPosition, GoToEntity, LookAtEntity, LookAtPosition
-from robot_behavior.ros_4_hri_interface import ROS4HRIInterface
+from robot_behavior.utils.ros_4_hri_interface import ROS4HRIInterface
class BehaviorGenerator:
diff --git a/src/robot_behavior/src/robot_behavior/behavior_global_path_planner_action_server.py b/src/robot_behavior/src/robot_behavior/behavior_global_path_planner_action_server.py
index b1b3cc9c35def4856f994be7f9979e3ce71a581c..9a0a4171121072ae34aece35230476a00a066e12 100755
--- a/src/robot_behavior/src/robot_behavior/behavior_global_path_planner_action_server.py
+++ b/src/robot_behavior/src/robot_behavior/behavior_global_path_planner_action_server.py
@@ -8,11 +8,11 @@ import cv2
import sys
import rospy
from nav_msgs.msg import OccupancyGrid, MapMetaData
-from robot_behavior.behavior_global_path_planner import GlobalPathPlanner
+from robot_behavior.utils.behavior_global_path_planner import GlobalPathPlanner
from social_mpc.config.config import ControllerConfig
import time
import actionlib
-from robot_behavior.utils import constraint_angle
+from robot_behavior.utils.utils import constraint_angle
from spring_msgs.msg import GlobalPlannerAction, GlobalPlannerFeedback, GlobalPlannerResult
diff --git a/src/robot_behavior/src/robot_behavior/behavior_goal_finder_action_server.py b/src/robot_behavior/src/robot_behavior/behavior_goal_finder_action_server.py
index afae120f94884e7fd85b6c1caeffbc1195ce369a..32ba0ebfb5e80a32ee2f767133d07b5699fe06fb 100755
--- a/src/robot_behavior/src/robot_behavior/behavior_goal_finder_action_server.py
+++ b/src/robot_behavior/src/robot_behavior/behavior_goal_finder_action_server.py
@@ -8,12 +8,12 @@ import yaml
import numpy as np
import rospy
from nav_msgs.msg import OccupancyGrid
-from robot_behavior.behavior_goal_finder import GoalFinder
-from robot_behavior.utils import constraint_angle
+from robot_behavior.utils.behavior_goal_finder import GoalFinder
+from robot_behavior.utils.utils import constraint_angle
from robot_behavior.behavior_generator_node import RobotState
from social_mpc.config.config import ControllerConfig, RobotConfig
import time
-from robot_behavior.ros_4_hri_interface import ROS4HRIInterface
+from robot_behavior.utils.ros_4_hri_interface import ROS4HRIInterface
from spring_msgs.msg import GoalFinderAction, GoalFinderFeedback, GoalFinderResult
diff --git a/src/robot_behavior/src/robot_behavior/behavior_mpc_local_path_planner_action_server.py b/src/robot_behavior/src/robot_behavior/behavior_mpc_local_path_planner_action_server.py
index a3775765214d767dba09ddd00ddfeafdd60fec5a..e427d71e4bdb1c03d4e69c17e8da350ec1b06fb7 100755
--- a/src/robot_behavior/src/robot_behavior/behavior_mpc_local_path_planner_action_server.py
+++ b/src/robot_behavior/src/robot_behavior/behavior_mpc_local_path_planner_action_server.py
@@ -7,9 +7,9 @@ import yaml
import sys
import rospy
from nav_msgs.msg import OccupancyGrid
-from robot_behavior.utils import constraint_angle, global_to_local
+from robot_behavior.utils.utils import constraint_angle, global_to_local
from social_mpc.config.config import ControllerConfig, RobotConfig
-from robot_behavior.behavior_local_path_planner_mpc import LocalPathPlannerMPC
+from robot_behavior.utils.behavior_local_path_planner_mpc import LocalPathPlannerMPC
import time
import actionlib
from spring_msgs.msg import LocalPlannerAction, LocalPlannerFeedback, LocalPlannerResult
diff --git a/src/robot_behavior/src/robot_behavior/behavior_planning_manager_node.py b/src/robot_behavior/src/robot_behavior/behavior_planning_manager_node.py
index 3e99e35bbae25a4d5fbe646953772a9a36641eee..d6e412c33e0c9c5ca879b2ab8a70d8e15a2ba152 100755
--- a/src/robot_behavior/src/robot_behavior/behavior_planning_manager_node.py
+++ b/src/robot_behavior/src/robot_behavior/behavior_planning_manager_node.py
@@ -19,14 +19,14 @@ from spring_msgs.msg import (GoToPosition,
from actionlib_msgs.msg import GoalStatus
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from sensor_msgs.msg import JointState
-from robot_behavior.utils import constraint_angle
+from robot_behavior.utils.utils import constraint_angle
import tf
import os
import sys
import yaml
import time
import numpy as np
-from robot_behavior.ros_4_hri_interface import ROS4HRIInterface
+from robot_behavior.utils.ros_4_hri_interface import ROS4HRIInterface
from social_mpc.config.config import ControllerConfig, RobotConfig
import pkg_resources
@@ -718,7 +718,6 @@ class PlanningManagerNode:
self.setting_goals = False
-
def _go_to_body_callback(self, data):
self.reset_go_to_data()
diff --git a/src/robot_behavior/src/robot_behavior/behavior_rl_local_path_planner_action_server.py b/src/robot_behavior/src/robot_behavior/behavior_rl_local_path_planner_action_server.py
index 2dd1094f1f3c4b65681ce50598e4e17dda1843f4..9ae2be604d9adf3314b35ed09bf3d7249fb9f4dc 100755
--- a/src/robot_behavior/src/robot_behavior/behavior_rl_local_path_planner_action_server.py
+++ b/src/robot_behavior/src/robot_behavior/behavior_rl_local_path_planner_action_server.py
@@ -1,18 +1,16 @@
#!/usr/bin/env python3
import numpy as np
-import pkg_resources
import tf
-import os
-import yaml
-import sys
import rospy
-from nav_msgs.msg import OccupancyGrid
-from robot_behavior.utils import constraint_angle
-import time
+from nav_msgs.msg import OccupancyGrid, MapMetaData
+from robot_behavior.utils.utils import constraint_angle
import actionlib
from spring_msgs.msg import LocalPlannerAction, LocalPlannerFeedback, LocalPlannerResult
from geometry_msgs.msg import Twist, Vector3
-
+from robot_behavior.utils.drl_local_path_planner import LocalPathPlannerDRL
+from robot_behavior.utils.utils_drl_local_path_planner import *
+from human_aware_navigation_rl.agent.sac.sac import SAC
+import exputils as eu
class RLLocalPathPlannerActionServer:
def __init__(self, name):
@@ -20,12 +18,14 @@ class RLLocalPathPlannerActionServer:
self.map_frame = rospy.get_param('~map_frame', 'map')
self.robot_frame = rospy.get_param('~robot_frame', 'base_footprint')
- self.local_occupancy_map_topic = rospy.get_param('~local_occupancy_map_topic', '/slam/local_occupancy_map')
+ self.local_occupancy_map_topic = rospy.get_param('~local_occupancy_map_topic', '/slam/local_map_rl_navigation')
self.local_social_cost_map_topic = rospy.get_param('~local_social_cost_map_topic', '/slam/local_social_cost_map')
self.timer_error_max_time = rospy.get_param('~timer_error_max_time', 5) # seconds
self.final_goal_frame = rospy.get_param('~final_goal_frame', 'final_goal')
self.intermediate_waypoint_goal_frame = rospy.get_param('~intermediate_waypoint_goal_frame', 'intermediate_waypoint_goal')
self.cmd_vel_topic = rospy.get_param('~cmd_vel_topic', '/nav_vel')
+ self.namespace_slam = rospy.get_param('~namespace_slam', '/rtabmap')
+ self.rl_publishing_rate = rospy.get_param('~rl_publishing_rate', 10)
self._feedback = LocalPlannerFeedback()
self._result = LocalPlannerResult()
@@ -34,16 +34,63 @@ class RLLocalPathPlannerActionServer:
self.local_social_cost_map_data = None
self.rtabmap_ready = False
self.local_map_static = None
- self.action_idx = None
+ self.local_social_cost_map = None
+
+ self.x_wp = 0.
+ self.y_wp = 0.
+ self.yaw_wp = 0.
+ self.robot_x_position = 0.
+ self.robot_y_position = 0.
+ self.robot_yaw = 0.
+ self.robot_position = [0, 0]
self.intermediate_waypoint = None
self.tf_robot_pose_error = False
self.tf_intermediate_wp_pose_error = False
self.flag_actions_okay = False
- self.flag_inital_step_mpc = True
- self.rate = 10
+ # Trained with obstacle
+ # folder_path = '/home/ros/robot_behavior_ws/modules/human_aware_navigation_rl/human_aware_navigation_rl/trained_models/campaign_real_env_obstacles/experiments/'
+ # import_weight = eu.AttrDict(
+ # navigation_encoder_actor = folder_path+'experiment_000010/repetition_000000/checkpoints/final_nav_encoder_actor_state_dict.pt',
+ # actor = folder_path+'experiment_000010/repetition_000000/checkpoints/final_actor_state_dict.pt',
+ # critic = folder_path+'experiment_000010/repetition_000000/checkpoints/final_critic_state_dict.pt'
+ # )
+
+ # Trained without obstacle
+ folder_path = '/home/ros/robot_behavior_ws/modules/human_aware_navigation_rl/human_aware_navigation_rl/trained_models/campaign_real_env_warning_zone_bis/experiments/'
+ import_weight = eu.AttrDict(
+ navigation_encoder_actor = folder_path+'experiment_000013/repetition_000000/checkpoints/final_nav_encoder_actor_state_dict.pt',
+ actor = folder_path+'experiment_000013/repetition_000000/checkpoints/final_actor_state_dict.pt',
+ critic = folder_path+'experiment_000013/repetition_000000/checkpoints/final_critic_state_dict.pt'
+ )
+
+ config_agent = eu.AttrDict(
+ list_of_input_to_memorize = [['static_occupancy_grid'],['angle_to_goal_position', 'distance_to_goal_position']],
+ list_of_input_to_encode = [['static_occupancy_grid'],['angle_to_goal_position', 'distance_to_goal_position']],
+ latent_shape = 288,
+ normalize_latent_space = True,
+ )
+ config_env = eu.AttrDict(
+ raycast_field_of_view = 360,
+ raycast_angle_between_two_ray=10,
+ vmin = 0,
+ vmax = 0.5,
+ wmin = -0.5,
+ wmax = 0.5,
+ grid_shape=(1,60,60),
+ local_map_depth = 3
+
+ )
+ class_agent_name = rospy.get_param('~agent_class', 'SAC')
+ # Creation of class attribute
+ class_agent = eval(class_agent_name)
+ self.agent = class_agent(env=config_env, import_weight = import_weight, config=config_agent, device = "cpu")
+ self.rl_path_planner = LocalPathPlannerDRL(list_of_input_to_encode = self.agent.unsorted_list_of_input, config_env=config_env)
+ self.goal_position = [0., 0.]
+ self.observation=None
+ self.lin_vel, self.ang_vel = 0, 0
self.init_ros_subscriber_and_publicher()
@@ -62,7 +109,7 @@ class RLLocalPathPlannerActionServer:
def execute_cb(self, goal):
# helper variables
- rate = rospy.Rate(self.rate)
+ rate = rospy.Rate(self.rl_publishing_rate)
self.init_time = rospy.Time.now()
self.current_time = (rospy.Time.now() - self.init_time).to_sec()
self.intermediate_waypoint = None
@@ -123,9 +170,10 @@ class RLLocalPathPlannerActionServer:
self._a_server.set_aborted(self._result)
break
+ self.flag_actions_okay = True
self.step(self.current_time)
- self.action_idx = 0
+ self.flag_actions_okay = False
self._result_msg(goal)
@@ -166,28 +214,21 @@ class RLLocalPathPlannerActionServer:
rospy.logdebug('tf_intermediate_wp_pose_error: {}'.format(self.tf_intermediate_wp_pose_error))
rospy.logdebug('tf_robot_pose_error'.format(self.tf_robot_pose_error))
- self.local_map[:, :, 0] = self.local_static_map
- self.local_map[:, :, 1] = self.local_social_cost_map
-
- # call Victor's step function
- # if self.do_step():
- # rospy.logdebug('self.goto_goal {}'.format(self.goto_goal))
- # actions = np.roll(self.actions, -(self.action_idx + 1), axis=0)
- # actions[-(self.action_idx + 1):] = np.tile(self.actions[-1, :], 1)
- # self.actions = self.mpc.step(
- # state=np.array([0.]), # not used anymore
- # actions=actions,
- # weights=self.weights,
- # reg_parameter=self.reg_parameter,
- # loss_coef=self.loss_coef,
- # loss_rad=self.loss_rad,
- # cost_map=self.local_map[:, :, :-1],
- # goto_goal=self.goto_goal,
- # pan_goal=None,
- # human_features=None
- # )
- # else:
- # self.actions = np.copy(self.initial_action)
+ if self.goal_position:
+ print('self.goal_position', self.goal_position)
+ if self.observation is not None:
+ print('--------IF------')
+ self.observation.linear_velocity = self.lin_vel
+ self.observation.angular_velocity = self.ang_vel
+ action, prediction = self.rl_path_planner.predict(agent = self.agent, observation = self.observation)
+
+ self.lin_vel = action[0]
+ self.ang_vel = action[1]
+ else:
+ print('--------ELSE------')
+ self.lin_vel=0
+ self.ang_vel=0
+ print('self.observation', self.observation)
def init_ros_subscriber_and_publicher(self):
@@ -204,8 +245,14 @@ class RLLocalPathPlannerActionServer:
### ROS Publishers
self._cmd_vel_pub = rospy.Publisher(self.cmd_vel_topic, Twist, queue_size=1)
self._publishers.append(self._cmd_vel_pub)
+ self._local_map_pub = rospy.Publisher(self.namespace_slam + '/occupancy_grid_for_drl_nav', OccupancyGrid, queue_size=10, latch=True)
+ self._local_map_raycast_pub = rospy.Publisher(self.namespace_slam + '/raycast_for_drl_nav', OccupancyGrid, queue_size=10, latch=True)
+ self._local_map_closest_obstacle_pub = rospy.Publisher(self.namespace_slam + '/closest_obstacle_map_for_drl_nav', OccupancyGrid, queue_size=10, latch=True)
+ self._publishers.append(self._local_map_pub)
+ self._publishers.append(self._local_map_raycast_pub)
+ self._publishers.append(self._local_map_closest_obstacle_pub)
- self._timers.append(rospy.Timer(rospy.Duration(1/self.rate), callback=self._pub_vel_callback))
+ self._timers.append(rospy.Timer(rospy.Duration(1/self.rl_publishing_rate), callback=self._pub_vel_callback))
rospy.loginfo("Initializing the %s " % self._action_name)
@@ -235,25 +282,14 @@ class RLLocalPathPlannerActionServer:
def _pub_vel_callback(self, event):
- if not self.flag_inital_step_mpc:
- if self.action_idx is not None:
- actions = self.actions[self.action_idx]
- self.lin_vel = 0.
- self.ang_vel = 0.
- if np.any(actions):
- self.publish_vel(actions)
- self.action_idx += 1
- if self.action_idx > (len(self.actions) - 1):
- self.action_idx = 0
- self.actions = np.copy(self.initial_action)
-
-
- def publish_vel(self, action):
- pan_vel, ang, lin = action[0], action[1], action[2]
- self.lin_vel, self.ang_vel = lin, ang # self._check_joint_vel_limits(lin, ang)
- twist_msg = Twist(Vector3(self.lin_vel, 0, 0), Vector3(0, 0, self.ang_vel))
- self._cmd_vel_pub.publish(twist_msg)
- rospy.loginfo("vel to robot : {}, {} -> {} {}".format(lin, ang, self.lin_vel, self.ang_vel))
+ r''' Function that checks the velocities in the memory and send it to the robot
+
+ Warning : This functions locks the reading of the velocitites while sending it to the robot '''
+ if self.flag_actions_okay:
+ self.lin_vel, self.ang_vel = self._check_joint_vel_limits(self.lin_vel, self.ang_vel)
+ print('lin_vel: {} \t ang_vel: {}'.format(self.lin_vel, self.ang_vel))
+ twist_msg = Twist(Vector3(self.lin_vel, 0, 0), Vector3(0, 0, self.ang_vel))
+ self._cmd_vel_pub.publish(twist_msg)
def get_waypoint_pose(self):
@@ -271,6 +307,7 @@ class RLLocalPathPlannerActionServer:
map_2_waypoint[1][3]
])
self.yaw_wp = float(constraint_angle(yaw))
+ self.goal_position = [self.x_wp, self.y_wp]
return True
@@ -308,7 +345,58 @@ class RLLocalPathPlannerActionServer:
self.local_map_size = [[self.x_local_map, self.x_local_map + self.local_map_width*self.local_map_resolution],[self.y_local_map, self.y_local_map + self.local_map_height*self.local_map_resolution]]
self.last_shape_local_map = (self.local_map_height, self.local_map_width)
self.local_static_map = (np.asarray(self.local_map_data.data) / 100).reshape(self.last_shape_local_map)
-
+ flag_updated_robot_pose = self.get_robot_pose()
+
+ if flag_updated_robot_pose and self.local_social_cost_map is not None:
+ self.observation, self.raycast_map, self.closest_obstacle_map = self.rl_path_planner.compute_observation_from_data(
+ static_occupancy_grid=np.flip(self.local_static_map, axis=0),
+ dynamic_social_grid =np.flip(self.local_social_cost_map, axis=0),
+ robot_position=self.robot_position,
+ robot_orientation=self.robot_yaw,
+ goal_position=np.array(self.goal_position),
+ linear_velocity=self.lin_vel,
+ angular_velocity=self.ang_vel,
+ goal_yaw=self.yaw_wp
+ )
+ if self.raycast_map is not None :
+ self.publish_raycast_map(self.raycast_map)
+ if self.closest_obstacle_map is not None :
+ self.publish_closest_obstacle_map(self.closest_obstacle_map)
+ if self.observation.static_occupancy_grid is not None :
+ self.publish_local_map(np.flip(self.observation.static_occupancy_grid, axis=1))
+
+
+ def publish_raycast_map(self, map):
+ r''' Publish raycast map '''
+ self._local_map_raycast_pub.publish(self.set_occupancy_grid_object(map))
+
+
+ def publish_closest_obstacle_map(self, map):
+ r''' Publish closest obstacle map '''
+ self._local_map_closest_obstacle_pub.publish(self.set_occupancy_grid_object(map))
+
+
+ def publish_local_map(self, map):
+ r''' Publish local occupancy grid map '''
+ self._local_map_pub.publish(self.set_occupancy_grid_object(map))
+
+
+ def set_occupancy_grid_object(self, map):
+ r''' Set the occupancy grid map object from the map'''
+ occupancy_grid = OccupancyGrid()
+ occupancy_grid.info = MapMetaData()
+ occupancy_grid.info.map_load_time = rospy.Time.now()
+ resolution = self.local_map_data.info.resolution
+ if self.local_map_width != np.shape(map)[-1] :
+ resolution = self.local_map_data.info.resolution * self.local_map_width/np.shape(map)[-1]
+ occupancy_grid.info.resolution = resolution
+ occupancy_grid.info.width = np.shape(map)[1]
+ occupancy_grid.info.height = np.shape(map)[1]
+ occupancy_grid.info.origin.position.x = self.local_map_data.info.origin.position.x
+ occupancy_grid.info.origin.position.y = self.local_map_data.info.origin.position.y
+ occupancy_grid.data = (map*100).flatten().astype(np.int8)
+ occupancy_grid.header = self.local_map_data.header
+ return occupancy_grid
def _local_social_cost_map_callback(self, data):
self.local_social_cost_map_data = data
@@ -320,8 +408,25 @@ class RLLocalPathPlannerActionServer:
self.local_social_cost_map_size = [[self.x_local_social_cost_map, self.x_local_social_cost_map + self.local_social_cost_map_width*self.local_social_cost_map_resolution],[self.y_local_social_cost_map, self.y_local_social_cost_map + self.local_social_cost_map_height*self.local_social_cost_map_resolution]]
self.last_shape_local_social_cost_map = (self.local_social_cost_map_height, self.local_social_cost_map_width)
self.local_social_cost_map = (np.asarray(self.local_social_cost_map_data.data) / 100).reshape(self.last_shape_local_social_cost_map)
-
-
+ self.local_social_cost_map = cv2.resize(self.local_social_cost_map, (60,60), interpolation = cv2.INTER_NEAREST)
+
+ def _check_joint_vel_limits(self, lin_vel, ang_vel):
+ flag_high_forward_speed = False
+ if lin_vel > self.rl_path_planner.forward_velocity_limit[1]:
+ rospy.logwarn("The linear velocity is greater than the maximum allowed. The maximum velocity is set.")
+ lin_vel = self.rl_path_planner.forward_velocity_limit[1]
+ flag_high_forward_speed = True
+ if lin_vel < self.rl_path_planner.forward_velocity_limit[0]:
+ rospy.logwarn("The linear velocity is lower than the minimum allowed. The minimum velocity is set.")
+ lin_vel = self.rl_path_planner.forward_velocity_limit[0]
+ if ang_vel > self.rl_path_planner.angular_velocity_limit[1] and flag_high_forward_speed:
+ rospy.logwarn("The angular velocity is greater than the maximum allowed. The maximum velocity is set.")
+ ang_vel = self.rl_path_planner.angular_velocity_limit[1]
+ if ang_vel < self.rl_path_planner.angular_velocity_limit[0] and flag_high_forward_speed:
+ rospy.logwarn("The angular velocity is lower than the minimum allowed. The minimum velocity is set.")
+ ang_vel = self.rl_path_planner.angular_velocity_limit[0]
+ return lin_vel, ang_vel
+
def _check_publishers_connection(self):
"""
Checks that all the publishers are working
diff --git a/src/robot_behavior/src/robot_behavior/behavior_global_path_planner.py b/src/robot_behavior/src/robot_behavior/utils/behavior_global_path_planner.py
similarity index 99%
rename from src/robot_behavior/src/robot_behavior/behavior_global_path_planner.py
rename to src/robot_behavior/src/robot_behavior/utils/behavior_global_path_planner.py
index 67449d2c9a8d1b862a969b10eccd22c789448e55..174096fcd14904fdfb1cfa111f2e1d55b80e4886 100644
--- a/src/robot_behavior/src/robot_behavior/behavior_global_path_planner.py
+++ b/src/robot_behavior/src/robot_behavior/utils/behavior_global_path_planner.py
@@ -16,7 +16,7 @@ import cv2
import skfmm
import numpy.ma as ma
import numpy as np
-from robot_behavior.utils import constraint_angle, optimal_path_2d
+from robot_behavior.utils.utils import constraint_angle, optimal_path_2d
import rospy
import exputils as eu
diff --git a/src/robot_behavior/src/robot_behavior/behavior_goal_finder.py b/src/robot_behavior/src/robot_behavior/utils/behavior_goal_finder.py
similarity index 100%
rename from src/robot_behavior/src/robot_behavior/behavior_goal_finder.py
rename to src/robot_behavior/src/robot_behavior/utils/behavior_goal_finder.py
diff --git a/src/robot_behavior/src/robot_behavior/behavior_local_path_planner_mpc.py b/src/robot_behavior/src/robot_behavior/utils/behavior_local_path_planner_mpc.py
similarity index 99%
rename from src/robot_behavior/src/robot_behavior/behavior_local_path_planner_mpc.py
rename to src/robot_behavior/src/robot_behavior/utils/behavior_local_path_planner_mpc.py
index 5b1b2c3b01157f26b76378d334414d95d7f62672..5d1c09d3250dbd299261354c05ab157f5db0ad70 100755
--- a/src/robot_behavior/src/robot_behavior/behavior_local_path_planner_mpc.py
+++ b/src/robot_behavior/src/robot_behavior/utils/behavior_local_path_planner_mpc.py
@@ -18,7 +18,7 @@ import jax.numpy as np
import jax
from jax import grad, jit, vmap, jacfwd, custom_jvp, partial
import rospy
-from robot_behavior.utils import local_to_global_jax
+from robot_behavior.utils.utils import local_to_global_jax
config.update("jax_enable_x64", True)
# config.update('jax_disable_jit', True)
diff --git a/src/robot_behavior/src/robot_behavior/utils/drl_local_path_planner.py b/src/robot_behavior/src/robot_behavior/utils/drl_local_path_planner.py
new file mode 100755
index 0000000000000000000000000000000000000000..107c17567e9cd658bb36a119e29c10a267b70c43
--- /dev/null
+++ b/src/robot_behavior/src/robot_behavior/utils/drl_local_path_planner.py
@@ -0,0 +1,178 @@
+import numpy as np
+import exputils as eu
+import cv2
+from robot_behavior.utils.utils_drl_local_path_planner import *
+from human_aware_navigation_rl.agent.sac.sac import SAC
+from human_aware_navigation_rl.agent.ddqn.ddqn import DDQN
+from human_aware_navigation_rl.utils.distance_grid_from_occupancy_grid import raycast_from_occupancy_grid
+from human_aware_navigation_rl.utils.distance_grid_from_occupancy_grid import distance_and_angle_to_nearest_obstacle_from_occupancy_grid
+
+class LocalPathPlannerDRL:
+ r''' LocalPathPlanner based of Deep Reinforcement Learning trained agent '''
+ def __init__(self, list_of_input_to_encode, config_env):
+
+ # Observation Space
+ self.list_of_input_to_encode = list_of_input_to_encode
+ self.nb_channels = 0
+ self.grid_data_dict = eu.AttrDict()
+ self.row, self.col = config_env.grid_shape[-2], config_env.grid_shape[-1]
+ self.resolution = 2*config_env.local_map_depth/self.row
+ self.local_map_depth = config_env.local_map_depth
+ for key in self.list_of_input_to_encode:
+ if 'grid' in key :
+ self.nb_channels+=1
+ self.grid_data_dict[key] = np.zeros((1, self.row, self.col))
+
+ # Action Space
+ self.action_list = set_action_list()
+ self.forward_velocity_limit = [config_env.vmin, config_env.vmax]
+ self.angular_velocity_limit = [config_env.wmin, config_env.wmax]
+
+ # Coordinte of goal
+ self.distance_to_goal = np.inf
+ self.angle_to_goal = 0
+
+ # Orientation data
+ self.goal_orientation = None
+ self.robot_orientation = None
+
+ # Flags
+ self.flag_collision = False
+
+ def compute_observation_from_data(self,
+ static_occupancy_grid,
+ dynamic_social_grid,
+ robot_position,
+ robot_orientation,
+ goal_position,
+ linear_velocity=0,
+ angular_velocity=0,
+ goal_yaw=None):
+ r"""
+ DRL Agent Step.
+
+ Args:
+ local_occupancy_grid : numpy.array of size (1, N, N)
+
+ goal_polar_coordinates : numpy.array of size (1, 2)
+
+ Returns:
+ int x int : linear and angular velocity
+ """
+ observation = eu.AttrDict()
+
+ self.update_grid_dict(
+ static_occupancy_grid=static_occupancy_grid,
+ dynamic_social_grid=dynamic_social_grid
+ )
+
+ raycast_map = None
+ closest_obs_map = None
+
+ self.goal_orientation = goal_yaw
+ self.robot_orientation = robot_orientation
+
+ closest_obs_map, distance_to_closest_object, angle_to_closest_object = distance_and_angle_to_nearest_obstacle_from_occupancy_grid(
+ occupancy_grid=self.grid_data_dict.static_occupancy_grid[0,:,:],
+ resolution = self.resolution
+ )
+
+ if distance_to_closest_object < 0.1 :
+ self.flag_collision = True
+ else :
+ self.flag_collision = False
+
+ if 'raycast' in self.list_of_input_to_encode :
+ raycast_distance, raycast_angles, raycast_map = raycast_from_occupancy_grid(
+ occupancy_grid=self.grid_data_dict.static_occupancy_grid[0,:,:],
+ resolution = self.resolution,
+ field_of_view=360,
+ angle_between_rays=10,
+ draw_ray = True
+ )
+ raycast_distance = np.clip(raycast_distance-0.2, 0, self.local_map_depth)
+
+
+ for key in self.list_of_input_to_encode :
+ if 'grid' in key:
+ observation[key] = self.grid_data_dict[key]
+
+ elif key == 'raycast' :
+ observation.raycast = raycast_distance
+
+ elif key == 'angle_to_goal_position' :
+ self.angle_to_goal = measure_relative_angle(robot_position, robot_orientation, goal_position)
+ observation.angle_to_goal_position = self.angle_to_goal
+
+ elif key == 'distance_to_goal_position' :
+ self.distance_to_goal = measure_distance(robot_position, goal_position)
+ observation.distance_to_goal_position = self.distance_to_goal
+
+ elif key == 'angle_to_closest_object' :
+ observation.angle_to_closest_object = angle_to_closest_object
+
+ elif key == 'distance_to_closest_object' :
+ observation.distance_to_closest_object = distance_to_closest_object
+
+ elif key == 'linear_velocity':
+ observation.linear_velocity = linear_velocity
+
+ elif key == 'angular_velocity':
+ observation.angular_velocity = angular_velocity
+ else:
+ raise Warning('The input name was not recognized')
+
+ return observation, raycast_map, closest_obs_map
+
+
+
+ def predict(self, agent, observation):
+ r''' Predict an action from an RL agent and an observation '''
+ if self.flag_collision:
+ print('Warning !!!!! The robot is close to an obstacle')
+
+ if isinstance(agent, SAC) :
+ # Continuous actions
+ prediction = agent.predict_action(observation)
+
+ elif isinstance(agent, DDQN) :
+ # Discrete actions
+ action_id = agent.predict_action(observation)
+ prediction = self.discrete_action_list[action_id]
+ else :
+ raise ValueError('Problem with agent, it has to be a DDQN or a SAC')
+
+ if self.distance_to_goal > 0.3:
+ action = prediction
+ else:
+ print('constraint_angle', constraint_angle(self.goal_orientation - self.robot_orientation), self.goal_orientation, self.robot_orientation)
+ if np.abs(constraint_angle(self.goal_orientation - self.robot_orientation)) > np.pi/12:
+ print('11111111111111111111')
+ lin_vel=0
+ ang_vel = 0.5*np.sign(constraint_angle(self.goal_orientation - self.robot_orientation))
+ action = lin_vel, ang_vel
+ else:
+ print('2222222222222222')
+ action = (0,0)
+
+ return action, prediction
+
+
+
+ def update_grid_dict(self, static_occupancy_grid, dynamic_social_grid):
+ ''' Update the grid dict with the static_occupancy_grid and dynamic_social_grid '''
+
+ for key in self.grid_data_dict:
+ if key == 'static_occupancy_grid':
+ if np.shape(static_occupancy_grid)[-1] != self.col :
+ static_occupancy_grid = cv2.resize(static_occupancy_grid, (self.row,self.col), interpolation = cv2.INTER_NEAREST)
+ self.grid_data_dict[key][0,:,:] = static_occupancy_grid
+ elif key == 'dynamic_social_grid':
+ if np.shape(dynamic_social_grid)[-1] != self.col :
+ dynamic_social_grid = cv2.resize(dynamic_social_grid, (self.row,self.col), interpolation = cv2.INTER_NEAREST)
+ self.grid_data_dict[key][0,:,:] = dynamic_social_grid
+
+
+
+
+
diff --git a/src/robot_behavior/src/robot_behavior/ros_4_hri_interface.py b/src/robot_behavior/src/robot_behavior/utils/ros_4_hri_interface.py
similarity index 100%
rename from src/robot_behavior/src/robot_behavior/ros_4_hri_interface.py
rename to src/robot_behavior/src/robot_behavior/utils/ros_4_hri_interface.py
diff --git a/src/robot_behavior/src/robot_behavior/utils.py b/src/robot_behavior/src/robot_behavior/utils/utils.py
similarity index 100%
rename from src/robot_behavior/src/robot_behavior/utils.py
rename to src/robot_behavior/src/robot_behavior/utils/utils.py
diff --git a/src/robot_behavior/src/robot_behavior/utils/utils_drl_local_path_planner.py b/src/robot_behavior/src/robot_behavior/utils/utils_drl_local_path_planner.py
new file mode 100755
index 0000000000000000000000000000000000000000..fc67cdaa534c50d9b12517ffeb14b07d247b9fc2
--- /dev/null
+++ b/src/robot_behavior/src/robot_behavior/utils/utils_drl_local_path_planner.py
@@ -0,0 +1,184 @@
+#!/usr/bin/env python3
+import numpy as np
+import tf
+import plotly.graph_objects as go
+import cv2
+import plotly.express as px
+import os
+from copy import deepcopy
+
+
+def set_action_list(linear_velocties_list = [0, 0.1, 0.2, 0.4], angular_velocities_list = [-0.4, -0.2, -0.1, 0, 0.1, 0.2, 0.4]) -> list:
+ r""" Function that returns a list of discrete pairs of actions
+
+ Args :
+ linear_velocties_list (list)
+ default value : [0, 0.2, 0.4, 0.6]
+
+ angular_velocities_list (list)
+ default value : [-np.pi/4, -np.pi/6, -np.pi/12, 0, np.pi/12, np.pi/6, np.pi/4]
+
+ Return :
+ actions (list)
+
+ """
+ actions = []
+ for linear_velocity in linear_velocties_list :
+ for angular_velocity in angular_velocities_list:
+ if linear_velocity == 0 and angular_velocity == 0:
+ pass
+ else :
+
+ # a single step of simulation is making the robot moving forward of 10 cm
+ # So with 5 loop of simulation the robot is moving forward with 0.5m
+ actions.append([linear_velocity, round(angular_velocity,1)])
+
+
+ return(actions)
+
+
+def measure_distance(current_position, goal_position):
+ return np.linalg.norm(np.array(current_position) - np.array(goal_position))
+
+def measure_relative_angle(current_position, current_orientation, goal_position):
+ """Measures the relative angle between two objects or points.
+
+ Args:
+ current_position : (np.array)
+
+ current_orientation : (float)
+
+ goal_position : (np.array)
+
+ Returns: The angle can be positive or negative indicating the direction.
+
+ Note : function inspired from : https://gitlab.inria.fr/spring/wp6_robot_behavior/multiparty_interaction_simulator/-/blob/main/mpi_sim/utils/measurements.py
+ """
+
+
+ position_diff = np.array(goal_position) - np.array(current_position)
+
+ if position_diff[0] == 0. and position_diff[1] == 0.:
+ angle = 0.
+ else:
+ angle_between_p1_and_p2 = -np.pi/2 + np.arctan2(position_diff[0], -position_diff[1])
+ angle = constraint_angle(angle_between_p1_and_p2 - current_orientation)
+
+ return angle
+
+def global_coord_to_local_coord(position_ari , orientation_ari, entities_positions_global):
+ ''' Take the global coordinates (in float) and transform it to local coordinates (in float) '''
+ # Translation (ARI is now the reference position)
+ entities_positions_local = np.array(entities_positions_global) - np.array(position_ari)
+ # Rotation (ARI is now the reference orientation)
+ rotation_matrix = np.array([[np.cos(orientation_ari), -np.sin(orientation_ari)],[np.sin(orientation_ari), np.cos(orientation_ari)]])
+ entities_positions_local = np.transpose(np.dot(rotation_matrix, np.transpose(entities_positions_local)))
+ assert len(entities_positions_local) == len(entities_positions_global)
+ return entities_positions_local
+
+def transform_position_to_map_coordinate(position, map_area, map_resolution):
+ r"""Transform a position (x, y) in the simulation to a coordinate (x, y) of a map of the simulation.
+
+ Args:
+ position (x,y): x,y - Position that should be transformed.
+ map_area (tuple): Area ((x_min, x_max), (y_min, y_max)) of the simulation that the map is depicting.
+ map_resolution (float): Resolution of the map in m per cells.
+
+ Returns: Map coordinate as a tuple (x, y).
+ """
+ x_min = map_area[0][0]
+ y_min = map_area[1][0]
+ x = int((position[0] - x_min) / map_resolution)
+ y = int((position[1] - y_min) / map_resolution)
+ return x, y
+
+
+
+def constraint_angle(angle, min_value=-np.pi, max_value=np.pi):
+ """Constrains the given angle between -pi and pi.
+
+ Note : function inspired from : https://gitlab.inria.fr/spring/wp6_robot_behavior/multiparty_interaction_simulator/-/blob/main/mpi_sim/utils/measurements.py """
+
+ length = max_value - min_value
+ new_angle = np.where(angle > max_value, min_value + ((angle - max_value) % length), angle)
+ new_angle = np.where(angle < min_value, max_value - ((min_value - angle) % length), new_angle)
+
+ return new_angle
+
+def pose_stamped_2_mat(p):
+ q = p.pose.orientation
+ pos = p.pose.position
+ T = tf.transformations.quaternion_matrix([q.x,q.y,q.z,q.w])
+ T[:3,3] = np.array([pos.x,pos.y,pos.z])
+ return T
+
+
+def get_color(idx):
+ idx = idx * 3
+ color = ((37 * idx) % 255, (17 * idx) % 255, (29 * idx) % 255)
+
+ return color
+
+
+def meter_to_px(point, map_size, resolution):
+ if point[0] < map_size[0][0]:
+ point[0] = map_size[0][0]
+ if point[0] > map_size[0][1]:
+ point[0] = map_size[0][1]
+ if point[1] < map_size[1][0]:
+ point[1] = map_size[1][0]
+ if point[1] > map_size[1][1]:
+ point[1] = map_size[1][1]
+
+ point_px = [0., 0.]
+ point_px[0] = abs(map_size[0][0] - point[0])/resolution
+ point_px[1] = abs(map_size[1][1] - point[1])/resolution
+
+ return np.rint(point_px)
+
+def discrete_colorscale(bvals, colors):
+ """
+ bvals - list of values bounding intervals/ranges of interest
+ colors - list of rgb or hex colorcodes for values in [bvals[k], bvals[k+1]],0<=k < len(bvals)-1
+ returns the plotly discrete colorscale
+
+ From : https://chart-studio.plotly.com/~empet/15229/heatmap-with-a-discrete-colorscale/#/
+ """
+
+ dcolorscale = [] #discrete colorscale
+ dcolorscale.extend([[bvals[0], colors[0]], [bvals[1], colors[1]]])
+ return dcolorscale
+
+def rotmat_2d(angle):
+ return np.matrix([[np.cos(angle), -np.sin(angle)],
+ [np.sin(angle), np.cos(angle)]])
+
+
+def local_to_global(robot_position, x):
+ angle = robot_position[-1]
+ y = rotmat_2d(angle).dot(x) + robot_position[:2]
+ return np.array(y).reshape(x.shape)
+
+
+def plot_img(img, name = 'img'):
+ # bvals = [0, 1]
+ # colors = ['#ffffff', '#000000']
+ # dcolorsc = discrete_colorscale(bvals, colors)
+ # heatmap = go.Heatmap(z=self.local_map_static.T,
+ # colorscale = dcolorsc,
+ # zmin=0, zmax=1,
+ # showscale=False
+ # )
+ # fig = go.Figure(data = heatmap, layout = go.Layout(height = 800, width = 800))
+ img = deepcopy(img)
+ if not os.path.exists('/home/ros/robot_behavior_ws/local_maps'):
+ os.makedirs('/home/ros/robot_behavior_ws/local_maps')
+ fig = px.imshow(np.flip(np.flip(img.T, axis = 0), axis = 1))
+ fig.write_image('/home/ros/robot_behavior_ws/local_maps/{}.png'.format(name))
+
+
+
+
+
+
+