BIG UPDATE: Implemented and tested successfully mobiman parallelizati…

…on using namespaces. All print-outs are commented out. NUA NOTE: Even the launch is terminated the node mobile_manipulator still remains active, which cause the computer slows down!
RIVeR-Lab · Oct 24, 2023 · de30c82 · de30c82
1 parent 6a78058
commit de30c82
Show file tree

Hide file tree

Showing 17 changed files with 4,484 additions and 331 deletions.
diff --git a/mobiman_simulation/config/config_mobiman_framework.yaml b/mobiman_simulation/config/config_mobiman_framework.yaml
@@ -14,7 +14,7 @@ flag_moveit: false
 
 ### Rviz:
 flag_rviz: true
-rviz_config_path: "rviz/mobile_manipulator_ocs2_igibson"
+rviz_config_path: "rviz/igibson/mobile_manipulator_ocs2_igibson"
 
 ### Map Server:
 flag_map_server: true
@@ -34,5 +34,5 @@ collision_points_config_path: "config/config_points_on_robot"
 flag_goal_server: true
 
 ### Mobiman:
-flag_mobiman: false
+flag_mobiman: true
 task_config_path: config/task/task_jackalJaco_igibson.info
diff --git a/mobiman_simulation/config/task/task_jackalJaco_igibson.info b/mobiman_simulation/config/task/task_jackalJaco_igibson.info
@@ -3,7 +3,6 @@ model_information
 {             
   envMode            0                ; Mode of the env, 0: sim, 1: exp
   sim                "igibson"        ; Simulator, Default: "gazebo", Others: "igibson" 
-  ns                 "jackalJaco_0"   ; Namespace 
   robotName          "mobiman"        ; Name of the robot in simulation
   robotModelType     2                ; 0: Mobile base, 1: Arm, 2: Mobile manipulator
 
@@ -54,21 +53,21 @@ model_information
 
   ;baseStateMsg                    "/gazebo/link_states"
   ;armStateMsg                     "/arm_controller/state"
-  armStateMsg                     "/jackalJaco_0/gibson_ros/joint_states"
+  armStateMsg                     "/gibson_ros/joint_states"
   baseControlMsg                  "/mobile_base_controller/cmd_vel"
   armControlMsg                   "/arm_controller/cmd_pos"
   selfCollisionMsg                "/self_collision_distances"
   occupancyDistanceBaseMsg        "/occupancy_distances_base"
   occupancyDistanceArmMsg         "/occupancy_distances_arm"
-  pointsOnRobotMsg                "points_on_robot"
+  pointsOnRobotMsg                "/points_on_robot"
   octomapMsg                      "/octomap_scan"
   modelModeMsg                    "/mobile_manipulator_model_mode"
   mpcTargetMsg                    "/mobile_manipulator_mpc_target"
   targetMsg                       "/mobile_manipulator_target"
-  goalFrame                       "grasp"
+  goalFrame                       "/grasp"
 
-  collisionConstraintPoints       "collision_constraint_points"
-  collisionCheckPoints            "collision_check_points"
+  collisionConstraintPoints       "/collision_constraint_points"
+  collisionCheckPoints            "/collision_check_points"
   logSavePathRel                  "dataset/ocs2/" ; If empty, not saved. Ex: "dataset/ocs2/"
 }
 

diff --git a/mobiman_simulation/include/mobiman_simulation/scan_utility.h b/mobiman_simulation/include/mobiman_simulation/scan_utility.h
@@ -155,6 +155,8 @@ class ScanUtility
 
     tf::TransformListener* tflistener_;
 
+    bool printOutFlag_ = false;
+
     string obj_name_;
     string pkg_dir_;
     string data_dir_;

diff --git a/mobiman_simulation/launch/utilities/map_server.launch b/mobiman_simulation/launch/utilities/map_server.launch
@@ -5,9 +5,9 @@
   <!-- CONFIGURATION OPTIONS
     config_map_server
   -->
-  <arg name="robot_ns"            default="robotName_0"/>
   <arg name="config_map_server"  default="config_map_server" />
-
+  <arg name="robot_ns"            default="robotName_0"/>
+
   <group ns="$(arg robot_ns)">
     <!-- PUBLISH CONFIGURATION PARAMETERS -->
     <rosparam file="$(find mobiman_simulation)/config/$(arg config_map_server).yaml"/>

diff --git a/mobiman_simulation/launch/utilities/ocs2_m4.launch b/mobiman_simulation/launch/utilities/ocs2_m4.launch
@@ -32,18 +32,19 @@
       <arg name="lib_path"                default="codegen/auto_generated/$(arg robot)" />
       <param name="libFolder"             value="$(find mobiman_simulation)/$(arg lib_path)" />
 
-
       <arg name="collision_points_config_path"  default="config/config_points_on_robot" />
       <rosparam file="$(find mobiman_simulation)/$(arg collision_points_config_path).yaml"/>
 
-      <node pkg="ocs2_mobile_manipulator_ros" 
-            type="mobile_manipulator_node" 
-            name="mobile_manipulator_node" 
-            output="screen"
-            launch-prefix="" />
-
-      <!-- Run Rviz visualization -->
-      <group if="$(eval arg('rviz') == true)">
-            <include file="$(find mobiman_simulation)/launch/utilities/rviz.launch" />
+      <group ns="$(arg robot_ns)">
+            <node pkg="ocs2_mobile_manipulator_ros" 
+                  type="mobile_manipulator_node" 
+                  name="mobile_manipulator_node" 
+                  output="screen"
+                  launch-prefix="" />
+
+            <!-- Run Rviz visualization -->
+            <group if="$(eval arg('rviz') == true)">
+                  <include file="$(find mobiman_simulation)/launch/utilities/rviz.launch" />
+            </group>
       </group>
 </launch>
diff --git a/mobiman_simulation/launch/utilities/ocs2_target.launch b/mobiman_simulation/launch/utilities/ocs2_target.launch
@@ -1,6 +1,8 @@
 <?xml version="1.0" ?>
 
 <launch>
+      <arg name="robot_ns"            default="robotName_0"/>
+
       <!-- Enable rviz visualization -->
       <!--arg name="rviz"        default="true" /-->
 
@@ -36,8 +38,10 @@
       <!--param name="urdfFile"   value="$(arg urdfFile)" />
       <param name="libFolder"  value="$(arg libFolder)" /-->
 
-      <node pkg="ocs2_mobile_manipulator_ros" 
-            type="mobile_manipulator_target" 
-            name="mobile_manipulator_target_node" 
-            output="screen" />
+      <group ns="$(arg robot_ns)">
+            <node pkg="ocs2_mobile_manipulator_ros" 
+                  type="mobile_manipulator_target" 
+                  name="mobile_manipulator_target_node" 
+                  output="screen" />
+      </group>
 </launch>
diff --git a/mobiman_simulation/launch/utilities/rviz.launch b/mobiman_simulation/launch/utilities/rviz.launch
@@ -1,10 +1,13 @@
 <launch>
   <arg name="rviz_config_path" default="rviz/debug" />
-
-  <node name="rviz" 
-        pkg="rviz" 
-        type="rviz" 
-        args="-d $(find mobiman_simulation)/$(arg rviz_config_path).rviz" 
-        required="true" />
+  <arg name="robot_ns"            default="robotName_0"/>
+
+  <group ns="$(arg robot_ns)">
+    <node name="rviz" 
+          pkg="rviz" 
+          type="rviz" 
+          args="-d $(find mobiman_simulation)/$(arg rviz_config_path).rviz" 
+          required="true" />
+  </group>
 
 </launch>