diff --git a/mobiman_simulation/config/config_mobiman_framework.yaml b/mobiman_simulation/config/config_mobiman_framework.yaml index 6acf93b..aca33a9 100644 --- a/mobiman_simulation/config/config_mobiman_framework.yaml +++ b/mobiman_simulation/config/config_mobiman_framework.yaml @@ -3,7 +3,7 @@ ### Simulator: # gazebo # igibson -sim: gazebo +sim: igibson flag_gazebo: true flag_robot: true @@ -14,7 +14,7 @@ flag_moveit: false ### Rviz: flag_rviz: true -rviz_config_path: "rviz/mobile_manipulator_ocs2_gazebo" +rviz_config_path: "rviz/mobile_manipulator_ocs2_igibson" ### Map Server: flag_map_server: true @@ -25,7 +25,8 @@ flag_ns: true robot_name: "jackalJaco" n_robot: 2 robot_frame_name: "base_link" -urdf_path: "urdf/jackal_fixedWheel_jaco_fixedFinger.urdf" +urdf_path: "urdf/jackal_jaco.urdf" +urdf_path_ocs2: "urdf/jackal_fixedWheel_jaco_fixedFinger.urdf" ## NUA NOTE: DEPRECATE THIS! lib_path: "codegen/auto_generated/jackalJaco" collision_points_config_path: "config/config_points_on_robot" @@ -33,5 +34,5 @@ collision_points_config_path: "config/config_points_on_robot" flag_goal_server: true ### Mobiman: -flag_mobiman: true -task_config_path: config/task/task_jackalJaco_gazebo.info \ No newline at end of file +flag_mobiman: false +task_config_path: config/task/task_jackalJaco_igibson.info \ No newline at end of file diff --git a/mobiman_simulation/launch/utilities/map_server.launch b/mobiman_simulation/launch/utilities/map_server.launch index 16f5d6b..34e879f 100644 --- a/mobiman_simulation/launch/utilities/map_server.launch +++ b/mobiman_simulation/launch/utilities/map_server.launch @@ -5,22 +5,25 @@ + - - + + + - - - + + + + \ No newline at end of file diff --git a/mobiman_simulation/launch/utilities/ocs2_m4.launch b/mobiman_simulation/launch/utilities/ocs2_m4.launch index 0b92df6..e467934 100644 --- a/mobiman_simulation/launch/utilities/ocs2_m4.launch +++ b/mobiman_simulation/launch/utilities/ocs2_m4.launch @@ -8,7 +8,7 @@ gazebo igibson --> - + + + diff --git a/mobiman_simulation/rviz/mobile_manipulator_ocs2_igibson.rviz b/mobiman_simulation/rviz/mobile_manipulator_ocs2_igibson.rviz index a9be7fe..890530d 100644 --- a/mobiman_simulation/rviz/mobile_manipulator_ocs2_igibson.rviz +++ b/mobiman_simulation/rviz/mobile_manipulator_ocs2_igibson.rviz @@ -290,6 +290,74 @@ Visualization Manager: Value: false jackalJaco_0/rear_right_wheel_link: Value: false + jackalJaco_1/base_link: + Value: false + jackalJaco_1/chassis_link: + Value: false + jackalJaco_1/d435_cam1camera_bottom_screw_frame: + Value: false + jackalJaco_1/d435_cam1camera_color_frame: + Value: false + jackalJaco_1/d435_cam1camera_color_optical_frame: + Value: false + jackalJaco_1/d435_cam1camera_depth_frame: + Value: false + jackalJaco_1/d435_cam1camera_depth_frame_gazebo: + Value: false + jackalJaco_1/d435_cam1camera_depth_optical_frame: + Value: false + jackalJaco_1/d435_cam1camera_left_ir_frame: + Value: false + jackalJaco_1/d435_cam1camera_left_ir_optical_frame: + Value: false + jackalJaco_1/d435_cam1camera_link: + Value: false + jackalJaco_1/d435_cam1camera_right_ir_frame: + Value: false + jackalJaco_1/d435_cam1camera_right_ir_optical_frame: + Value: false + jackalJaco_1/front_fender_link: + Value: false + jackalJaco_1/front_left_wheel_link: + Value: false + jackalJaco_1/front_mount: + Value: false + jackalJaco_1/front_right_wheel_link: + Value: false + jackalJaco_1/imu_link: + Value: false + jackalJaco_1/j2n6s300_end_effector: + Value: false + jackalJaco_1/j2n6s300_link_1: + Value: false + jackalJaco_1/j2n6s300_link_2: + Value: false + jackalJaco_1/j2n6s300_link_3: + Value: false + jackalJaco_1/j2n6s300_link_4: + Value: false + jackalJaco_1/j2n6s300_link_5: + Value: false + jackalJaco_1/j2n6s300_link_6: + Value: false + jackalJaco_1/j2n6s300_link_base: + Value: false + jackalJaco_1/jaco_base_link: + Value: false + jackalJaco_1/mid_mount: + Value: false + jackalJaco_1/navsat_link: + Value: false + jackalJaco_1/odom: + Value: false + jackalJaco_1/rear_fender_link: + Value: false + jackalJaco_1/rear_left_wheel_link: + Value: false + jackalJaco_1/rear_mount: + Value: false + jackalJaco_1/rear_right_wheel_link: + Value: false world: Value: true Marker Alpha: 1 @@ -350,6 +418,56 @@ Visualization Manager: jackalJaco_0/j2n6s300_link_6: jackalJaco_0/j2n6s300_end_effector: {} + jackalJaco_1/odom: + jackalJaco_1/base_link: + jackalJaco_1/chassis_link: + jackalJaco_1/front_fender_link: + {} + jackalJaco_1/front_left_wheel_link: + {} + jackalJaco_1/front_right_wheel_link: + {} + jackalJaco_1/imu_link: + {} + jackalJaco_1/mid_mount: + jackalJaco_1/front_mount: + {} + jackalJaco_1/rear_mount: + {} + jackalJaco_1/navsat_link: + {} + jackalJaco_1/rear_fender_link: + {} + jackalJaco_1/rear_left_wheel_link: + {} + jackalJaco_1/rear_right_wheel_link: + {} + jackalJaco_1/d435_cam1camera_bottom_screw_frame: + jackalJaco_1/d435_cam1camera_link: + jackalJaco_1/d435_cam1camera_depth_frame: + jackalJaco_1/d435_cam1camera_color_frame: + jackalJaco_1/d435_cam1camera_color_optical_frame: + {} + jackalJaco_1/d435_cam1camera_depth_optical_frame: + {} + jackalJaco_1/d435_cam1camera_left_ir_frame: + jackalJaco_1/d435_cam1camera_left_ir_optical_frame: + {} + jackalJaco_1/d435_cam1camera_right_ir_frame: + jackalJaco_1/d435_cam1camera_right_ir_optical_frame: + {} + jackalJaco_1/d435_cam1camera_depth_frame_gazebo: + {} + jackalJaco_1/jaco_base_link: + jackalJaco_1/j2n6s300_link_base: + jackalJaco_1/j2n6s300_link_1: + jackalJaco_1/j2n6s300_link_2: + jackalJaco_1/j2n6s300_link_3: + jackalJaco_1/j2n6s300_link_4: + jackalJaco_1/j2n6s300_link_5: + jackalJaco_1/j2n6s300_link_6: + jackalJaco_1/j2n6s300_end_effector: + {} Update Interval: 0 Value: true - Class: rviz/InteractiveMarkers @@ -510,8 +628,7 @@ Visualization Manager: Marker Topic: /mobile_manipulator/optimizedStateTrajectory Name: MarkerArray Namespaces: - Base Trajectory: true - EE Trajectory: true + {} Queue Size: 100 Value: true - Class: rviz/MarkerArray @@ -527,9 +644,7 @@ Visualization Manager: Marker Topic: /mobile_manipulator_target Name: MarkerArray Namespaces: - target_1: true - target_2: true - target_3: true + {} Queue Size: 100 Value: true - Class: rviz/MarkerArray @@ -568,7 +683,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 4.615935802459717 + Distance: 3.889058828353882 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -584,9 +699,9 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.48479706048965454 + Pitch: 0.5497972965240479 Target Frame: - Yaw: 1.8479151725769043 + Yaw: 1.6029189825057983 Saved: ~ Window Geometry: Displays: @@ -594,7 +709,7 @@ Window Geometry: Height: 1043 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 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 + QMainWindow State: 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 Selection: collapsed: false Time: diff --git a/mobiman_simulation/scripts/mobiman_framework_launch.py b/mobiman_simulation/scripts/mobiman_framework_launch.py index cc62c39..ecfb2cd 100755 --- a/mobiman_simulation/scripts/mobiman_framework_launch.py +++ b/mobiman_simulation/scripts/mobiman_framework_launch.py @@ -74,6 +74,7 @@ def load_config_file(config_file_path, namespace=""): n_robot = rospy.get_param('n_robot', "") robot_frame_name = rospy.get_param('robot_frame_name', "") urdf_path = rospy.get_param('urdf_path', "") + urdf_path_ocs2 = rospy.get_param('urdf_path_ocs2', "") lib_path = rospy.get_param('lib_path', "") collision_points_config_path = rospy.get_param('collision_points_config_path', "") @@ -106,6 +107,7 @@ def load_config_file(config_file_path, namespace=""): print("[mobiman_framework_launch:: __main__ ] n_robot: " + str(n_robot)) print("[mobiman_framework_launch:: __main__ ] robot_frame_name: " + str(robot_frame_name)) print("[mobiman_framework_launch:: __main__ ] urdf_path: " + str(urdf_path)) + print("[mobiman_framework_launch:: __main__ ] urdf_path_ocs2: " + str(urdf_path_ocs2)) print("[mobiman_framework_launch:: __main__ ] lib_path: " + str(lib_path)) print("[mobiman_framework_launch:: __main__ ] collision_points_config_path: " + str(collision_points_config_path)) @@ -120,21 +122,28 @@ def load_config_file(config_file_path, namespace=""): print("") ## Set Namespace - namespaces = [] - robot_frame_names = [] + robot_ns_vec = [] + robot_base_frame_name_vec = [] if flag_ns: - print("[mobiman_framework_launch:: __main__ ] namespaces:") + print("[mobiman_framework_launch:: __main__ ] robot_ns_vec:") for i in range(n_robot): - ns_tmp = robot_name + "_" + str(i) + "/" - namespaces.append(ns_tmp) + ns_tmp = robot_name + "_" + str(i) + robot_ns_vec.append(ns_tmp) print(ns_tmp) # Update frame names - robot_frame_names.append(ns_tmp + str(robot_frame_name)) - print(robot_frame_names) + robot_base_frame_name_vec.append(ns_tmp + "/" + str(robot_frame_name)) + + print("[mobiman_framework_launch:: __main__ ] robot_base_frame_name_vec:") + print(robot_base_frame_name_vec) + else: print("[mobiman_framework_launch:: __main__ ] No namespace!") + #print("[mobiman_framework_launch:: __main__ ] DEBUG_INF") + #while 1: + # continue + ## Launch Simulation if sim == "gazebo": @@ -155,17 +164,18 @@ def load_config_file(config_file_path, namespace=""): print("[mobiman_framework_launch:: __main__ ] Launched simulation in Gazebo!") elif sim == "igibson": - ns1 = "turtlebot2_0" # Launch iGibson - mobile_garden_path = mobiman_launch_path + "utilities/mobile_garden_igibson.launch" - mobile_garden_args = [mobile_garden_path, - 'ns1:=' + str(ns1), - 'world_frame_name:=' + str(world_frame_name)] + sim_launch_path = igibson_path + "launch/mobiman_jackal_jaco.launch" - mobile_garden_launch = [ (roslaunch.rlutil.resolve_launch_arguments(mobile_garden_args)[0], mobile_garden_args[1:]) ] - mobile_garden = roslaunch.parent.ROSLaunchParent(uuid, mobile_garden_launch) - mobile_garden.start() + for i, rns in enumerate(robot_ns_vec): + sim_args = [sim_launch_path, + 'robot_ns:=' + str(rns), + 'urdf_path:=' + str(urdf_path)] + + sim_launch = [ (roslaunch.rlutil.resolve_launch_arguments(sim_args)[0], sim_args[1:]) ] + sim = roslaunch.parent.ROSLaunchParent(uuid, sim_launch) + sim.start() print("[mobiman_framework_launch:: __main__ ] Launched simulation in iGibson!") @@ -189,13 +199,15 @@ def load_config_file(config_file_path, namespace=""): ## Wait for simulation to be ready! tfBuffer = tf2_ros.Buffer() listener = tf2_ros.TransformListener(tfBuffer) - print("Waiting the transform between " + world_frame_name + " and " + robot_frame_name + "...") + if flag_ns: + robot_frame_name = robot_base_frame_name_vec[0] + print("[mobiman_framework_launch:: __main__ ] Waiting the transform between " + world_frame_name + " and " + robot_frame_name + "...") trans = None while (not rospy.is_shutdown()) and (not trans): try: trans = tfBuffer.lookup_transform(world_frame_name, robot_frame_name, rospy.Time(0)) except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as ex: - rospy.logwarn(str(ex)) + rospy.logwarn("[mobiman_framework_launch:: __main__ ] ERROR: " + str(ex)) rospy.sleep(1.0) ## Launch Map Server @@ -233,7 +245,7 @@ def load_config_file(config_file_path, namespace=""): mobiman_path = mobiman_launch_path + "utilities/ocs2_m4.launch" mobiman_args = [mobiman_path, 'task_config_path:=' + str(task_config_path), - 'urdf_path:=' + str(urdf_path), + 'urdf_path:=' + str(urdf_path_ocs2), 'lib_path:=' + str(lib_path), 'collision_points_config_path:=' + str(collision_points_config_path)]