From b533bdd7556ce4107a7c6e4cf340b02bd083d79e Mon Sep 17 00:00:00 2001 From: RBinsonB Date: Fri, 6 Sep 2024 05:32:50 +0200 Subject: [PATCH] Add lunar_pole_exploration_rover demo (fix issue #36) Add the demo launch files and control nodes. Also adds the controller configuration and a gazebo lunar world. A dockerfile is included for test. --- lunar_pole_exploration_rover/CMakeLists.txt | 51 ++++ .../lunar_pole_exploration_rover_control.yaml | 51 ++++ .../docker/Dockerfile | 97 +++++++ .../docker/demo_manual_pkgs.repos | 34 +++ .../docker/entrypoint.sh | 6 + .../lunar_pole_exploration_rover.launch.py | 233 +++++++++++++++++ .../lunar_pole_exploration_rover/__init__.py | 0 lunar_pole_exploration_rover/nodes/move_mast | 74 ++++++ lunar_pole_exploration_rover/nodes/move_wheel | 239 ++++++++++++++++++ .../nodes/odom_tf_publisher | 68 +++++ lunar_pole_exploration_rover/nodes/run_demo | 155 ++++++++++++ lunar_pole_exploration_rover/package.xml | 42 +++ .../worlds/lunar_pole.world | 81 ++++++ 13 files changed, 1131 insertions(+) create mode 100644 lunar_pole_exploration_rover/CMakeLists.txt create mode 100644 lunar_pole_exploration_rover/config/lunar_pole_exploration_rover_control.yaml create mode 100644 lunar_pole_exploration_rover/docker/Dockerfile create mode 100644 lunar_pole_exploration_rover/docker/demo_manual_pkgs.repos create mode 100755 lunar_pole_exploration_rover/docker/entrypoint.sh create mode 100644 lunar_pole_exploration_rover/launch/lunar_pole_exploration_rover.launch.py create mode 100644 lunar_pole_exploration_rover/lunar_pole_exploration_rover/__init__.py create mode 100644 lunar_pole_exploration_rover/nodes/move_mast create mode 100644 lunar_pole_exploration_rover/nodes/move_wheel create mode 100644 lunar_pole_exploration_rover/nodes/odom_tf_publisher create mode 100644 lunar_pole_exploration_rover/nodes/run_demo create mode 100644 lunar_pole_exploration_rover/package.xml create mode 100755 lunar_pole_exploration_rover/worlds/lunar_pole.world diff --git a/lunar_pole_exploration_rover/CMakeLists.txt b/lunar_pole_exploration_rover/CMakeLists.txt new file mode 100644 index 00000000..27411dab --- /dev/null +++ b/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( 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() diff --git a/lunar_pole_exploration_rover/config/lunar_pole_exploration_rover_control.yaml b/lunar_pole_exploration_rover/config/lunar_pole_exploration_rover_control.yaml new file mode 100644 index 00000000..ef4503f3 --- /dev/null +++ b/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 diff --git a/lunar_pole_exploration_rover/docker/Dockerfile b/lunar_pole_exploration_rover/docker/Dockerfile new file mode 100644 index 00000000..1f46b039 --- /dev/null +++ b/lunar_pole_exploration_rover/docker/Dockerfile @@ -0,0 +1,97 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# A Docker configuration script to build the Space ROS image. +# +# The script provides the following build arguments: +# +# VCS_REF - The git revision of the Space ROS source code (no default value). +# VERSION - The version of Space ROS (default: "preview") + +FROM openrobotics/moveit2:latest + +# Define arguments used in the metadata definition +ARG VCS_REF +ARG VERSION="preview" + +# Specify the docker image metadata +LABEL org.label-schema.schema-version="1.0" +LABEL org.label-schema.name="Lunar Pole Exploration Rover" +LABEL org.label-schema.description="Lunar Pole Exploration Rover entry for the NASA Space ROS Sim Summer Sprint Challenge" +LABEL org.label-schema.vendor="Open Robotics" +LABEL org.label-schema.version=${VERSION} +LABEL org.label-schema.url="https://github.com/space-ros" +LABEL org.label-schema.vcs-url="https://github.com/space-ros/demos" +LABEL org.label-schema.vcs-ref=${VCS_REF} + +# Define a few key variables +ENV DEMO_DIR=${HOME_DIR}/demos_ws +ENV IGNITION_VERSION=fortress +ENV GZ_VERSION=fortress + +# Disable prompting during package installation +ARG DEBIAN_FRONTEND=noninteractive + +# Get rosinstall_generator +# Using Docker BuildKit cache mounts for /var/cache/apt and /var/lib/apt ensures that +# the cache won't make it into the built image but will be maintained between steps. +RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ + --mount=type=cache,target=/var/lib/apt,sharing=locked \ + sudo apt-get update -y && sudo apt-get install -y python3-rosinstall-generator + + +RUN mkdir -p ${DEMO_DIR}/src +WORKDIR ${DEMO_DIR} + + +# Install libmongoc for development +RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ + --mount=type=cache,target=/var/lib/apt,sharing=locked \ + sudo apt-get install libmongoc-dev -y + +# Compile mongo cxx driver https://mongocxx.org/mongocxx-v3/installation/linux/ +RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ + --mount=type=cache,target=/var/lib/apt,sharing=locked \ + sudo apt-get install libssl-dev build-essential devscripts debian-keyring fakeroot debhelper cmake libboost-dev libsasl2-dev libicu-dev libzstd-dev doxygen -y +RUN wget https://github.com/mongodb/mongo-cxx-driver/releases/download/r3.6.7/mongo-cxx-driver-r3.6.7.tar.gz +RUN tar -xzf mongo-cxx-driver-r3.6.7.tar.gz +RUN cd mongo-cxx-driver-r3.6.7/build && cmake .. -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX=/usr/local && sudo cmake --build . --target EP_mnmlstc_core && cmake --build . && sudo cmake --build . --target install + +# Get the source for the dependencies +# RUN vcs import src < /tmp/demo_generated_pkgs.repos +COPY --chown=${USERNAME}:${USERNAME} demos/lunar_pole_exploration_rover/docker/demo_manual_pkgs.repos /tmp/ +RUN vcs import src < /tmp/demo_manual_pkgs.repos && /bin/bash -c 'source "${SPACEROS_DIR}/install/setup.bash"' + +# Copy the "demos" and "simulation" repos into the workspace +COPY demos ./src/demos +COPY simulation ./src/simulation + +RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ + --mount=type=cache,target=/var/lib/apt,sharing=locked \ + sudo apt-get update -y \ +&& /bin/bash -c 'source "${SPACEROS_DIR}/install/setup.bash"' \ +&& /bin/bash -c 'source "${MOVEIT2_DIR}/install/setup.bash"' \ +&& rosdep install --from-paths src --ignore-src -r -y --rosdistro ${ROSDISTRO} + +# Build the demo +RUN /bin/bash -c 'source ${SPACEROS_DIR}/install/setup.bash && source ${MOVEIT2_DIR}/install/setup.bash \ + && colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release' + +# Add the user to the render group so that the user can access /dev/dri/renderD128 +RUN sudo usermod -aG render $USERNAME + +# Setup the entrypoint +COPY demos/lunar_pole_exploration_rover/docker/entrypoint.sh / +ENTRYPOINT ["/entrypoint.sh"] +CMD ["bash"] diff --git a/lunar_pole_exploration_rover/docker/demo_manual_pkgs.repos b/lunar_pole_exploration_rover/docker/demo_manual_pkgs.repos new file mode 100644 index 00000000..863ee3f0 --- /dev/null +++ b/lunar_pole_exploration_rover/docker/demo_manual_pkgs.repos @@ -0,0 +1,34 @@ +repositories: + gz_ros2_control: + type: git + url: https://github.com/ros-controls/gz_ros2_control.git + version: humble + qt_gui_core: + type: git + url: https://github.com/ros-visualization/qt_gui_core.git + version: humble + ros2_controllers: + type: git + url: https://github.com/tonylitianyu/ros2_controllers.git + version: effort_group_position_controller_2 + actuator_msgs: + type: git + url: https://github.com/rudislabs/actuator_msgs.git + version: main + ros_gz: + type: git + url: https://github.com/gazebosim/ros_gz.git + version: humble + ros-humble-warehouse-ros-mongo: + type: git + url: https://github.com/ros-planning/warehouse_ros_mongo.git + version: ros2 + vision_msgs: + type: git + url: https://github.com/ros-perception/vision_msgs.git + version: ros2 + gps_msgs: + type: git + url: https://github.com/swri-robotics/gps_umd.git + path: gps_msgs + version: 113782d diff --git a/lunar_pole_exploration_rover/docker/entrypoint.sh b/lunar_pole_exploration_rover/docker/entrypoint.sh new file mode 100755 index 00000000..d1b51f26 --- /dev/null +++ b/lunar_pole_exploration_rover/docker/entrypoint.sh @@ -0,0 +1,6 @@ +#!/bin/bash +set -e + +# Setup the Demo environment +source "${DEMO_DIR}/install/setup.bash" +exec "$@" diff --git a/lunar_pole_exploration_rover/launch/lunar_pole_exploration_rover.launch.py b/lunar_pole_exploration_rover/launch/lunar_pole_exploration_rover.launch.py new file mode 100644 index 00000000..72dbf7db --- /dev/null +++ b/lunar_pole_exploration_rover/launch/lunar_pole_exploration_rover.launch.py @@ -0,0 +1,233 @@ +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', + 'aft_cam_left/camera_info@sensor_msgs/msg/CameraInfo@ignition.msgs.CameraInfo', + '/aft_cam_right/camera_info@sensor_msgs/msg/CameraInfo@ignition.msgs.CameraInfo', + '/nav_cam_left/camera_info@sensor_msgs/msg/CameraInfo@ignition.msgs.CameraInfo', + '/nav_cam_right/camera_info@sensor_msgs/msg/CameraInfo@ignition.msgs.CameraInfo', + '/haz_cam_left_front/camera_info@sensor_msgs/msg/CameraInfo@ignition.msgs.CameraInfo', + '/haz_cam_left_rear/camera_info@sensor_msgs/msg/CameraInfo@ignition.msgs.CameraInfo', + '/haz_cam_right_front/camera_info@sensor_msgs/msg/CameraInfo@ignition.msgs.CameraInfo', + '/haz_cam_right_rear/camera_info@sensor_msgs/msg/CameraInfo@ignition.msgs.CameraInfo', + '/model/lunar_pole_exploration_rover/odometry@nav_msgs/msg/Odometry@ignition.msgs.Odometry', + '/model/lunar_pole_exploration_rover/left_solar_panel/solar_panel_output@std_msgs/msg/Float32@ignition.msgs.Float', + '/model/lunar_pole_exploration_rover/right_solar_panel/solar_panel_output@std_msgs/msg/Float32@ignition.msgs.Float', + '/model/lunar_pole_exploration_rover/rear_solar_panel/solar_panel_output@std_msgs/msg/Float32@ignition.msgs.Float', + '/model/lunar_pole_exploration_rover/battery/rechargeable_battery/state@sensor_msgs/msg/BatteryState@ignition.msgs.BatteryState', + ], + output='screen') + + navcam_left_image_bridge = Node( + package='ros_gz_image', + executable='image_bridge', + arguments=['navcam_left/image_raw', 'navcam_left/image_raw'], + output='screen') + + navcam_right_image_bridge = Node( + package='ros_gz_image', + executable='image_bridge', + arguments=['navcam_right/image_raw', 'navcam_right/image_raw'], + output='screen') + + aftcam_left_image_bridge = Node( + package='ros_gz_image', + executable='image_bridge', + arguments=['aftcam_left/image_raw', 'aftcam_left/image_raw'], + output='screen') + + aftcam_right_image_bridge = Node( + package='ros_gz_image', + executable='image_bridge', + arguments=['aftcam_right/image_raw', 'aftcam_right/image_raw'], + output='screen') + + hazcam_left_front_image_bridge = Node( + package='ros_gz_image', + executable='image_bridge', + arguments=['hazcam_left_front/image_raw', 'hazcam_left_front/image_raw'], + output='screen') + + hazcam_left_rear_image_bridge = Node( + package='ros_gz_image', + executable='image_bridge', + arguments=['hazcam_left_rear/image_raw', 'hazcam_left_rear/image_raw'], + output='screen') + + hazcam_right_front_image_bridge = Node( + package='ros_gz_image', + executable='image_bridge', + arguments=['hazcam_right_front/image_raw', 'hazcam_right_front/image_raw'], + output='screen') + + hazcam_right_rear_image_bridge = Node( + package='ros_gz_image', + executable='image_bridge', + arguments=['hazcam_right_rear/image_raw', 'hazcam_right_rear/image_raw'], + output='screen') + + spawn = Node( + package='ros_ign_gazebo', executable='create', + arguments=[ + '-name', 'lunar_pole_exploration_rover', + '-topic', robot_description, + '-z', '0.0', + ], + 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, + navcam_left_image_bridge, + navcam_right_image_bridge, + aftcam_left_image_bridge, + aftcam_right_image_bridge, + hazcam_left_front_image_bridge, + hazcam_left_rear_image_bridge, + hazcam_right_front_image_bridge, + hazcam_right_rear_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], + ) + ), + ]) diff --git a/lunar_pole_exploration_rover/lunar_pole_exploration_rover/__init__.py b/lunar_pole_exploration_rover/lunar_pole_exploration_rover/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/lunar_pole_exploration_rover/nodes/move_mast b/lunar_pole_exploration_rover/nodes/move_mast new file mode 100644 index 00000000..c9705e77 --- /dev/null +++ b/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() diff --git a/lunar_pole_exploration_rover/nodes/move_wheel b/lunar_pole_exploration_rover/nodes/move_wheel new file mode 100644 index 00000000..d7040a94 --- /dev/null +++ b/lunar_pole_exploration_rover/nodes/move_wheel @@ -0,0 +1,239 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from builtin_interfaces.msg import Duration + +from std_msgs.msg import Float64MultiArray +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint +from geometry_msgs.msg import Twist +import math + + +class MoveWheel(Node): + + def __init__(self): + super().__init__('wheel_node') + self.wheel_publisher_ = self.create_publisher( + Float64MultiArray, + '/wheel_velocity_controller/commands', 10) + self.steer_publisher_ = self.create_publisher( + JointTrajectory, + '/steer_position_controller/joint_trajectory', 10) + timer_period = 0.1 # seconds + self.timer = self.create_timer(timer_period, self.timer_callback) + + self.vel_sub = self.create_subscription( + Twist, '/cmd_vel', self.vel_callback, 10) + self.cmd_msg = Twist() + + # Wheel steering angles + # [front_left, rear_front, front_right, rear_right] + self.wheel_steer_angles = [0, 0, 0, 0] + self.prev_steer = self.wheel_steer_angles + # Wheel linear velocities + # [front_left, rear_front, front_right, rear_right] + self.wheel_linear_velocities = [0, 0, 0, 0] + + # Robot characteristics + self.wheel_radius = 0.225 + self.L = 0.870 # Chassis Length + self.T = 1.050 # Chassis Width + self.wheel_steer_limit = 0.9 + # Angle to apply to wheel steer to be perpendicular to robot center + self.wheel_steer_centering_angle = math.atan(self.L / self.T) + # Distance between wheel steering axis and robot center + self.dist_wheel_center = math.sqrt( + (self.L / 2)**2 + + (self.T / 2)**2) + + def vel_callback(self, msg): + self.cmd_msg = msg + + def compute_command(self, msg): + ## Rotate on place + if ( + (abs(msg.angular.z) > 0.01) and + (msg.linear.x == 0) and + (msg.linear.y == 0) + ): + self.set_rotate_on_place(msg.angular.z) + + ## Stop motion + elif ( + (abs(msg.angular.z) < 0.01) and + (abs(msg.linear.x) < 0.001) and + (abs(msg.linear.y) < 0.001) + ): + self.wheel_steer_angles = [0.0, 0.0, 0.0, 0.0] + self.wheel_linear_velocities = [0.0, 0.0, 0.0, 0.0] + + ## Combined motion + else: + # Set first translational motion + common_steer_angle, wheel_lin_vel = self.get_translation_motion( + msg.linear.x, msg.linear.y) + + # If angular vel not negligeable, add to steer angles and + # wheel velocities + if (abs(msg.angular.z) > 0.01): + self.wheel_steer_angles, self.wheel_linear_velocities = self.get_rotation_motion( + common_steer_angle, + wheel_lin_vel, + msg.angular.z) + else: + # Wheel steering angles for a translation motion (same for all) + # [front_left, rear_front, front_right, rear_right] + self.wheel_steer_angles = [ + common_steer_angle, + common_steer_angle, + common_steer_angle, + common_steer_angle] + + # Wheel linear velocities for a translation motion (same for all) + # [front_left, rear_front, front_right, rear_right] + self.wheel_linear_velocities = [ + wheel_lin_vel, + wheel_lin_vel, + wheel_lin_vel, + wheel_lin_vel] + + def normalize(self, angle): + return math.atan2(math.sin(angle), math.cos(angle)) + + def set_rotate_on_place(self, angular_vel): + # Wheel steering angles to rotate on place + # [front_left, rear_front, front_right, rear_right] + self.wheel_steer_angles = [ + -self.wheel_steer_centering_angle, + self.wheel_steer_centering_angle, + self.wheel_steer_centering_angle, + -self.wheel_steer_centering_angle] + + # Calculate wheel linear velocities + # [front_left, rear_front, front_right, rear_right] + wheel_lin_vel = self.dist_wheel_center * angular_vel + self.wheel_linear_velocities = [ + -wheel_lin_vel, + -wheel_lin_vel, + wheel_lin_vel, + wheel_lin_vel] + + def get_translation_motion(self, linear_vel_x, linear_vel_y): + vel_norm = math.sqrt(linear_vel_x**2 + linear_vel_y**2) + common_steer_angle = math.atan2(linear_vel_y, linear_vel_x) + + if abs(common_steer_angle) > (math.pi / 2): + common_steer_angle = self.normalize(common_steer_angle - math.pi) + vel_norm = - vel_norm + + return common_steer_angle, vel_norm + + def get_rotation_motion(self, robot_side_angle, lin_vel, angular_vel): + # Robot turning radius according to Ideal Ackerman Steering + steering_radius = lin_vel / angular_vel + + # Center of rotation (rotated by previous robot side motion + # direction angle) + xR = - steering_radius * math.sin(robot_side_angle) + yR = steering_radius * math.cos(robot_side_angle) + + # Each wheel coordinate (frame in the center of the robot) + # [front_left, rear_front, front_right, rear_right] + wheel_coords = [ + (self.L / 2, self.T / 2), + (- self.L / 2, self.T / 2), + (self.L / 2, -self.T / 2), + (-self.L / 2, -self.T / 2), + ] + + wheel_steer_angles = [0, 0, 0, 0] + wheel_velocities = [0, 0, 0, 0] + # vector from rotation center to robot center + rot_center_to_robot = (-xR, -yR) + for i in range(len(wheel_coords)): + # vector from rotation center to wheel point + rot_center_to_wheel = ( + wheel_coords[i][0] - xR, + wheel_coords[i][1] - yR) + + # https://wumbo.net/formulas/angle-between-two-vectors-2d/ + # Get the angle between the rotation center and the wheel to steer + # the wheel so that it runs perpendicular to the turning radius + wheel_steer_angles[i] = robot_side_angle + math.atan2( + rot_center_to_wheel[1] * rot_center_to_robot[0] - rot_center_to_wheel[0] * rot_center_to_robot[1], + rot_center_to_wheel[0] * rot_center_to_robot[0] + rot_center_to_wheel[1] * rot_center_to_robot[1]) + # Adjust wheel velocity according to distance to rotation center + dist_rot_center_to_wheel = math.sqrt( + rot_center_to_wheel[0]**2 + rot_center_to_wheel[1]**2) + wheel_velocities[i] = math.copysign( + angular_vel * dist_rot_center_to_wheel, lin_vel) + + return wheel_steer_angles, wheel_velocities + + def publish_wheel_vel_control(self): + wheel_rotation_vel = [ + (vel / self.wheel_radius) for vel in self.wheel_linear_velocities + ] + target_vel = Float64MultiArray() + target_vel.data = wheel_rotation_vel + self.wheel_publisher_.publish(target_vel) + + def clamp_steer_angle(self, angle) -> float: + return min( + self.wheel_steer_limit, max(angle, - self.wheel_steer_limit)) + + def publish_wheel_steer_control(self): + # clamp steering angles + clamped_angles = [ + self.clamp_steer_angle(angle) for angle in self.wheel_steer_angles + ] + # Check if steering changed + changed = False + for i in range(len(clamped_angles)): + if abs(self.prev_steer[i] - clamped_angles[i]) > 1e-3: + changed = True + break + + if changed: + target_steer = JointTrajectory() + target_steer.joint_names = [ + "front_left_wheel_axle_joint", + "rear_left_wheel_axle_joint", + "front_right_wheel_axle_joint", + "rear_right_wheel_axle_joint"] + steer_point = JointTrajectoryPoint() + + steer_point.positions = clamped_angles + steer_point.time_from_start = Duration(sec=0, nanosec=1000000) + target_steer.points.append(steer_point) + + self.steer_publisher_.publish(target_steer) + self.prev_steer = clamped_angles + + def timer_callback(self): + self.compute_command(self.cmd_msg) + self.publish_wheel_vel_control() + self.publish_wheel_steer_control() + + # Reset command (will stop robot if not receiving + # commands at a higher rate than timer) + self.cmd_msg = Twist() + + +def main(args=None): + rclpy.init(args=args) + + wheel_node = MoveWheel() + + rclpy.spin(wheel_node) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + wheel_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/lunar_pole_exploration_rover/nodes/odom_tf_publisher b/lunar_pole_exploration_rover/nodes/odom_tf_publisher new file mode 100644 index 00000000..d6f33c73 --- /dev/null +++ b/lunar_pole_exploration_rover/nodes/odom_tf_publisher @@ -0,0 +1,68 @@ +#!/usr/bin/env python3 + +# adapted from ROS 2 tutorial +# https://docs.ros.org/en/rolling/Tutorials/Intermediate/Tf2/Writing-A-Tf2-Broadcaster-Py.html + +import rclpy + +from nav_msgs.msg import Odometry +from geometry_msgs.msg import TransformStamped +from rclpy.node import Node +from tf2_ros import TransformBroadcaster + + +class OdomTFBroadcaster(Node): + + def __init__(self): + super().__init__('odom_tf_publisher') + + # Initialize the transform broadcaster + self.tf_broadcaster = TransformBroadcaster(self) + + # Subscribe to Odometry topic + # TODO: make the topic name a parameter instead of hard-coded + self.subscription = self.create_subscription( + Odometry, + '/model/lunar_pole_exploration_rover/odometry', + self.handle_odometry, + 1) + self.subscription # prevent unused variable warning + + def handle_odometry(self, msg): + tf = TransformStamped() + + # Read message content and assign it to + # corresponding tf variables + tf.header.stamp = msg.header.stamp + tf.header.frame_id = msg.header.frame_id + tf.child_frame_id = msg.child_frame_id + + # get translation coordinates from the message pose + tf.transform.translation.x = msg.pose.pose.position.x + tf.transform.translation.y = msg.pose.pose.position.y + tf.transform.translation.z = msg.pose.pose.position.z + + # get rotation from the message pose + tf.transform.rotation.x = msg.pose.pose.orientation.x + tf.transform.rotation.y = msg.pose.pose.orientation.y + tf.transform.rotation.z = msg.pose.pose.orientation.z + tf.transform.rotation.w = msg.pose.pose.orientation.w + + # Send the transformation + self.tf_broadcaster.sendTransform(tf) + + +def main(): + rclpy.init() + node = OdomTFBroadcaster() + node.get_logger().info('Starting odometry_tf_publisher node') + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/lunar_pole_exploration_rover/nodes/run_demo b/lunar_pole_exploration_rover/nodes/run_demo new file mode 100644 index 00000000..097ea14a --- /dev/null +++ b/lunar_pole_exploration_rover/nodes/run_demo @@ -0,0 +1,155 @@ +#!/usr/bin/env python3 + + +import rclpy +from rclpy.node import Node + +from geometry_msgs.msg import Twist +from std_srvs.srv import Empty + + +# A node for performing combination of motions from different moving parts of the VIPER rover. +class RunDemo(Node): + def __init__(self) -> None: + super().__init__('run_node') + self.motion_publisher_ = self.create_publisher(Twist, '/cmd_vel', 10) + + self.forward_srv = self.create_service( + Empty, + 'move_forward', + self.move_forward_callback) + + self.stop_srv = self.create_service( + Empty, + 'move_stop', + self.move_stop_callback) + + self.left_srv = self.create_service( + Empty, + 'turn_left', + self.turn_left_callback) + + self.right_srv = self.create_service( + Empty, + 'turn_right', + self.turn_right_callback) + + self.sideway_left_srv = self.create_service( + Empty, + 'move_sideway_left', + self.move_sideway_left_callback) + + self.sideway_right_srv = self.create_service( + Empty, + 'move_sideway_right', + self.move_sideway_right_callback) + + self.rotate_on_place_srv = self.create_service( + Empty, + 'rotate_on_place', + self.rotate_on_place_callback) + + self.move_sideway_and_turn_left_srv = self.create_service( + Empty, + 'move_sideway_and_turn_left', + self.move_sideway_and_turn_left_callback) + + self.stopped = True + + timer_period = 0.08 + self.timer = self.create_timer(timer_period, self.timer_callback) + + self.curr_action = Twist() + + def timer_callback(self): + if (not self.stopped): + self.motion_publisher_.publish(self.curr_action) + + def move_forward_callback(self, request, response): + self.get_logger().info("Moving forward") + action = Twist() + action.linear.x = 2.0 + self.curr_action = action + self.stopped = False + return response + + def move_stop_callback(self, request, response): + # stop timer from publishing + self.stopped = True + self.get_logger().info("Stopping") + self.curr_action = Twist() + # publish once to ensure we stop + self.motion_publisher_.publish(self.curr_action) + return response + + def turn_left_callback(self, request, response): + self.get_logger().info("Turning left") + action = Twist() + action.linear.x = 1.0 + action.angular.z = 0.4 + self.curr_action = action + self.stopped = False + return response + + def turn_right_callback(self, request, response): + self.get_logger().info("Turning right") + action = Twist() + action.linear.x = 1.0 + action.angular.z = -0.4 + self.curr_action = action + self.stopped = False + return response + + def move_sideway_left_callback(self, request, response): + self.get_logger().info("Moving sideway left") + action = Twist() + action.linear.x = 0.1 + action.linear.y = 0.8 + self.curr_action = action + self.stopped = False + return response + + def move_sideway_right_callback(self, request, response): + self.get_logger().info("Moving sideway right") + action = Twist() + action.linear.x = 0.1 + action.linear.y = -0.8 + self.curr_action = action + self.stopped = False + return response + + def rotate_on_place_callback(self, request, response): + self.get_logger().info("Rotating on place") + action = Twist() + action.angular.z = 0.8 + self.curr_action = action + self.stopped = False + return response + + def move_sideway_and_turn_left_callback(self, request, response): + self.get_logger().info("Moving sideway and turning left ") + action = Twist() + action.linear.x = 0.1 + action.linear.y = 0.8 + action.angular.z = 0.8 + self.curr_action = action + self.stopped = False + return response + + +def main(args=None): + rclpy.init(args=args) + + run_demo = RunDemo() + + rclpy.spin(run_demo) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + run_demo.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/lunar_pole_exploration_rover/package.xml b/lunar_pole_exploration_rover/package.xml new file mode 100644 index 00000000..f44cbb47 --- /dev/null +++ b/lunar_pole_exploration_rover/package.xml @@ -0,0 +1,42 @@ + + + + lunar_pole_exploration_rover + 0.0.1 + A lunar pole exploration rover mission for SpaceROS inspired by the Volatiles Investigating Polar Exploration Rover + Robin BARAN + Apache-2.0 + + ament_cmake + ament_cmake_python + + rclcpp + rclpy + simulation + + ament_index_python + control_msgs + diff_drive_controller + effort_controllers + geometry_msgs + hardware_interface + ign_ros2_control + imu_sensor_broadcaster + joint_state_broadcaster + joint_trajectory_controller + launch + launch_ros + robot_state_publisher + ros_ign_gazebo + ros2controlcli + std_msgs + velocity_controllers + xacro + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/lunar_pole_exploration_rover/worlds/lunar_pole.world b/lunar_pole_exploration_rover/worlds/lunar_pole.world new file mode 100755 index 00000000..91fadb92 --- /dev/null +++ b/lunar_pole_exploration_rover/worlds/lunar_pole.world @@ -0,0 +1,81 @@ + + + + + + + 3D View + false + docked + + ogre2 + scene + false + 0.0 0.0 0.0 + -5.0 5.0 2.0 0.0 -0.2 -0.78 + + + + + floating + + + + + + + + + + ogre2 + + + + + + + + + + + true + -2000 2000 100 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + 1.5 + + 100000000 + 1.0 + 0.0 + 0.001 + + 0.9 -0.9 -0.2 + + + + 0 0 -1.62 + + 0 0 0 + + 0.0 + 300.0 + 0.0 + + + + model://moon_mons_mouton + moon_mons_mouton + -240 260 -41.5 0 0 0 + + + +