From 96a80cb78edc9f0b0c10b687552812fb05a5ffa2 Mon Sep 17 00:00:00 2001
From: Alex AUTERNAUD <alex.auternaud@inria.fr>
Date: Thu, 30 Sep 2021 16:26:08 +0200
Subject: [PATCH] message filters slop
---
launch/rtabmap_apriltag.launch | 7 ++++++-
src/robot_behavior/controller_node.py | 4 ++--
2 files changed, 8 insertions(+), 3 deletions(-)
diff --git a/launch/rtabmap_apriltag.launch b/launch/rtabmap_apriltag.launch
index ac323d4..e15dc11 100644
--- a/launch/rtabmap_apriltag.launch
+++ b/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>
diff --git a/src/robot_behavior/controller_node.py b/src/robot_behavior/controller_node.py
index 3767b08..2a5da3e 100644
--- a/src/robot_behavior/controller_node.py
+++ b/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:
--
GitLab