[Robotics] Refactor Pick & Place simulation (#2196)

Author: jb-balaji

robotics-ai-suite/components/simulations/PicknPlace/README.md

@@ -13,7 +13,7 @@ The aim is to harnesses the capabilities of both the Nav2 and MoveIt2 stacks, pr
 
 OS: Ubuntu 22.04
 
-ROS2: Tested on Humble (but Should work with Foxy too)
+ROS2: Tested on Humble
 
 ### Prerequisites
 

robotics-ai-suite/components/simulations/PicknPlace/gazebo_plugins/src/ConveyorBeltPlugin.cpp

@@ -75,6 +75,19 @@ void ConveyorBeltPlugin::Configure(
   this->publish_period_ = std::chrono::nanoseconds(
       static_cast<int64_t>((1.0 / this->publish_rate_) * 1e9));
 
+  // Initialize ROS node if it hasn't been initialized yet
+  if (!rclcpp::ok()) {
+    rclcpp::init(0, nullptr);
+  }
+
+  // Initialize ROS node
+  std::string node_name = "conveyor_belt_simplified";
+  if (_sdf) {
+    node_name =
+        _sdf->Get<std::string>("node_name", "conveyor_belt_simplified").first;
+  }
+  this->ros_node_ = rclcpp::Node::make_shared(node_name);
+
   // Resolve joint
   this->joint = model.JointByName(_ecm, this->joint_name_);
   if (this->joint == gazebo::kNullEntity) {
@@ -88,16 +101,6 @@ void ConveyorBeltPlugin::Configure(
     _ecm.CreateComponent(this->joint,
                          gazebo::components::JointVelocityCmd({0.0}));
 
-  // Initialize ROS node if it hasn't been initialized yet
-  if (!rclcpp::ok()) {
-    rclcpp::init(0, nullptr);
-  }
-
-  // // Initialize ROS node
-  auto node_name =
-      _sdf->Get<std::string>("node_name", "conveyor_belt_simplified").first;
-  this->ros_node_ = rclcpp::Node::make_shared(node_name);
-
   std::string stateTopic = "conveyor/state";
   std::string controlTopic = "conveyor/control";
 

robotics-ai-suite/components/simulations/PicknPlace/robot_config/launch/arm.launch.py

@@ -197,10 +197,6 @@ def launch_setup(context: LaunchContext):
                 ]
                 }"""
 
-        controller_run_state = 'active'
-        if os.environ.get('ROS_DISTRO') == 'foxy':
-            controller_run_state = 'start'
-
         # Set initial joint position for robot - needed for all ROS distros
         set_initial_pose = ExecuteProcess(
             cmd=[

robotics-ai-suite/components/simulations/PicknPlace/robot_config/launch/gazebo.launch.py

@@ -16,20 +16,6 @@
 # and limitations under the License.
 
 # Description: Helper launch file to spawn AMR in Gazebo separated by namespace
-# Example usage:
-#
-#    gazebo_launch_cmd = IncludeLaunchDescription(
-#        PythonLaunchDescriptionSource(
-#            os.path.join(robot_config_launch_dir, 'gazebo.launch.py')),
-#        launch_arguments={ 'use_sim_time': 'true',
-#                           'world': os.path.join(
-#                                        package_path,
-#                                        'worlds',
-#                                        'warehouse.world',
-#                                    )
-#                          }.items()
-#                        )
-#    ld.add_action(gazebo_launch_cmd)
 
 import os
 
@@ -71,7 +57,7 @@ def generate_launch_description():
             os.path.join(ros_gz_sim, 'launch', 'gz_sim.launch.py')
         ),
         launch_arguments={
-            'gz_args': [world, ' -v 4'],
+            'gz_args': [world, ' -v 4 -r'],
             'on_exit_shutdown': 'true'
         }.items(),
     )
@@ -85,21 +71,4 @@ def generate_launch_description():
         output='screen',
     )
     ld.add_action(gz_ros_bridge_clock)
-
-    # Remove faulty TF Bridge - TF will be handled by robot state publishers
-    # and static transform publishers instead of trying to bridge from Gazebo
-    # gz_ros_bridge_tf = Node(
-    #     package='ros_gz_bridge',
-    #     executable='parameter_bridge',
-    #     arguments=[
-    #         '/tf@tf2_msgs/msg/TFMessage[gz.msgs.Pose_V',
-    #         '/tf_static@tf2_msgs/msg/TFMessage[gz.msgs.Pose_V'
-    #     ],
-    #     output='screen',
-    # )
-    # ld.add_action(gz_ros_bridge_tf)
-
-    # Entity service bridges are intentionally disabled for Harmonic in this launch.
-    # Primary runtime uses TF-based object/AMR tracking, which is stable and sufficient.
-
     return ld