message filters slop

launch/rtabmap_apriltag.launch

@@ -14,6 +14,12 @@
   <arg name="rviz_rtabmap"                     default="False"/>
   <arg name="rtabmapviz"                       default="False"/>
 
+  <arg name="front_fisheye_camera_ns"          default="/front_camera/fisheye/"/>
+
+
+  <include file="$(find image_projection_demo)/launch/projections_inria.launch">
+    <arg name="camera_ns"                        value="$(arg front_fisheye_camera_ns)"/>
+  </include>
 
   <!-- <node pkg="image_transport" type="republish"  name="republish" output="screen" args="compressed in:=$(arg in_topic_republish) raw out:=$(arg out_topic_republish)" /> -->
   <include file="$(find robot_behavior)/launch/apriltag_continuous_detection.launch">
@@ -30,5 +36,4 @@
     <arg name="database_path"                    value="$(find robot_behavior)/maps/$(arg database_name).db"/>
   </include>
 
-
 </launch>

src/robot_behavior/controller_node.py

@@ -82,7 +82,7 @@ class Controller:
         local_map_sub = message_filters.Subscriber('/local_map', OccupancyGrid)
         rospy.Subscriber('/global_map', OccupancyGrid, callback=self._global_map_callback, queue_size=1)
 
-        ts = message_filters.ApproximateTimeSynchronizer(fs=[odom_sub, joint_states_sub, tracked_persons_sub, local_map_sub], queue_size=2, slop=0.30, allow_headerless=False)
+        ts = message_filters.ApproximateTimeSynchronizer(fs=[odom_sub, joint_states_sub, tracked_persons_sub, local_map_sub], queue_size=2, slop=0.50, allow_headerless=False)
         ts.registerCallback(self._ts_callback)
 
         rospy.Subscriber('/look_at', LookAt, callback=self._look_at_callback, queue_size=1)
@@ -301,7 +301,7 @@ class Controller:
             if self.pan_ind is not None:
                 self.head_joint_errors.append(np.abs(constraint_angle(-np.arctan2(self.x_pan_human_target - x_robot, self.y_pan_human_target - y_robot) - constraint_angle(yaw_robot + self.current_pan))))
             if self.tilt_ind is not None:
-                self.head_joint_errors.append(np.abs(constraint_angle(-np.arctan2(self.x_pan_human_target - x_robot, self.y_pan_human_target - y_robot) - constraint_angle(yaw_robot + self.current_tilt))))
+                self.head_joint_errors.append(0.)
             controller_status_msg.head_joint_errors = self.head_joint_errors
 
         if controller_status_msg.pan_target_id == -1 and controller_status_msg.human_target_id == -1 and controller_status_msg.goto_target_id == -1 and self.controller.controller_config.goto_target_pose[-1] == 0: