Mentions légales du service

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

idem

parent ec684bf3
Branches
No related tags found
No related merge requests found
...@@ -82,7 +82,7 @@ class LocalPathPlannerMPCNode: ...@@ -82,7 +82,7 @@ class LocalPathPlannerMPCNode:
rospy.loginfo("Initializing the LocalPathPlannerMPCNode") rospy.loginfo("Initializing the LocalPathPlannerMPCNode")
# self._check_publishers_connection() self._check_publishers_connection()
def run(self): def run(self):
...@@ -346,6 +346,22 @@ class LocalPathPlannerMPCNode: ...@@ -346,6 +346,22 @@ class LocalPathPlannerMPCNode:
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= (np.asarray(self.local_social_cost_map_data.data) / 100).reshape(self.last_shape_local_social_cost_map)
def _check_publishers_connection(self):
"""
Checks that all the publishers are working
:return:
"""
rate = rospy.Rate(10) # 10hz
while self._cmd_vel_pub.get_num_connections() == 0 and not rospy.is_shutdown():
rospy.logdebug("No susbribers to _cmd_vel_pub yet so we wait and try again")
try:
rate.sleep()
except rospy.ROSInterruptException:
# This is to avoid error when world is rested, time when backwards.
pass
rospy.logdebug("_cmd_vel_pub Publisher Connected")
def _check_all_sensors_ready(self): def _check_all_sensors_ready(self):
rospy.logdebug("START ALL SENSORS READY") rospy.logdebug("START ALL SENSORS READY")
self._check_rtabmap_ready() self._check_rtabmap_ready()
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment