Fix gazebo

.devcontainer/Dockerfile

@@ -29,17 +29,41 @@ RUN apt update && apt upgrade -y
 # Install Git
 RUN apt install -y git
 
+# Install necessary packages for Gazebo Harmonic and its dependencies
+RUN apt-get update && apt-get install -y \
+    software-properties-common \
+    wget \
+    lsb-release \
+    gnupg \
+    curl
+
+# Add Gazebo Harmonic repository
+RUN wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg
+RUN echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null
+
+RUN apt update -y 
+
 # Change user
 RUN chown $USER_UID:$USER_GID /workspace/$USERNAME
 USER $USERNAME
 
 # Copy to preload the ros packages
 COPY ./ /workspace/$USERNAME/
 
+# Source the ROS setup file
+RUN echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> ~/.bashrc
+
+COPY --chown=$USERNAME ./ /workspace/$USERNAME
+
+# Add needed packages
+RUN vcs import src < ovis.repos
+
+
 # Rosdep update
 RUN rosdep update
-RUN rosdep install --from-paths src --ignore-src --rosdistro humble -y
 
-# Source the ROS setup file
-RUN echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> ~/.bashrc
-RUN echo "source /gazebo/gazebo_ws/install/setup.bash" >> ~/.bashrc
+# Install ROS dependencies
+RUN rosdep install --from-paths src --ignore-src --rosdistro humble -y --skip-keys=gz-plugin2 --skip-keys=gz-sim8
+
+# Install gzharmonic after rosdep install
+RUN sudo apt install -y ros-humble-ros-gzharmonic

README.md

@@ -9,6 +9,7 @@ Clone and build the repository :
 ```bash
 git clone https://github.com/clubcapra/ovis_ros2.git
 cd ovis_ros2
+vcs import src < ovis.repos
 colcon build --symlink-install
 source install/setup.bash
 ```

ovis.repos

@@ -0,0 +1,5 @@
+repositories:
+  gz_ros2_control:
+    type: git
+    url: https://github.com/clubcapra/gz_ros2_control.git
+    version: iron

src/ovis_bringup/launch/launch.py

@@ -1,35 +1,29 @@
 import os
 from ament_index_python import get_package_share_directory
 from launch import LaunchDescription
-from launch.actions import RegisterEventHandler
-from launch.event_handlers import OnProcessExit
-from launch.substitutions import Command, FindExecutable, PathJoinSubstitution
+from launch.substitutions import Command
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.actions import IncludeLaunchDescription
+from launch.actions import ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler
+from launch.event_handlers import OnProcessExit
 
 from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
 from launch_ros.parameter_descriptions import ParameterValue
 
 def generate_launch_description():
     # Get the launch directory
-    pkg_ovis_description = get_package_share_directory('ovis_description')
-    bringup_pkg_path = get_package_share_directory('ovis_bringup')
     moveit_pkg_path = get_package_share_directory('ovis_moveit')
-    pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim')
     pkg_ovis_servo = get_package_share_directory('ovis_servo')
 
     # Get the URDF file (robot)
     urdf_path = os.path.join(moveit_pkg_path, 'config', 'ovis.urdf.xacro')
-    robot_desc = ParameterValue(Command(['xacro ', urdf_path]), value_type=str)
-
-    # Get the URDF file (world)
-    world_file_name = 'worlds/base_world.world'
-    world = os.path.join(pkg_ovis_description, world_file_name)
+    robot_desc = ParameterValue(Command(['xacro ', urdf_path, ' hardware_type:=', "mock"]), value_type=str)
 
     # Get the launch directory
     pkg_ovis_moveit = get_package_share_directory('ovis_moveit')
 
+    namespace = '/ovis'
+
 
     # Include launch files based on the configuration
     virtual_joints_launch = IncludeLaunchDescription(
@@ -42,6 +36,7 @@ def generate_launch_description():
     robot_state_publisher = Node(
         package='robot_state_publisher',
         executable='robot_state_publisher',
+        namespace=namespace,
         name='robot_state_publisher',
         output='both',
         parameters=[
@@ -69,6 +64,7 @@ def generate_launch_description():
     # Fake joint driver
     fake_joint_driver = Node(
         package='controller_manager',
+        namespace='/ovis',
         executable='ros2_control_node',
         parameters=[
             {'robot_description': robot_desc},
@@ -77,42 +73,15 @@ def generate_launch_description():
         ],
     )
 
-    spawn_controllers_launch = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(pkg_ovis_moveit, 'launch', 'spawn_controllers.launch.py')
-        )
-    )
-
-    # Setup to launch the simulator and Gazebo world
-    gz_sim = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py')),
-        launch_arguments={'gz_args': "-v 4 -r " + world}.items(),
-    )
-
-    # Spawn robot
-    create = Node(
-        package='ros_gz_sim',
-        executable='create',
-        arguments=['-name', 'ovis',
-                   '-topic', 'robot_description',
-                   '-x', '0',
-                   '-y', '0',
-                   '-z', '0.1',
-                   ],
-        output='screen',
+    load_joint_state_broadcaster = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
+             'joint_state_broadcaster', '-c', '/ovis/controller_manager'],
+        output='screen'
     )
 
-    # Bridge ROS topics and Gazebo messages for establishing communication
-    bridge = Node(
-        package='ros_gz_bridge',
-        executable='parameter_bridge',
-        parameters=[{
-            'config_file': os.path.join(pkg_ovis_description, 'config',
-                                        'default_bridge.yaml'),
-            'qos_overrides./tf_static.publisher.durability': 'transient_local',
-            "use_sim_time": True,
-        }],
+    load_arm_controller = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
+             'arm_controller', '-c', '/ovis/controller_manager'],
         output='screen'
     )
 
@@ -123,14 +92,22 @@ def generate_launch_description():
     )
 
     return LaunchDescription([
+            RegisterEventHandler(
+                event_handler=OnProcessExit(
+                    target_action=robot_state_publisher,
+                    on_exit=[load_joint_state_broadcaster],
+                )
+            ),
+            RegisterEventHandler(
+                event_handler=OnProcessExit(
+                    target_action=load_joint_state_broadcaster,
+                    on_exit=[load_arm_controller],
+                )
+            ),
             virtual_joints_launch,
             robot_state_publisher,
             move_group_launch,
             rviz_launch,
             fake_joint_driver,
-            spawn_controllers_launch,
             servo,
-            #gz_sim,
-            #bridge,
-            #create,
             ])

src/ovis_bringup/launch/real.launch.py

@@ -0,0 +1,90 @@
+import os
+from ament_index_python import get_package_share_directory
+from launch import LaunchDescription
+from launch.substitutions import Command
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.actions import IncludeLaunchDescription
+
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+
+def generate_launch_description():
+    # Get the launch directory
+    moveit_pkg_path = get_package_share_directory('ovis_moveit')
+    pkg_ovis_servo = get_package_share_directory('ovis_servo')
+
+    # Get the URDF file (robot)
+    urdf_path = os.path.join(moveit_pkg_path, 'config', 'ovis.urdf.xacro')
+    robot_desc = ParameterValue(Command(['xacro ', urdf_path, ' hardware_type:=', "ovis"]), value_type=str)
+
+    # Get the launch directory
+    pkg_ovis_moveit = get_package_share_directory('ovis_moveit')
+
+
+    # Include launch files based on the configuration
+    virtual_joints_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_ovis_moveit, 'launch', 'static_virtual_joint_tfs.launch.py')
+        )
+    )
+
+    # Takes the description and joint angles as inputs and publishes
+    robot_state_publisher = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        name='robot_state_publisher',
+        output='both',
+        parameters=[
+            {'robot_description': robot_desc},
+            {"use_sim_time": True, }
+        ]
+    )
+
+    move_group_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_ovis_moveit, 'launch', 'move_group.launch.py'),
+        ),
+        launch_arguments={
+            "use_sim_time": "true",
+        }.items(),
+    )
+
+    rviz_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_ovis_moveit, 'launch', 'moveit_rviz.launch.py')
+        ),
+    )
+
+
+    # kinova driver
+    kinova_joint_driver = Node(
+        package='controller_manager',
+        executable='ros2_control_node',
+        parameters=[
+            {'robot_description': robot_desc},
+            os.path.join(pkg_ovis_moveit, 'config', 'ros2_controllers.yaml'),
+
+        ],
+    )
+
+    spawn_controllers_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_ovis_moveit, 'launch', 'spawn_controllers.launch.py')
+        )
+    )
+
+    servo =  IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(pkg_ovis_servo, 'launch', 'servo.launch.py')
+        )
+    )
+
+    return LaunchDescription([
+            virtual_joints_launch,
+            robot_state_publisher,
+            move_group_launch,
+            rviz_launch,
+            kinova_joint_driver,
+            spawn_controllers_launch,
+            servo,
+            ])