Add lunar pole exploration rover demo package

lunar_pole_exploration_rover/CMakeLists.txt

@@ -0,0 +1,51 @@
+cmake_minimum_required(VERSION 3.8)
+project(lunar_pole_exploration_rover)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_python REQUIRED)
+find_package(control_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(simulation REQUIRED)
+find_package(std_msgs REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+install(DIRECTORY
+  config
+  launch
+  worlds
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_python_install_package(${PROJECT_NAME})
+
+install(PROGRAMS
+  nodes/move_mast
+  nodes/move_wheel
+  nodes/run_demo
+  nodes/odom_tf_publisher
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+ament_package()

lunar_pole_exploration_rover/config/lunar_pole_exploration_rover_control.yaml

@@ -0,0 +1,51 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 100
+
+    mast_camera_joint_trajectory_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+    wheel_velocity_controller:
+      type: velocity_controllers/JointGroupVelocityController
+
+    steer_position_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+
+mast_camera_joint_trajectory_controller:
+  ros__parameters:
+    joints:
+      - mast_head_pivot_joint
+      - mast_camera_joint
+    interface_name: position
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity
+
+wheel_velocity_controller:
+  ros__parameters:
+    joints:
+      - front_left_wheel_joint
+      - rear_left_wheel_joint
+      - front_right_wheel_joint
+      - rear_right_wheel_joint
+    interface_name: velocity
+
+steer_position_controller:
+  ros__parameters:
+    joints:
+      - front_left_wheel_axle_joint
+      - rear_left_wheel_axle_joint
+      - front_right_wheel_axle_joint
+      - rear_right_wheel_axle_joint
+    interface_name: position
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity

lunar_pole_exploration_rover/launch/lunar_pole_exploration_rover.launch.py

@@ -0,0 +1,181 @@
+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
+
+from ament_index_python.packages import get_package_share_directory
+
+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():
+
+    lunar_pole_exploration_rover_demos_path = get_package_share_directory('lunar_pole_exploration_rover')
+    lunar_pole_exploration_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=''), lunar_pole_exploration_rover_demos_path])}
+
+    urdf_model_path = os.path.join(lunar_pole_exploration_rover_models_path, 'models', 'lunar_pole_exploration_rover',
+        'urdf', 'lunar_pole_exploration_rover.xacro')
+    lunar_pole_world_model = os.path.join(lunar_pole_exploration_rover_demos_path, 'worlds', 'lunar_pole.world')
+
+    doc = xacro.process_file(urdf_model_path)
+    robot_description = {'robot_description': doc.toxml()}
+
+    mast_node = Node(
+        package="lunar_pole_exploration_rover",
+        executable="move_mast",
+        output='screen'
+    )
+
+    wheel_node = Node(
+        package="lunar_pole_exploration_rover",
+        executable="move_wheel",
+        output='screen'
+    )
+
+    run_node = Node(
+        package="lunar_pole_exploration_rover",
+        executable="run_demo",
+        output='screen'
+    )
+
+    odom_node = Node(
+        package="lunar_pole_exploration_rover",
+        executable="odom_tf_publisher",
+        output='screen'
+    )
+
+    start_world = ExecuteProcess(
+        cmd=['ign gazebo', lunar_pole_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/lunar_pole_exploration_rover/odometry@nav_msgs/msg/[email protected]',
+                '/model/lunar_pole_exploration_rover/left_solar_panel/solar_panel_output@std_msgs/msg/[email protected]',
+                '/model/lunar_pole_exploration_rover/right_solar_panel/solar_panel_output@std_msgs/msg/[email protected]',
+                '/model/lunar_pole_exploration_rover/rear_solar_panel/solar_panel_output@std_msgs/msg/[email protected]',
+            ],
+            output='screen')
+
+    image_bridge = Node(
+            package='ros_gz_image',
+            executable='image_bridge',
+            arguments=['navcam_left/image_raw', 'navcam_left/image_raw'],
+            output='screen')
+
+    image_bridge = Node(
+            package='ros_gz_image',
+            executable='image_bridge',
+            arguments=['navcam_right/image_raw', 'navcam_right/image_raw'],
+            output='screen')
+
+    spawn = Node(
+        package='ros_ign_gazebo', executable='create',
+        arguments=[
+            '-name', 'lunar_pole_exploration_rover',
+            '-topic', robot_description,
+            '-z', '0.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_mast_camera_joint_traj_controller = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
+             'mast_camera_joint_trajectory_controller'],
+        output='screen'
+    )
+
+    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'
+    )
+
+
+    return LaunchDescription([
+        SetParameter(name='use_sim_time', value=True),
+        start_world,
+        robot_state_publisher,
+        spawn,
+        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_mast_camera_joint_traj_controller,
+                         load_wheel_joint_traj_controller,
+                         load_steer_joint_traj_controller],
+            )
+        ),
+    ])

lunar_pole_exploration_rover/nodes/move_mast

@@ -0,0 +1,74 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from builtin_interfaces.msg import Duration
+
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+from std_srvs.srv import Empty
+
+
+class MastCamera(Node):
+
+    def __init__(self):
+        super().__init__('mast_node')
+        self.mast_publisher_ = self.create_publisher(JointTrajectory, '/mast_camera_joint_trajectory_controller/joint_trajectory', 10)
+        self.camera_center_srv = self.create_service(Empty, 'camera_center', self.camera_center_callback)
+        self.camera_rotate_srv = self.create_service(Empty, 'camera_rotate', self.camera_rotate_callback)
+
+    def camera_center_callback(self, request, response):
+        traj = JointTrajectory()
+        traj.joint_names = ["mast_head_pivot_joint", "mast_camera_joint"]
+
+        point = JointTrajectoryPoint()
+        point.positions = [0.0, 0.7854]
+        point.time_from_start = Duration(sec=1)
+
+        traj.points.append(point)
+        self.mast_publisher_.publish(traj)
+        return response
+
+    def camera_rotate_callback(self, request, response):
+        traj = JointTrajectory()
+        traj.joint_names = ["mast_head_pivot_joint", "mast_camera_joint"]
+
+        point1 = JointTrajectoryPoint()
+        point1.positions = [0.0, 0.7854]
+        point1.time_from_start = Duration(sec=1)
+        traj.points.append(point1)
+
+        point2 = JointTrajectoryPoint()
+        point2.positions = [-3.49, 0.0]
+        point2.time_from_start = Duration(sec=3)
+        traj.points.append(point2)
+
+        point3 = JointTrajectoryPoint()
+        point3.positions = [0.0, 0.7854]
+        point3.time_from_start = Duration(sec=6)
+        traj.points.append(point3)
+
+        point4 = JointTrajectoryPoint()
+        point4.positions = [3.49, 0.0]
+        point4.time_from_start = Duration(sec=9)
+        traj.points.append(point4)
+
+        self.mast_publisher_.publish(traj)
+        return response
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    mast_node = MastCamera()
+
+    rclpy.spin(mast_node)
+
+    # Destroy the node explicitly
+    # (optional - otherwise it will be done automatically
+    # when the garbage collector destroys the node object)
+    mast_node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()