run.py

import time
import argparse
import subprocess
import os
from os.path import join

import numpy as np
import rospy
import rospkg

from gazebo_simulation import GazeboSimulation
from geometry_msgs.msg import Quaternion

INIT_POSITION = [-2, 3, 1.57]  # in world frame
GOAL_POSITION = [0, 10]  # relative to the initial position

def compute_distance(p1, p2):
    return ((p1[0] - p2[0]) ** 2 + (p1[1] - p2[1]) ** 2) ** 0.5

def path_coord_to_gazebo_coord(x, y):
        RADIUS = 0.075
        r_shift = -RADIUS - (30 * RADIUS * 2)
        c_shift = RADIUS + 5

        gazebo_x = x * (RADIUS * 2) + r_shift
        gazebo_y = y * (RADIUS * 2) + c_shift

        return (gazebo_x, gazebo_y)

if __name__ == "__main__":
    parser = argparse.ArgumentParser(description = 'test BARN navigation challenge')
    parser.add_argument('--world_idx', type=int, default=0)
    parser.add_argument('--gui', action="store_true")
    parser.add_argument('--out', type=str, default="out.txt")
    args = parser.parse_args()
    
    ##########################################################################################
    ## 0. Launch Gazebo Simulation
    ##########################################################################################
    
    os.environ["JACKAL_LASER"] = "1"
    os.environ["JACKAL_LASER_MODEL"] = "ust10"
    os.environ["JACKAL_LASER_OFFSET"] = "-0.065 0 0.01"
    
    world_name = "BARN/world_%d.world" %(args.world_idx)
    map_name = "yaml_%d.yaml" %(args.world_idx)
    print(map_name)
    print(">>>>>>>>>>>>>>>>>> Loading Gazebo Simulation with %s <<<<<<<<<<<<<<<<<<" %(world_name))   
    rospack = rospkg.RosPack()
    base_path = rospack.get_path('jackal_helper')
    
    launch_file = join(base_path, 'launch', 'gazebo_launch.launch')
    world_name = join(base_path, "worlds", world_name)
    map_yaml_file = join(base_path,"map_files",map_name)
    
    gazebo_process = subprocess.Popen([
        'roslaunch',
        launch_file,
        'world_name:=' + world_name,
        'gui:=' + ("true" if args.gui else "false")
    ])
    time.sleep(5)  # sleep to wait until the gazebo being created
    
    rospy.init_node('gym', anonymous=True) #, log_level=rospy.FATAL)
    rospy.set_param('/use_sim_time', True)
    
    # GazeboSimulation provides useful interface to communicate with gazebo  
    gazebo_sim = GazeboSimulation(init_position=INIT_POSITION)
    
    init_coor = (INIT_POSITION[0], INIT_POSITION[1])
    goal_coor = (INIT_POSITION[0] + GOAL_POSITION[0], INIT_POSITION[1] + GOAL_POSITION[1])
    
    pos = gazebo_sim.get_model_state().pose.position
    curr_coor = (pos.x, pos.y)
    collided = True
    
    # check whether the robot is reset, the collision is False
    while compute_distance(init_coor, curr_coor) > 0.1 or collided:
        gazebo_sim.reset() # Reset to the initial position
        pos = gazebo_sim.get_model_state().pose.position
        curr_coor = (pos.x, pos.y)
        collided = gazebo_sim.get_hard_collision()
        time.sleep(1)


    ##########################################################################################
    ## 1. Launch your navigation stack
    ## (Customize this block to add your own navigation stack)
    ##########################################################################################

    mrpt_map_launch = join(base_path,'..', 'tps_astar/launch/mrpt_map_server.launch')
    mrpt_map_process = subprocess.Popen([
        'roslaunch',
        mrpt_map_launch,
        'map_yaml_file:=' + map_yaml_file
    ])

    mrpt_local_obstacles = join(base_path, '..', 'tps_astar/launch/mrpt_local_obstacles.launch')
    mrpt_obs_process = subprocess.Popen([
        'roslaunch',
        mrpt_local_obstacles,
    ])

    mrpt_localization = join(base_path,'..', 'tps_astar/launch/mrpt_localization.launch')
    mrpt_localization_process = subprocess.Popen([
        'roslaunch',
        mrpt_localization,
        'odom_frame_id:='+'odom',
        'global_frame_id:='+'map',
        'base_frame_id:='+'base_link',
        'sensor_sources:='+'/front/scan',
        'use_map_topic:='+'true', 
        'initial_pose_x:='+ str(INIT_POSITION[0]),
        "initial_pose_y:="+ str(INIT_POSITION[1]),
        'initial_pose_phi:='+str(INIT_POSITION[2]),
        'initial_pose_std_phi:='+str(1.5) # rad (1 sigma)
    ])

    time.sleep(5)
    
    tps_astar = join(base_path, '..', 'tps_astar/launch/tps_astar_nav.launch')
    tps_astar_process = subprocess.Popen([
        'roslaunch',
        tps_astar,
        'nav_goal:=['+ str(INIT_POSITION[0]+GOAL_POSITION[0])+','+
                       str(INIT_POSITION[1]+GOAL_POSITION[1])+','+ 
                       str(INIT_POSITION[2]) +']',
        'start_pose:=['+ str(INIT_POSITION[0])+','+str(INIT_POSITION[1])+','+str(INIT_POSITION[2])+']',
        'start_vel:=2.0',
        'mrpt_gui:= True',
        'topic_map_sub:='+'map',
        'topic_localization_sub:='+'/mrpt_pose',
        'topic_odometry_sub:='+'/odometry/filtered',
        'topic_obstacles_sub:='+'/map_pointcloud',
        'topic_replan_sub:='+'/replan',
        'topic_cmd_vel_pub:='+'/enq_motion',
        'topic_wp_seq_pub:='+'/waypoints'
    ])

    mrpt_reactivenav2d = join(base_path, '..', 'tps_astar/launch/mrpt_reactivenav2d.launch')
    mrpt_reactivenav2d_process = subprocess.Popen([
        'roslaunch',
        mrpt_reactivenav2d,
        'topic_odometry:='+'/odometry/filtered',
        'topic_obstacles:='+'/map_pointcloud',
        'topic_cmd_vel:='+'/cmd_vel',
        'topic_wp_seq:='+'/waypoints'
    ])
    
     
    # launch_file = join(base_path, '..', 'jackal_helper/launch/move_base_DWA.launch')
    # nav_stack_process = subprocess.Popen([
    #     'roslaunch',
    #     launch_file,
    # ])
    # Make sure your navigation stack recives a goal of (0, 10, 0), which is 10 meters away
    # along postive y-axis.
    # import actionlib
    # from move_base_msgs.msg import MoveBaseGoal, MoveBaseAction
    # nav_as = actionlib.SimpleActionClient('/move_base', MoveBaseAction)
    # mb_goal = MoveBaseGoal()
    # mb_goal.target_pose.header.frame_id = 'odom'
    # mb_goal.target_pose.pose.position.x = GOAL_POSITION[0]
    # mb_goal.target_pose.pose.position.y = GOAL_POSITION[1]
    # mb_goal.target_pose.pose.position.z = 0
    # mb_goal.target_pose.pose.orientation = Quaternion(0, 0, 0, 1)

    # nav_as.wait_for_server()
    # nav_as.send_goal(mb_goal)

    ##########################################################################################
    ## 2. Start navigation
    ##########################################################################################
    
    curr_time = rospy.get_time()
    pos = gazebo_sim.get_model_state().pose.position
    curr_coor = (pos.x, pos.y)

    
    # check whether the robot started to move
    while compute_distance(init_coor, curr_coor) < 0.1:
        curr_time = rospy.get_time()
        pos = gazebo_sim.get_model_state().pose.position
        curr_coor = (pos.x, pos.y)
        time.sleep(0.01)
    
    # start navigation, check position, time and collision
    start_time = curr_time
    start_time_cpu = time.time()
    collided = False
    
    while compute_distance(goal_coor, curr_coor) > 1 and not collided and curr_time - start_time < 100:
        curr_time = rospy.get_time()
        pos = gazebo_sim.get_model_state().pose.position
        curr_coor = (pos.x, pos.y)
        print("Time: %.2f (s), x: %.2f (m), y: %.2f (m)" %(curr_time - start_time, *curr_coor), end="\r")
        collided = gazebo_sim.get_hard_collision()
        while rospy.get_time() - curr_time < 0.1:
            time.sleep(0.01)


    
    
    ##########################################################################################
    ## 3. Report metrics and generate log
    ##########################################################################################
    
    print(">>>>>>>>>>>>>>>>>> Test finished! <<<<<<<<<<<<<<<<<<")
    success = False
    if collided:
        status = "collided"
    elif curr_time - start_time >= 100:
        status = "timeout"
    else:
        status = "succeeded"
        success = True
    print("Navigation %s with time %.4f (s)" %(status, curr_time - start_time))
    
    path_file_name = join(base_path, "worlds/BARN/path_files", "path_%d.npy" %args.world_idx)
    path_array = np.load(path_file_name)
    path_array = [path_coord_to_gazebo_coord(*p) for p in path_array]
    path_array = np.insert(path_array, 0, (INIT_POSITION[0], INIT_POSITION[1]), axis=0)
    path_array = np.insert(path_array, len(path_array), (INIT_POSITION[0] + GOAL_POSITION[0], INIT_POSITION[1] + GOAL_POSITION[1]), axis=0)
    path_length = 0
    for p1, p2 in zip(path_array[:-1], path_array[1:]):
        path_length += compute_distance(p1, p2)
    
    # Navigation metric: 1_success *  optimal_time / clip(actual_time, 4 * optimal_time, 8 * optimal_time)
    optimal_time = path_length / 2
    actual_time = curr_time - start_time
    nav_metric = int(success) * optimal_time / np.clip(actual_time, 4 * optimal_time, 8 * optimal_time)
    print("Navigation metric: %.4f" %(nav_metric))
    
    with open(args.out, "a") as f:
        f.write("%d %d %d %d %.4f %.4f\n" %(args.world_idx, success, collided, (curr_time - start_time)>=100, curr_time - start_time, nav_metric))
    
    gazebo_process.terminate()
    gazebo_process.wait()
    mrpt_map_process.terminate()
    mrpt_map_process.wait()
    mrpt_obs_process.terminate()
    mrpt_obs_process.wait()
    mrpt_localization_process.terminate()
    mrpt_localization_process.wait()
    tps_astar_process.terminate()
    tps_astar_process.wait()
    mrpt_reactivenav2d_process.terminate()
    mrpt_reactivenav2d_process.wait()