IN PROGRESS: TODO: add namespace to rviz, map_server and mobiman so t…

…hat target can be set separately.
RIVeR-Lab · Oct 21, 2023 · 6a78058 · 6a78058
1 parent 3c5dfd9
commit 6a78058
Show file tree

Hide file tree

Showing 5 changed files with 181 additions and 48 deletions.
diff --git 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,13 +25,14 @@ 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"
 
 ### Goal Server:
 flag_goal_server: true
 
 ### Mobiman:
-flag_mobiman: true
-task_config_path: config/task/task_jackalJaco_gazebo.info
+flag_mobiman: false
+task_config_path: config/task/task_jackalJaco_igibson.info
diff --git a/mobiman_simulation/launch/utilities/map_server.launch b/mobiman_simulation/launch/utilities/map_server.launch
@@ -5,22 +5,25 @@
   <!-- CONFIGURATION OPTIONS
     config_map_server
   -->
+  <arg name="robot_ns"            default="robotName_0"/>
   <arg name="config_map_server"  default="config_map_server" />
 
-  <!-- PUBLISH CONFIGURATION PARAMETERS -->
-  <rosparam file="$(find mobiman_simulation)/config/$(arg config_map_server).yaml"/>
+  <group ns="$(arg robot_ns)">
+    <!-- PUBLISH CONFIGURATION PARAMETERS -->
+    <rosparam file="$(find mobiman_simulation)/config/$(arg config_map_server).yaml"/>
 
-  <node name="map_server" pkg="mobiman_simulation" type="map_server" output="screen" >
-    <!--remap from="pointcloud" to="pc2_scan"/>
-    <remap from="map_server/esdf_map_out" to="esdf_map" />    
-    <param name="tsdf_voxel_size" value="0.1" />
-    <param name="tsdf_voxels_per_side" value="16" />
-    <param name="publish_esdf_map" value="true" />
-    <param name="publish_pointclouds" value="true" />
-    <param name="use_tf_transforms" value="true" />
-    <param name="allow_clear" value="true" />
-    <param name="update_mesh_every_n_sec" value="0.05" />
-    <param name="clear_sphere_for_planning" value="false" />
-    <param name="world_frame" value="world" /-->
-  </node>
+    <node name="map_server" pkg="mobiman_simulation" type="map_server" output="screen" >
+      <!--remap from="pointcloud" to="pc2_scan"/>
+      <remap from="map_server/esdf_map_out" to="esdf_map" />    
+      <param name="tsdf_voxel_size" value="0.1" />
+      <param name="tsdf_voxels_per_side" value="16" />
+      <param name="publish_esdf_map" value="true" />
+      <param name="publish_pointclouds" value="true" />
+      <param name="use_tf_transforms" value="true" />
+      <param name="allow_clear" value="true" />
+      <param name="update_mesh_every_n_sec" value="0.05" />
+      <param name="clear_sphere_for_planning" value="false" />
+      <param name="world_frame" value="world" /-->
+    </node>
+  </group>
 </launch>
diff --git a/mobiman_simulation/launch/utilities/ocs2_m4.launch b/mobiman_simulation/launch/utilities/ocs2_m4.launch
@@ -8,7 +8,7 @@
             gazebo
             igibson
       -->
-      <arg name="sim"        default="igibson" />
+      <arg name="sim"         default="igibson" />
 
       <!--
             ur5
@@ -17,6 +17,8 @@
       -->
       <arg name="robot"       default="jackalJaco" />
 
+      <arg name="robot_ns"    default="robotName_0"/>
+
       <!-- The task file for the mpc -->
       <arg name="task_config_path"        default="config/task/task_$(arg robot)_$(arg sim).info" />
       <param name="taskFile"              value="$(find mobiman_simulation)/$(arg task_config_path)" />

diff --git 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,17 +699,17 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.48479706048965454
+      Pitch: 0.5497972965240479
       Target Frame: <Fixed Frame>
-      Yaw: 1.8479151725769043
+      Yaw: 1.6029189825057983
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
   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
@@ -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)]