Update and bugfix of pedestrian plugin and evaluation metrics for cabot_navigation run_test.py

cabot_navigation2/test/evaluation_metrics.py

@@ -331,69 +331,6 @@ def person_on_robot_collision(agents, robot):
     return [collision[1], collision[3]]
 
 
-def collision_count(agents, robot):
-    robot_coll_list = [0] * len(robot)
-    person_coll_list = [0] * len(robot)
-    robot_collisions = 0
-    person_collisions = 0
-
-    agents_in_collision = {}
-
-    for i in range(len(robot)):
-        for agent in agents[i].agents:
-            agent_name = agent.name
-
-            if euclidean_distance(robot[i].position, agent.position) - robot[i].radius - agent.radius < 0.02:
-                in_collision = agents_in_collision.get(agent_name, False)
-
-                # Robot's angle
-                nrx = (robot[i].position.position.x - agent.position.position.x) * math.cos(agent.yaw) + (robot[i].position.position.y - agent.position.position.y) * math.sin(agent.yaw)
-                nry = -(robot[i].position.position.x - agent.position.position.x) * math.sin(agent.yaw) + (robot[i].position.position.y - agent.position.position.y) * math.cos(agent.yaw)
-                alpha = math.atan2(nry, nrx)
-
-                # Agent's angle
-                nrx = (agent.position.position.x - robot[i].position.position.x) * math.cos(robot[i].yaw) + (agent.position.position.y - robot[i].position.position.y) * math.sin(robot[i].yaw)
-                nry = -(agent.position.position.x - robot[i].position.position.x) * math.sin(robot[i].yaw) + (agent.position.position.y - robot[i].position.position.y) * math.cos(robot[i].yaw)
-                alpha2 = math.atan2(nrx, nry)
-
-                if not in_collision:
-                    if abs(alpha) < abs(alpha2) and robot[i].linear_vel > agent.linear_vel:
-                        robot_collisions += 1
-                        robot_coll_list[i] += 1
-                    elif abs(alpha) > abs(alpha2) and robot[i].linear_vel < agent.linear_vel:
-                        person_collisions += 1
-                        person_coll_list[i] += 1
-                    elif abs(alpha) < abs(alpha2) and robot[i].linear_vel < agent.linear_vel:
-                        # person_collision += 1
-                        robot_collisions += 1
-                        robot_coll_list[i] += 1
-                    elif abs(alpha) > abs(alpha2) and robot[i].linear_vel > agent.linear_vel:
-                        # robot_collision += 1
-                        person_collisions += 1
-                        person_coll_list[i] += 1
-                    elif abs(alpha) == abs(alpha2) and robot[i].linear_vel == agent.linear_vel:
-                        robot_collisions += 1
-                        person_collisions += 1
-                        robot_coll_list[i] += 1
-                        person_coll_list[i] += 1
-
-                agents_in_collision[agent_name] = True
-            else:
-                agents_in_collision[agent_name] = False
-
-    return robot_collisions, person_collisions, robot_coll_list, person_coll_list
-
-
-def robot_on_person_collision_count(agents, robot):
-    collision = collision_count(agents, robot)
-    return [collision[0], collision[2]]
-
-
-def person_on_robot_collision_count(agents, robot):
-    collision = collision_count(agents, robot)
-    return [collision[1], collision[3]]
-
-
 def time_not_moving(agents, robot, linear_vel_threshold=0.01, angular_vel_threshold=0.02):
     time_stopped = 0
     not_moving = [0]*len(robot)
@@ -448,6 +385,74 @@ def minimum_goal_distance(agents, robot):
     return [min_dist]
 
 
+# collision count based on the collision criteria implemented in SEAN 2.0
+# https://github.com/yale-sean/social_sim_unity/blob/main/Assets/Scripts/SEAN/Metrics/CountCollisions.cs
+
+def collision_count(agents, robot):
+    return count_collisions_sean2(agents, robot)
+
+
+def robot_on_person_collision_count(agents, robot):
+    return count_robot_on_person_collisions_sean2(agents, robot)
+
+
+def person_on_robot_collision_count(agents, robot):
+    return count_person_on_robot_collisions_sean2(agents, robot)
+
+
+def count_collisions_sean2(agents, robot):
+    robot_coll_list = [0] * len(robot)
+    person_coll_list = [0] * len(robot)
+    robot_collisions = 0
+    person_collisions = 0
+
+    # variable to count independent collisions
+    agents_in_collision = {}
+
+    for i in range(len(robot)):
+        for agent in agents[i].agents:
+            agent_name = agent.name
+
+            if euclidean_distance(robot[i].position, agent.position) - robot[i].radius - agent.radius <= 0.0:  # exact collision couting without margin
+                in_collision = agents_in_collision.get(agent_name, False)
+
+                # agent angle with respect to the robot orientation (yaw)
+                nrx = (agent.position.position.x - robot[i].position.position.x) * math.cos(robot[i].yaw) + (agent.position.position.y - robot[i].position.position.y) * math.sin(robot[i].yaw)
+                nry = -(agent.position.position.x - robot[i].position.position.x) * math.sin(robot[i].yaw) + (agent.position.position.y - robot[i].position.position.y) * math.cos(robot[i].yaw)
+                alpha2 = math.atan2(nry, nrx)  # math.atan2(y, x)
+
+                # collision criteria based on SEAN 2.0
+                # If the robot isn't moving, it can't be at fault
+                vel = round(robot[i].linear_vel, 3)
+                # If the robot is oriented towards the other agent
+                oriented_towards_other = np.abs(alpha2) < np.radians(90.0)
+                is_robot_at_fault = (vel != 0.0) and oriented_towards_other
+
+                if not in_collision:
+                    if is_robot_at_fault:
+                        robot_collisions += 1
+                        robot_coll_list[i] += 1
+                    else:
+                        person_collisions += 1
+                        person_coll_list[i] += 1
+
+                agents_in_collision[agent_name] = True
+            else:
+                agents_in_collision[agent_name] = False
+
+    return robot_collisions, person_collisions, robot_coll_list, person_coll_list
+
+
+def count_robot_on_person_collisions_sean2(agents, robot):
+    collision = count_collisions_sean2(agents, robot)
+    return [collision[0], collision[2]]
+
+
+def count_person_on_robot_collisions_sean2(agents, robot):
+    collision = count_collisions_sean2(agents, robot)
+    return [collision[1], collision[3]]
+
+
 # SocNavBench: A Grounded Simulation Testing Framework for Evaluating Social Navigation
 # ABHIJAT BISWAS, ALLAN WANG, GUSTAVO SILVERA, AARON STEINFELD, and HENNY AD-MONI, Carnegie Mellon University
 

cabot_navigation2/test/run_test.py

@@ -41,7 +41,7 @@
 from cabot_common.util import callee_name
 
 from geometry_msgs.msg import PoseWithCovarianceStamped
-from mf_localization_msgs.srv import RestartLocalization
+from mf_localization_msgs.srv import StartLocalization, StopLocalization, MFSetInt
 from gazebo_msgs.srv import SetEntityState
 
 from pedestrian.manager import PedestrianManager
@@ -108,7 +108,9 @@ def __init__(self, node):
         self.futures = {}
         self.timers = {}
         self.actor_count = 0
-        self.restart_localization_client = self.node.create_client(RestartLocalization, '/restart_localization')
+        self.stop_localization_client = self.node.create_client(StopLocalization, '/stop_localization')
+        self.start_localization_client = self.node.create_client(StartLocalization, '/start_localization')
+        self.set_current_floor_client = self.node.create_client(MFSetInt, '/set_current_floor')
         self.initialpose_pub = self.node.create_publisher(PoseWithCovarianceStamped, '/initialpose', 1)
         self.set_entity_state_client = self.node.create_client(SetEntityState, '/gazebo/set_entity_state')
         self.test_func_name = None
@@ -234,6 +236,7 @@ def default_config(self):
             'init_y': 0.0,
             'init_z': 0.0,
             'init_a': 0.0,
+            'init_floor': 0
         }
 
     def info(self, text):
@@ -474,33 +477,34 @@ def reset_position(self, case, test_action):
         topic_type = import_class('mf_localization_msgs/msg/MFLocalizeStatus')
         condition = "msg.status==msg.TRACKING"
 
-        # change gazebo model position
-        request = SetEntityState.Request()
-        request.state.name = 'mobile_base'
-        init_x = test_action['x'] if 'x' in test_action else self.config['init_x']
-        init_y = test_action['y'] if 'y' in test_action else self.config['init_y']
-        init_z = test_action['z'] if 'z' in test_action else self.config['init_z']
-        init_a = test_action['a'] if 'a' in test_action else self.config['init_a']
-        init_yaw = init_a / 180.0 * math.pi
-        request.state.pose.position.x = float(init_x)
-        request.state.pose.position.y = float(init_y)
-        request.state.pose.position.z = float(init_z)
-        q = quaternion_from_euler(0, 0, init_yaw)
-        request.state.pose.orientation.x = q[0]
-        request.state.pose.orientation.y = q[1]
-        request.state.pose.orientation.z = q[2]
-        request.state.pose.orientation.w = q[3]
-        future = self.set_entity_state_client.call_async(request)
-        self.futures[uuid] = future
-
-        def done_callback(future):
-            # request to restart localization
-            request = RestartLocalization.Request()
-            self.restart_localization_client.call_async(request)
-            self.futures[uuid] = future
-
-            def done_callback2(future):
-                # check localize status to be tracking
+        # use true pose as initial pose guess or not
+        use_initialpose = test_action.get("use_initialpose", True)
+
+        # request to stop localization
+        request = StopLocalization.Request()
+        self.futures[uuid] = self.stop_localization_client.call_async(request)
+
+        def done_stop_localization_callback(future):
+            # change gazebo model position
+            request = SetEntityState.Request()
+            request.state.name = 'mobile_base'
+            init_x = test_action['x'] if 'x' in test_action else self.config['init_x']
+            init_y = test_action['y'] if 'y' in test_action else self.config['init_y']
+            init_z = test_action['z'] if 'z' in test_action else self.config['init_z']
+            init_a = test_action['a'] if 'a' in test_action else self.config['init_a']
+            init_yaw = init_a / 180.0 * math.pi
+            request.state.pose.position.x = float(init_x)
+            request.state.pose.position.y = float(init_y)
+            request.state.pose.position.z = float(init_z)
+            q = quaternion_from_euler(0, 0, init_yaw)
+            request.state.pose.orientation.x = q[0]
+            request.state.pose.orientation.y = q[1]
+            request.state.pose.orientation.z = q[2]
+            request.state.pose.orientation.w = q[3]
+            self.futures[uuid] = self.set_entity_state_client.call_async(request)
+
+            def done_set_entity_state_callback(future):
+                # define callback to check localize status to be tracking
                 def topic_callback(msg):
                     try:
                         context = {'msg': msg}
@@ -511,24 +515,47 @@ def topic_callback(msg):
                             time.sleep(2)
                     except:  # noqa: #722
                         logger.error(traceback.format_exc())
-                sub = self.node.create_subscription(topic_type, topic, topic_callback, 10)
-                self.add_subscription(case, sub)
-
-                time.sleep(1)
-                # publish initialpose for localization hint
-                pose = PoseWithCovarianceStamped()
-                pose.header.frame_id = "map"
-                pose.pose.pose.position.x = float(init_x)
-                pose.pose.pose.position.y = float(init_y)
-                pose.pose.pose.position.z = float(init_z)
-                pose.pose.pose.orientation.x = q[0]
-                pose.pose.pose.orientation.y = q[1]
-                pose.pose.pose.orientation.z = q[2]
-                pose.pose.pose.orientation.w = q[3]
-                self.initialpose_pub.publish(pose)
-
-            future.add_done_callback(done_callback2)
-        future.add_done_callback(done_callback)
+
+                if use_initialpose:
+                    # request to set current floor
+                    request = MFSetInt.Request()
+                    init_floor = test_action['floor'] if 'floor' in test_action else self.config['init_floor']
+                    request.data = int(init_floor)
+                    self.futures[uuid] = self.set_current_floor_client.call_async(request)
+
+                    def done_set_current_floor_callback(future):
+                        sub = self.node.create_subscription(topic_type, topic, topic_callback, 10)
+                        self.add_subscription(case, sub)
+
+                        time.sleep(1)
+                        # publish initialpose to start localization with initial pose guess
+                        pose = PoseWithCovarianceStamped()
+                        pose.header.frame_id = "map"
+                        pose.pose.pose.position.x = float(init_x)
+                        pose.pose.pose.position.y = float(init_y)
+                        pose.pose.pose.position.z = float(init_z)
+                        pose.pose.pose.orientation.x = q[0]
+                        pose.pose.pose.orientation.y = q[1]
+                        pose.pose.pose.orientation.z = q[2]
+                        pose.pose.pose.orientation.w = q[3]
+                        self.initialpose_pub.publish(pose)
+
+                    self.futures[uuid].add_done_callback(done_set_current_floor_callback)
+
+                else:
+                    # request to start localization
+                    request = StartLocalization.Request()
+                    self.futures[uuid] = self.start_localization_client.call_async(request)
+
+                    def done_start_localization_callback(future):
+                        sub = self.node.create_subscription(topic_type, topic, topic_callback, 10)
+                        self.add_subscription(case, sub)
+
+                    self.futures[uuid].add_done_callback(done_start_localization_callback)
+
+            self.futures[uuid].add_done_callback(done_set_entity_state_callback)
+
+        self.futures[uuid].add_done_callback(done_stop_localization_callback)
 
     @wait_test()
     def setup_actors(self, case, test_action):

mf_localization/configuration_files/cartographer/cartographer_2d.lua

@@ -49,6 +49,7 @@ options = {
   fixed_frame_pose_sampling_ratio = 1.,
   imu_sampling_ratio = 1.,
   landmarks_sampling_ratio = 1.,
+  ignore_out_of_order = true, -- option added by https://github.com/cartographer-project/cartographer_ros/pull/1737
 }
 
 MAP_BUILDER.use_trajectory_builder_2d = true

mf_localization/script/multi_floor_manager.py

@@ -517,7 +517,11 @@ def initialpose_callback(self, pose_with_covariance_stamped_msg: PoseWithCovaria
         # substitute ROS time to prevent error when gazebo is running and the pose message is published by rviz
         pose_with_covariance_stamped_msg.header.stamp = self.clock.now().to_msg()
 
-        self.initialize_with_global_pose(pose_with_covariance_stamped_msg, mode=LocalizationMode.TRACK)
+        status_code = self.initialize_with_global_pose(pose_with_covariance_stamped_msg, mode=LocalizationMode.TRACK)
+
+        # set is_active to True if succeeded to start trajectory
+        if status_code == 0:
+            self.is_active = True
 
     def initialize_with_global_pose(self, pose_with_covariance_stamped_msg: PoseWithCovarianceStamped, mode=None):
 
@@ -575,7 +579,7 @@ def initialize_with_global_pose(self, pose_with_covariance_stamped_msg: PoseWith
             self.mode = target_mode
             self.area = target_area
 
-            self.start_trajectory_with_pose(local_pose)
+            status_code = self.start_trajectory_with_pose(local_pose)
             self.logger.info(F"called /{node_id}/{self.mode}/start_trajectory")
 
             # reset altitude_floor_estimator in floor initialization process
@@ -592,6 +596,8 @@ def initialize_with_global_pose(self, pose_with_covariance_stamped_msg: PoseWith
             elif self.mode == LocalizationMode.TRACK:
                 self.localize_status = MFLocalizeStatus.TRACKING
 
+            return status_code  # result of start_trajectory_with_pose
+
     def restart_floor(self, local_pose: Pose):
         # set z = 0 to ensure 2D position on the local map
         local_pose.position.z = 0.0
@@ -1050,7 +1056,7 @@ def stop_localization_callback(self, request, response):
         if not self.is_active:
             response.status.code = 1
             response.status.message = "Stop localization failed. (localization is aleady stopped.)"
-            return resp
+            return response
         try:
             self.is_active = False
             self.finish_trajectory()

mf_localization_mapping/configuration_files/cartographer/cartographer_2d_mapping.lua

@@ -49,6 +49,7 @@ options = {
   fixed_frame_pose_sampling_ratio = 1.,
   imu_sampling_ratio = 1.,
   landmarks_sampling_ratio = 1.,
+  ignore_out_of_order = true, -- option added by https://github.com/cartographer-project/cartographer_ros/pull/1737
 }
 
 MAP_BUILDER.use_trajectory_builder_2d = true