Mentions légales du service

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

local_path_planner_mpc next

parent cf2e714a
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/robot_behavior/launch/local_path_planner_mpc.launch
+ 17
− 14
View file @ ec684bf3
<?xml version="1.0" encoding="UTF-8"?>
<launch>
<arg name="mpc_config_name" default="social_mpc"/>
<arg name="robot_config_name" default="robot"/>
<arg name="map_frame" default="map"/>
<arg name="robot_frame" default="base_footprint"/>
<arg name="namespace_slam" default="slam"/>
<arg name="global_occupancy_map_topic" default="global_map"/>
<arg name="local_occupancy_map_topic" default="local_static_cost_map"/>
<arg name="mpc_config_name" default="social_mpc"/>
<arg name="robot_config_name" default="robot"/>
<arg name="map_frame" default="map"/>
<arg name="robot_frame" default="base_footprint"/>
<arg name="namespace_slam" default="slam"/>
<arg name="global_occupancy_map_topic" default="global_map"/>
<arg name="local_occupancy_map_topic" default="local_static_cost_map"/>
<arg name="local_social_cost_map_topic" default="local_social_cost_map"/>
<node pkg="robot_behavior" type="behavior_local_path_planner_mpc_main.py" name="local_path_planner_mpc" output="screen" >
<param name="mpc_config_name" type="str" value="$(find robot_behavior)/config/social_mpc/$(arg mpc_config_name).yaml"/>
<param name="robot_config_name" type="str" value="$(find robot_behavior)/config/social_mpc/$(arg robot_config_name).yaml"/>
<param name="robot_frame" type="str" value="$(arg robot_frame)"/>
<param name="map_frame" type="str" value="$(arg map_frame)"/>
<param name="namespace_slam" type="str" value="$(arg namespace_slam)"/>
<param name="global_occupancy_map_topic" type="str" value="/$(arg namespace_slam)/$(arg global_occupancy_map_topic)"/>
<param name="local_occupancy_map_topic" type="str" value="/$(arg namespace_slam)/$(arg local_occupancy_map_topic)"/>
<param name="mpc_config_name" type="str" value="$(find robot_behavior)/config/social_mpc/$(arg mpc_config_name).yaml"/>
<param name="robot_config_name" type="str" value="$(find robot_behavior)/config/social_mpc/$(arg robot_config_name).yaml"/>
<param name="robot_frame" type="str" value="$(arg robot_frame)"/>
<param name="map_frame" type="str" value="$(arg map_frame)"/>
<param name="namespace_slam" type="str" value="$(arg namespace_slam)"/>
<param name="global_occupancy_map_topic" type="str" value="/$(arg namespace_slam)/$(arg global_occupancy_map_topic)"/>
<param name="local_occupancy_map_topic" type="str" value="/$(arg namespace_slam)/$(arg local_occupancy_map_topic)"/>
<param name="local_social_cost_map_topic" type="str" value="/$(arg namespace_slam)/$(arg local_social_cost_map_topic)"/>
<remap from="/cmd_vel" to="/nav_vel"/>
</node>
......
src/robot_behavior/src/robot_behavior/behavior_local_path_planner_mpc_node.py
+ 197
− 7
View file @ ec684bf3
......@@ -11,12 +11,12 @@ import numpy as np
import rospy
from nav_msgs.msg import OccupancyGrid
from robot_behavior.behavior_local_path_planner_mpc import LocalPathPlannerMPC
from robot_behavior.utils import constraint_angle, local_to_global
from robot_behavior.utils import constraint_angle, local_to_global, global_to_local
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 spring_msgs.msg import GoToPosition, GoToEntity, LookAtEntity, LookAtPosition
from geometry_msgs.msg import Twist, Vector3
class LocalPathPlannerMPCNode:
......@@ -33,6 +33,7 @@ class LocalPathPlannerMPCNode:
self.robot_frame = rospy.get_param('~robot_frame', 'base_footprint')
self.global_occupancy_map_topic = rospy.get_param('~global_occupancy_map_topic', '/slam/global_occupancy_map')
self.local_occupancy_map_topic = rospy.get_param('~local_occupancy_map_topic', '/slam/local_occupancy_map')
self.local_social_cost_map_topic = rospy.get_param('~local_social_cost_map_topic', '/slam/local_social_cost_map')
self.max_humans_world = rospy.get_param('max_humans_world', 20)
self.max_groups_world = rospy.get_param('max_groups_world', 10)
self.namespace = rospy.get_param('namespace', 'behavior_generator')
......@@ -41,8 +42,11 @@ class LocalPathPlannerMPCNode:
self.read_config(filename=self.mpc_config_path)
self.local_map_data = None
self.local_social_cost_map_data = None
self.rtabmap_ready = False
self.local_map_static = None
self.mode = None
self.action_idx = None
self.init_ros_subscriber_and_publicher()
......@@ -51,6 +55,7 @@ class LocalPathPlannerMPCNode:
self._check_all_sensors_ready()
self.init_var()
self.init_mpc()
......@@ -63,14 +68,17 @@ class LocalPathPlannerMPCNode:
### ROS Subscribers
self._subscribers.append(rospy.Subscriber(self.local_occupancy_map_topic, OccupancyGrid, callback=self._local_map_callback, queue_size=1))
self._subscribers.append(rospy.Subscriber(self.local_social_cost_map_topic, OccupancyGrid, callback=self._local_social_cost_map_callback, queue_size=1))
# self._subscribers.append(rospy.Subscriber(self.namespace + '/action/go_to_position', GoToPosition, callback=self._go_to_position_callback, queue_size=1))
# self._subscribers.append(rospy.Subscriber(self.namespace + '/action/go_to_body', GoToEntity, callback=self._go_to_body_callback, queue_size=1))
# self._subscribers.append(rospy.Subscriber(self.namespace + '/action/go_to_person', GoToEntity, callback=self._go_to_person_callback, queue_size=1))
# self._subscribers.append(rospy.Subscriber(self.namespace + '/action/go_to_group', GoToEntity, callback=self._go_to_group_callback, queue_size=1))
### ROS Publishers
# self._cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
# self._publishers.append(self._cmd_vel_pub)
self._cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
self._publishers.append(self._cmd_vel_pub)
self._timers.append(rospy.Timer(rospy.Duration(self.controller.controller_config.h), callback=self._pub_vel_callback))
rospy.loginfo("Initializing the LocalPathPlannerMPCNode")
......@@ -87,11 +95,169 @@ class LocalPathPlannerMPCNode:
def step(self, init_time):
r''' Step the ros wrapper'''
print('MPC')
self.set_mode()
self.set_weights()
self.local_map[:, :, 0] = self.local_static_map
self.local_map[:, :, 1] = self.local_social_cost_map
if self.update_goals_enabled:
self.update_goals()
if self.do_step():
self.actions = self.mpc.step(
state=np.array([0.]), # not used anymore
actions=self.actions + self.initial_action,
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)
rospy.sleep(2)
def _pub_vel_callback(self, event):
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) or self.i == 0:
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):
ang, lin = action[0], action[1], action[2]
self.lin_vel, self.ang_vel = 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))
def _check_joint_vel_limits(self, lin_vel, ang_vel):
if lin_vel > self.controller_config.u_ub[-1]:
rospy.logwarn("The linear velocity is greater than the maximum allowed. The maximum velocity is set.")
lin_vel = self.controller_config.u_ub[-1]
if lin_vel < self.controller_config.u_lb[-1]:
rospy.logwarn("The linear velocity is lower than the minimum allowed. The minimum velocity is set.")
lin_vel = self.controller_config.u_lb[-1]
if ang_vel > self.controller_config.u_ub[-2]:
rospy.logwarn("The angular velocity is greater than the maximum allowed. The maximum velocity is set.")
ang_vel = self.controller_config.u_ub[-2]
if ang_vel < self.controller_config.u_lb[-2]:
rospy.logwarn("The angular velocity is lower than the minimum allowed. The minimum velocity is set.")
ang_vel = self.controller_config.u_lb[-2]
return lin_vel, ang_vel
def get_robot_pose(self):
r""" Function that compute the position of the robot on the map """
# try:
# map_2_robot = self.tf_listener.lookupTransform(self.map_frame, self.robot_frame, self.timestamp_tracking_latest)
# except:
try:
map_2_robot = self.tf_listener.lookupTransform(self.map_frame, self.robot_frame, rospy.Time(0))
#rospy.logwarn("Could not get transformation between {} and {} at correct time. Using latest available.".format(self.map_frame, self.robot_frame))
except:
#rospy.logerr("Could not get transformation between {} and {}.".format(self.map_frame, self.robot_frame))
return None
self.robot_x_position, self.robot_y_position, _ = map_2_robot[0]
(_, _, yaw) = tf.transformations.euler_from_quaternion([map_2_robot[1][0],
map_2_robot[1][1],
map_2_robot[1][2],
map_2_robot[1][3]])
self.robot_yaw = float(constraint_angle(yaw))
self.robot_position = np.array([self.robot_x_position, self.robot_y_position])
def get_waypoint_pose(self):
try:
map_2_waypoint = self.tf_listener.lookupTransform(self.map_frame, 'intermediate waypoint goal', rospy.Time(0))
#rospy.logwarn("Could not get transformation between {} and {} at correct time. Using latest available.".format(self.map_frame, self.robot_frame))
except:
#rospy.logerr("Could not get transformation between {} and {}.".format(self.map_frame, self.robot_frame))
return None
self.x_wp, self.y_wp, _ = map_2_waypoint[0]
(_, _, yaw) = tf.transformations.euler_from_quaternion([map_2_waypoint[1][0],
map_2_waypoint[1][1],
map_2_waypoint[1][2],
map_2_waypoint[1][3]])
self.yaw_wp = float(constraint_angle(yaw))
def update_goals(self):
if self.mode == 0:
self.get_robot_pose()
self.get_waypoint_pose()
local_wpt = global_to_local(self.robot_position, np.asarray(self.x_wp, self.y_wp))
wpt_angle = constraint_angle(self.yaw_wp - self.robot_yaw)
self.goto_goal[:] = [local_wpt[0], local_wpt[1], wpt_angle, 0., 0., 0, 0, 1]
self.goto_goal[-2] = 1
else:
# TODO: To implement the other mode
pass
def set_weights(self):
if self.mode is not None:
for i, weight_name in enumerate(self.weights_description):
self.weights[i] = self.controller_config.__getattribute__(
weight_name)
self.loss_coef = self.controller_config.loss_coef[self.mode]
self.loss_rad = self.controller_config.loss_rad[self.mode]
self.reg_parameter = self.controller_config.reg_parameter.copy()
def set_mode(self):
if self.go_to_target_flag:
if not self.human_target_flag:
self.mode = 0 # join
else:
self.mode = 1 # escort
elif self.human_target_flag:
self.mode = 2 # follow
else:
rospy.logwarn('No mode was chosen, nothing to do')
def do_step(self):
# check if a MPC step is necessary of not
return True
def init_var(self):
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.go_to_target_flag = False
self.human_target_flag = False
self.action_idx = 0
self.lin_vel = 0.
self.ang_vel = 0.
self.goto_goal = np.zeros(self.controller_config.goto_target_dim)
self.weights_description = self.controller_config.weights_description
self.weights = np.zeros(len(self.weights_description))
self.loss_coef = np.zeros_like(self.controller_config.loss_coef[0])
self.loss_rad = np.zeros_like(self.controller_config.loss_rad[0])
self.reg_parameter = np.zeros_like(self.controller_config.reg_parameter)
self.local_map = np.zeros((*self.local_static_map.shape, 3))
def init_mpc(self):
dim_config = {'goto_target_dim': self.controller_config.goto_target_dim,
'human_target_dim': self.controller_config.human_target_dim,
......@@ -168,10 +334,23 @@ class LocalPathPlannerMPCNode:
self.local_static_map= (np.asarray(self.local_map_data.data) / 100).reshape(self.last_shape_local_map)
def _local_social_cost_map_callback(self, data):
self.local_social_cost_map_data = data
self.x_local_social_cost_map = self.local_social_cost_map_data.info.origin.position.x
self.y_local_social_cost_map = self.local_social_cost_map_data.info.origin.position.y
self.local_social_cost_map_width = self.local_social_cost_map_data.info.width
self.local_social_cost_map_height = self.local_social_cost_map_data.info.height
self.local_social_cost_map_resolution = self.local_social_cost_map_data.info.resolution
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)
def _check_all_sensors_ready(self):
rospy.logdebug("START ALL SENSORS READY")
self._check_rtabmap_ready()
self._check_local_map_ready()
self._check_local_social_cost_map_ready()
rospy.logdebug("ALL SENSORS READY")
......@@ -199,6 +378,19 @@ class LocalPathPlannerMPCNode:
except:
rospy.logerr("Current {} not ready yet, retrying for getting local map".format(self.local_occupancy_map_topic))
return self.local_map_data
def _check_local_social_cost_map_ready(self):
self.local_social_cost_map_data = None
rospy.logdebug("Waiting for {} to be READY...".format(self.local_social_cost_map_topic))
while self.local_social_cost_map_data is None and not rospy.is_shutdown():
try:
self.local_social_cost_map_data = rospy.wait_for_message(self.local_social_cost_map_topic, OccupancyGrid, timeout=5.0)
rospy.logdebug("Current {} READY=>".format(self.local_social_cost_map_topic))
except:
rospy.logerr("Current {} not ready yet, retrying for getting local map".format(self.local_social_cost_map_topic))
return self.local_social_cost_map_data
def shutdown(self):
......@@ -224,6 +416,4 @@ class LocalPathPlannerMPCNode:
timer.shutdown()
if self._action_server:
self._action_server.shutdown()
# self.ros_4_hri_interface.close()
self._action_server.shutdown()
\ No newline at end of file
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment