Update curiosity_rover_demo to the changes

related to space-ros/space-ros#178
 - Cleaned up rover launch file for the demo
 - Updated comments for demo node

curiosity_rover/.defaults.yaml

@@ -0,0 +1,6 @@
+{
+    "build": {
+        "symlink-install": true,
+        "cmake_args": "-DCMAKE_BUILD_TYPE=Release"
+    }
+}

curiosity_rover/.gitignore

@@ -0,0 +1,3 @@
+build/
+install/
+log/

curiosity_rover/curiosity_rover_demo/CMakeLists.txt

@@ -13,7 +13,6 @@ find_package(geometry_msgs REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_action REQUIRED)
 find_package(rclpy REQUIRED)
-find_package(curiosity_description REQUIRED)
 find_package(std_msgs REQUIRED)
 
 

curiosity_rover/curiosity_rover_demo/launch/mars_rover.launch.py

@@ -1,194 +1,34 @@
-from http.server import executable
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, ExecuteProcess, RegisterEventHandler, IncludeLaunchDescription
-from launch.substitutions import TextSubstitution, PathJoinSubstitution, LaunchConfiguration, Command
-from launch_ros.actions import Node, SetParameter
-from launch_ros.substitutions import FindPackageShare
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.event_handlers import OnProcessExit, OnExecutionComplete
-import os
-from os import environ
+"""Launch the curiosity rover demo."""
 
-from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription  # type: ignore
+from launch_ros.actions import Node, SetParameter  # type: ignore
 
-import xacro
-
-
-
-# . ../spaceros_ws/install/setup.bash && . ../depends_ws/install/setup.bash
-# rm -rf build install log && colcon build && . install/setup.bash
 
 def generate_launch_description():
-
-    mars_rover_demos_path = get_package_share_directory('mars_rover')
-    mars_rover_models_path = get_package_share_directory('simulation')
-
-    env = {'IGN_GAZEBO_SYSTEM_PLUGIN_PATH':
-           ':'.join([environ.get('IGN_GAZEBO_SYSTEM_PLUGIN_PATH', default=''),
-                     environ.get('LD_LIBRARY_PATH', default='')]),
-           'IGN_GAZEBO_RESOURCE_PATH':
-           ':'.join([environ.get('IGN_GAZEBO_RESOURCE_PATH', default=''), mars_rover_demos_path])}
-
-    urdf_model_path = os.path.join(mars_rover_models_path, 'models', 'curiosity_path',
-        'urdf', 'curiosity_mars_rover.xacro')
-    mars_world_model = os.path.join(mars_rover_demos_path, 'worlds', 'mars_curiosity.world')
-
-    doc = xacro.process_file(urdf_model_path)
-    robot_description = {'robot_description': doc.toxml()}
+    """Launch the curiosity rover demo."""
 
     arm_node = Node(
-        package="mars_rover",
-        executable="move_arm",
-        output='screen'
+        package="curiosity_rover_demo", executable="move_arm", output="screen"
     )
 
     mast_node = Node(
-        package="mars_rover",
-        executable="move_mast",
-        output='screen'
+        package="curiosity_rover_demo", executable="move_mast", output="screen"
     )
 
     wheel_node = Node(
-        package="mars_rover",
-        executable="move_wheel",
-        output='screen'
+        package="curiosity_rover_demo", executable="move_wheel", output="screen"
     )
 
     run_node = Node(
-        package="mars_rover",
-        executable="run_demo",
-        output='screen'
-    )
-
-    odom_node = Node(
-        package="mars_rover",
-        executable="odom_tf_publisher",
-        output='screen'
-    )
-
-    start_world = ExecuteProcess(
-        cmd=['ign gazebo', mars_world_model, '-r'],
-        output='screen',
-        additional_env=env,
-        shell=True
-    )
-
-    robot_state_publisher = Node(
-            package='robot_state_publisher',
-            executable='robot_state_publisher',
-            name='robot_state_publisher',
-            output='screen',
-            parameters=[robot_description])
-
-    ros_gz_bridge = Node(
-            package='ros_gz_bridge',
-            executable='parameter_bridge',
-            arguments=[
-                '/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock',
-                '/model/curiosity_mars_rover/odometry@nav_msgs/msg/Odometry@ignition.msgs.Odometry',
-                '/scan@sensor_msgs/msg/LaserScan@ignition.msgs.LaserScan',
-            ],
-            output='screen')
-
-    image_bridge = Node(
-            package='ros_gz_image',
-            executable='image_bridge',
-            arguments=['/image_raw', '/image_raw'],
-            output='screen')
-
-    spawn = Node(
-        package='ros_ign_gazebo', executable='create',
-        arguments=[
-            '-name', 'curiosity_mars_rover',
-            '-topic', robot_description,
-            '-z', '-7.5'
-        ],
-        output='screen'
-
-    )
-
-
-    ## Control Components
-
-    component_state_msg = '{name: "IgnitionSystem", target_state: {id: 3, label: ""}}'
-
-    ## a hack to resolve current bug
-    set_hardware_interface_active = ExecuteProcess(
-        cmd=['ros2', 'service', 'call', 
-            'controller_manager/set_hardware_component_state',
-            'controller_manager_msgs/srv/SetHardwareComponentState',
-            component_state_msg]
-    )
-
-    load_joint_state_broadcaster = ExecuteProcess(
-        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
-             'joint_state_broadcaster'],
-        output='screen'
-    )
-
-    load_arm_joint_traj_controller = ExecuteProcess(
-        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
-             'arm_joint_trajectory_controller'],
-        output='screen'
+        package="curiosity_rover_demo", executable="run_demo", output="screen"
     )
 
-    load_mast_joint_traj_controller = ExecuteProcess(
-        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
-             'mast_joint_trajectory_controller'],
-        output='screen'
+    return LaunchDescription(
+        [
+            SetParameter(name="use_sim_time", value=True),
+            arm_node,
+            mast_node,
+            wheel_node,
+            run_node,
+        ]
     )
-
-    load_wheel_joint_traj_controller = ExecuteProcess(
-        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
-             'wheel_velocity_controller'],
-        output='screen'
-    )
-
-    load_steer_joint_traj_controller = ExecuteProcess(
-        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
-             'steer_position_controller'],
-        output='screen'
-    )
-
-    load_suspension_joint_traj_controller = ExecuteProcess(
-        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
-             'wheel_tree_position_controller'],
-        output='screen'
-    )
-
-    return LaunchDescription([
-        SetParameter(name='use_sim_time', value=True),
-        start_world,
-        robot_state_publisher,
-        spawn,
-        arm_node,
-        mast_node,
-        wheel_node,
-        run_node,
-        odom_node,
-        ros_gz_bridge,
-        image_bridge,
-
-        RegisterEventHandler(
-            OnProcessExit(
-                target_action=spawn,
-                on_exit=[set_hardware_interface_active],
-            )
-        ),
-        RegisterEventHandler(
-            OnProcessExit(
-                target_action=set_hardware_interface_active,
-                on_exit=[load_joint_state_broadcaster],
-            )
-        ),
-        RegisterEventHandler(
-            OnProcessExit(
-                target_action=load_joint_state_broadcaster,
-                on_exit=[load_arm_joint_traj_controller,
-                        load_mast_joint_traj_controller,
-                        load_wheel_joint_traj_controller,
-                        load_steer_joint_traj_controller,
-                        load_suspension_joint_traj_controller],
-            )
-        ),
-    ])

curiosity_rover/curiosity_rover_demo/nodes/move_arm

@@ -1,47 +1,64 @@
 #!/usr/bin/env python3
+"""Move the arm of the curiosity rover."""
 
-import rclpy
-from rclpy.node import Node
-from builtin_interfaces.msg import Duration
+import rclpy  # type: ignore
+from rclpy.node import Node  # type: ignore
+from builtin_interfaces.msg import Duration  # type: ignore
+
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint  # type: ignore
+from std_srvs.srv import Empty  # type: ignore
 
-from std_msgs.msg import String, Float64
-from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
-from std_srvs.srv import Empty
 
 class MoveArm(Node):
+    """Move the arm of the curiosity rover."""
 
     def __init__(self):
-        super().__init__('arm_node')
-        self.arm_publisher_ = self.create_publisher(JointTrajectory, '/arm_joint_trajectory_controller/joint_trajectory', 10)
-        self.open_srv = self.create_service(Empty, 'open_arm', self.open_arm_callback)
-        self.close_srv = self.create_service(Empty, 'close_arm', self.close_arm_callback)
-
+        super().__init__("arm_node")
+        self.arm_publisher_ = self.create_publisher(
+            JointTrajectory, "/arm_joint_trajectory_controller/joint_trajectory", 10
+        )
+        self.open_srv = self.create_service(Empty, "open_arm", self.open_arm_callback)
+        self.close_srv = self.create_service(
+            Empty, "close_arm", self.close_arm_callback
+        )
 
         self.open = True
 
-
-    def open_arm_callback(self, request, response):
+    def open_arm_callback(self, request, response):  # pylint: disable=unused-argument
+        """Open the arm of the curiosity rover."""
         self.open = True
         traj = JointTrajectory()
-        traj.joint_names = ["arm_01_joint", "arm_02_joint", "arm_03_joint", "arm_04_joint", "arm_tools_joint"]
-
+        traj.joint_names = [
+            "arm_01_joint",
+            "arm_02_joint",
+            "arm_03_joint",
+            "arm_04_joint",
+            "arm_tools_joint",
+        ]
+
         point1 = JointTrajectoryPoint()
-        point1.positions = [0.0,-0.5,3.0,1.0,0.0]
+        point1.positions = [0.0, -0.5, 3.0, 1.0, 0.0]
         point1.time_from_start = Duration(sec=4)
 
         traj.points.append(point1)
         self.arm_publisher_.publish(traj)
 
-
         return response
 
-    def close_arm_callback(self, request, response):
+    def close_arm_callback(self, request, response):  # pylint: disable=unused-argument
+        """Close the arm of the curiosity rover."""
         self.open = False
         traj = JointTrajectory()
-        traj.joint_names = ["arm_01_joint", "arm_02_joint", "arm_03_joint", "arm_04_joint", "arm_tools_joint"]
-
+        traj.joint_names = [
+            "arm_01_joint",
+            "arm_02_joint",
+            "arm_03_joint",
+            "arm_04_joint",
+            "arm_tools_joint",
+        ]
+
         point1 = JointTrajectoryPoint()
-        point1.positions = [-1.57,-0.4,-1.1,-1.57,-1.57]
+        point1.positions = [-1.57, -0.4, -1.1, -1.57, -1.57]
         point1.time_from_start = Duration(sec=4)
 
         traj.points.append(point1)
@@ -50,8 +67,8 @@ class MoveArm(Node):
         return response
 
 
-
 def main(args=None):
+    """Main function"""
     rclpy.init(args=args)
 
     arm_node = MoveArm()
@@ -64,5 +81,5 @@ def main(args=None):
     rclpy.shutdown()
 
 
-if __name__ == '__main__':
-    main()
+if __name__ == "__main__":
+    main()