Add coverage test

tests/custom_behavior.py

@@ -0,0 +1,20 @@
+import irsim
+
+'''
+- Refer to the irsim/lib/behavior_methods.py file for the custom behavior design.
+- custom_behavior_methods.py is the designated module name. It should be placed in the same directory as the current implementation script.
+- The behavior names defined in custom_behavior_methods.py (e.g., dash_custom) must match the behavior names specified in the YAML file.
+'''
+
+env = irsim.make()
+env.load_behavior("custom_behavior_methods")
+
+for i in range(1000):
+
+    env.step()
+    env.render(0.01)
+
+    if env.done():
+        break
+
+env.end(5)

tests/custom_behavior.yaml

@@ -0,0 +1,67 @@
+world:
+  height: 10  # the height of the world
+  width: 10   # the height of the world
+  step_time: 0.1  # 10Hz calculate each step
+  sample_time: 0.1  # 10 Hz for render and data extraction 
+  offset: [0, 0] # the offset of the world on x and y 
+  collision_mode: 'stop'  # 'stop', 'unobstructed', 'reactive'
+  control_mode: 'auto'  # 'keyboard', 'auto'
+
+
+robot:
+  - number: 3
+    distribution: {name: 'circle', radius: 4.0, center: [5, 5]}  
+    kinematics: {name: 'diff'}
+    shape: 
+      - {name: 'circle', radius: 0.2}  # radius
+    behavior: {name: 'rvo', vxmax: 1.0, vymax: 1.0, accer: 1.0, factor: 1.0}
+    vel_min: [-3, -3.0]
+    vel_max: [3, 3.0]
+    color: ['royalblue', 'red', 'green'] 
+    arrive_mode: position
+    goal_threshold: 0.2
+    plot:
+      show_trail: true
+      show_goal: true
+      trail_fill: true
+      trail_alpha: 0.2
+      show_trajectory: false
+
+  - number: 2
+    distribution: {name: 'circle', radius: 4.0, center: [5, 5]}  
+    kinematics: {name: 'diff'}
+    shape: 
+      - {name: 'circle', radius: 0.2}  # radius
+    behavior: {name: 'dash', vxmax: 1.5, vymax: 1.5, accer: 1.0, factor: 1.0}
+    vel_min: [-3, -3.0]
+    vel_max: [3, 3.0]
+    color: ['orange', 'purple'] 
+    arrive_mode: position
+    goal_threshold: 0.2
+    plot:
+      show_trail: true
+      show_goal: true
+      trail_fill: true
+      trail_alpha: 0.2
+      show_trajectory: false
+
+  - number: 2
+    distribution: {name: 'random'}  
+    kinematics: {name: 'diff'}
+    shape: 
+      - {name: 'circle', radius: 0.2}  # radius
+    behavior: {name: 'dash_custom'}
+    vel_min: [-3, -3.0]
+    vel_max: [3, 3.0]
+    color: ['pink', 'brown'] 
+    arrive_mode: position
+    goal_threshold: 0.2
+    plot:
+      show_trail: true
+      show_goal: true
+      trail_fill: true
+      trail_alpha: 0.2
+      show_trajectory: false
+
+
+

tests/custom_behavior_methods.py

@@ -0,0 +1,49 @@
+from irsim.lib import register_behavior
+from irsim.util.util import relative_position, WrapToPi
+import numpy as np
+
+
+@register_behavior("diff", "dash_custom")
+def beh_diff_dash(ego_object, objects, **kwargs):
+
+    print("This is a custom behavior example for differential drive with dash2")
+
+    state = ego_object.state
+    goal = ego_object.goal
+    goal_threshold = ego_object.goal_threshold
+    _, max_vel = ego_object.get_vel_range()
+    angle_tolerance = kwargs.get("angle_tolerance", 0.1)
+
+    behavior_vel = DiffDash2(state, goal, max_vel, goal_threshold, angle_tolerance)
+
+    return behavior_vel
+
+
+def DiffDash2(state, goal, max_vel, goal_threshold=0.1, angle_tolerance=0.2):
+    """
+    Calculate the differential drive velocity to reach a goal.
+
+    Args:
+        state (np.array): Current state [x, y, theta] (3x1).
+        goal (np.array): Goal position [x, y, theta] (3x1).
+        max_vel (np.array): Maximum velocity [linear, angular] (2x1).
+        goal_threshold (float): Distance threshold to consider goal reached (default 0.1).
+        angle_tolerance (float): Allowable angular deviation (default 0.2).
+
+    Returns:
+        np.array: Velocity [linear, angular] (2x1).
+    """
+    distance, radian = relative_position(state, goal)
+
+    if distance < goal_threshold:
+        return np.zeros((2, 1))
+
+    diff_radian = WrapToPi(radian - state[2, 0])
+    linear = max_vel[0, 0] * np.cos(diff_radian)
+
+    if abs(diff_radian) < angle_tolerance:
+        angular = 0
+    else:
+        angular = max_vel[1, 0] * np.sign(diff_radian)
+
+    return np.array([[linear], [angular]])

tests/test_all_objects.py

@@ -83,6 +83,14 @@ def test_all_objects():
 
     env7.end()
 
+    env8 = irsim.make('custom_behavior.yaml')
+    env8.load_behavior("custom_behavior_methods")
+
+    for i in range(10):
+        env8.step()
+        env8.render(0.01)
+
+    env8.end()
 
 if __name__ == "__main__":
     test_all_objects()