diff --git a/lunar_rover/CMakeLists.txt b/lunar_rover/CMakeLists.txt new file mode 100644 index 00000000..68c00ec9 --- /dev/null +++ b/lunar_rover/CMakeLists.txt @@ -0,0 +1,48 @@ +cmake_minimum_required(VERSION 3.8) +project(lunar_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 + launch + worlds + DESTINATION share/${PROJECT_NAME} +) + +ament_python_install_package(${PROJECT_NAME}) + +install(PROGRAMS + lunar_rover/waypoints.py + DESTINATION lib/${PROJECT_NAME} +) + + +ament_package() diff --git a/lunar_rover/README.md b/lunar_rover/README.md new file mode 100644 index 00000000..73619434 --- /dev/null +++ b/lunar_rover/README.md @@ -0,0 +1,40 @@ +# Lunar Rover +This package provides a simulation of the Apollo 15 Lunar Roving Vehicle navigating the Apollo 15 landing site. Included is an Ackermann steering model for the rover, along with a simple proportional controller for steering. The rover is also equipped with two cameras and an IMU. + + + + + +This package was created by Jasper Grant, as part of the NASA Space ROS Summer Sprint Challenge 2024. + +This package can be run with the existing 'space_robots' docker image. + +## Starting the Simulation + +To start the simulation, run the following command: + +```bash +ros2 launch lunar_rover lunar_rover.launch.py +``` + +To drive the rover using teleop, run the following command in a new terminal: + +```bash +ros2 run teleop_twist_keyboard teleop_twist_keyboard +``` + +To start the vehicle's proportional controller driving in a square, run the following command in a new terminal: + +```bash +ros2 run lunar_rover waypoints.py +``` + +## Topics + +| Topic | Message Type | Description | +|-------|--------------|-------------| +| /model/lunar_roving_vehicle/odometry | nav_msgs/Odometry | The odometry data from the rover | +| /imu | sensor_msgs/Imu | The IMU data from the rover | +| /cmd_vel | geometry_msgs/Twist | The velocity command for the rover | +| /img_raw_front | sensor_msgs/Image | The raw image data from the front camera | +| /img_raw_handheld | sensor_msgs/Image | The raw image data from the handheld camera | \ No newline at end of file diff --git a/lunar_rover/img/LRV_front_view.png b/lunar_rover/img/LRV_front_view.png new file mode 100644 index 00000000..a8edce72 Binary files /dev/null and b/lunar_rover/img/LRV_front_view.png differ diff --git a/lunar_rover/img/LRV_side_view.png b/lunar_rover/img/LRV_side_view.png new file mode 100644 index 00000000..fccb63ff Binary files /dev/null and b/lunar_rover/img/LRV_side_view.png differ diff --git a/lunar_rover/launch/lunar_rover.launch.py b/lunar_rover/launch/lunar_rover.launch.py new file mode 100644 index 00000000..25a2196b --- /dev/null +++ b/lunar_rover/launch/lunar_rover.launch.py @@ -0,0 +1,73 @@ +from http.server import executable +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, RegisterEventHandler, IncludeLaunchDescription, SetEnvironmentVariable +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_rover_demos_path = get_package_share_directory('lunar_rover') + + 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_rover_demos_path])} + + lunar_world_model = os.path.join(lunar_rover_demos_path, 'worlds', 'moon.world') + + start_world = ExecuteProcess( + cmd=['ign gazebo', lunar_world_model, '-r'], + output='screen', + additional_env=env, + shell=True + ) + + # Gazebo Bridge + ros_gz_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + arguments=[ + '/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock', + '/model/lunar_roving_vehicle/odometry@nav_msgs/msg/Odometry@ignition.msgs.Odometry', + '/cmd_vel@geometry_msgs/msg/Twist]ignition.msgs.Twist', + 'imu@sensor_msgs/msg/Imu[ignition.msgs.IMU', + ], + output='screen') + + # Image Bridge for handheld camera + img_bridge_handheld = Node( + package='ros_gz_image', + executable='image_bridge', + arguments=['/img_raw_handheld', '/img_raw_handheld'], + output='screen') + + # Image Bridge for front camera + img_bridge_front = Node( + package='ros_gz_image', + executable='image_bridge', + arguments=['/img_raw_front', '/img_raw_front'], + output='screen') + + + return LaunchDescription([ + SetParameter(name='use_sim_time', value=True), + start_world, + ros_gz_bridge, + img_bridge_handheld, + img_bridge_front, + ]) diff --git a/lunar_rover/lunar_rover/__init__.py b/lunar_rover/lunar_rover/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/lunar_rover/lunar_rover/waypoints.py b/lunar_rover/lunar_rover/waypoints.py new file mode 100755 index 00000000..f2c92e93 --- /dev/null +++ b/lunar_rover/lunar_rover/waypoints.py @@ -0,0 +1,106 @@ +#!/usr/bin/env python3 +# Python demo of Lunar Roving Vehicle Waypoint Following +# Jasper Grant +# August 26th, 2024 + +from math import sqrt, atan2, pi +# ROS2 Python API import +import rclpy +from rclpy.node import Node +# Messages used for diff drive control import +from geometry_msgs.msg import Twist +from nav_msgs.msg import Odometry + +# Constants for proportional control +KP = [0.2, 0.5] +LINEAR_MINIMUM_SPEED = 0.3 +WAYPOINT_DISTANCE_FOR_EQUALITY = 0.1 + +# Waypoints to follow +WAYPOINTS = [(10, 0), (10, 10), (0, 10), (0, 0)] +# In a practical system, waypoints would be read from a file or other source + +class WaypointFollower(Node): + + # init class + def __init__(self): + # Super init Node class + super().__init__('waypoint_follower') + # Create publisher to control robot from cmd_vel + self.vel_pub = self.create_publisher(Twist, '/cmd_vel',10) + # Create subscriber to observe robot odom + self.odom_sub = self.create_subscription( + Odometry, + '/model/lunar_roving_vehicle/odometry', + self.odom_callback, + 10 + ) + # Start odom as empty odom object + self.current_odom = Odometry() + self.waypoints = WAYPOINTS + self.current_waypoint_index = 0 + + # Update model's odom belief from recieved message + def odom_callback(self, msg): + self.current_odom = msg + #self.get_logger().info(f'Currrent Position: {self.current_odom.pose.pose.position.x}, {self.current_odom.pose.pose.position.y}') + self.navigate_to_waypoint() + + # Calculate distance and angle to the next waypoint + def calculate_distance_and_angle(self, waypoint): + self.get_logger().info(f'Waypoint: {waypoint}') + x = self.current_odom.pose.pose.position.x + y = self.current_odom.pose.pose.position.y + self.get_logger().info(f'Current Position: {x}, {y}') + dx = waypoint[0] - x + dy = waypoint[1] - y + #self.get_logger().info(f'dx: {dx}, dy: {dy}') + distance = sqrt(dx**2 + dy**2) + angle = atan2(dy, dx) + # Convert angle to be relative to the robot + # Get the robot's current orientation from quaternion + angle = angle - 2 * atan2(self.current_odom.pose.pose.orientation.z, self.current_odom.pose.pose.orientation.w) + # Normalize angle to be between -pi and pi + angle = (angle + pi) % (2 * pi) - pi + self.get_logger().info(f'distance: {distance}, angle: {angle}') + return distance, angle + + def navigate_to_waypoint(self): + if self.current_waypoint_index >= len(self.waypoints): + self.get_logger().info('All waypoints reached') + return + + + waypoint = self.waypoints[self.current_waypoint_index] + distance, angle = self.calculate_distance_and_angle(waypoint) + + if distance < WAYPOINT_DISTANCE_FOR_EQUALITY: + self.get_logger().info(f'Waypoint {self.current_waypoint_index} reached.') + self.current_waypoint_index += 1 + return + + msg = Twist() + # Keep the robot moving forward at a minimum speed + msg.linear.x = max([KP[0] * distance, LINEAR_MINIMUM_SPEED]) + + msg.angular.z = KP[1] * angle + self.vel_pub.publish(msg) + + + + +def main(args=None): + # Initialize ROS2 node + rclpy.init(args=args) + # Create WaypointFollower object + waypoint_follower = WaypointFollower() + # Spin ROS2 node + rclpy.spin(waypoint_follower) + # Destroy node on shutdown + waypoint_follower.destroy_node() + # Shutdown ROS2 + rclpy.shutdown() + +if __name__ == '__main__': + main() + diff --git a/lunar_rover/package.xml b/lunar_rover/package.xml new file mode 100644 index 00000000..a9bbf335 --- /dev/null +++ b/lunar_rover/package.xml @@ -0,0 +1,42 @@ + + + + lunar_rover + 0.0.1 + A Lunar Rover Demo for SpaceROS + JasperGrant + 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_rover/worlds/moon.world b/lunar_rover/worlds/moon.world new file mode 100755 index 00000000..024797be --- /dev/null +++ b/lunar_rover/worlds/moon.world @@ -0,0 +1,82 @@ + + + + + 0.01 + 1.0 + + + + + + + + + ogre2 + 0 0 0 + + + + + + + + 3D View + false + docked + + ogre2 + scene + 0 0 0 + 0.001 0.001 0.001 1 + -6 0 7 0 0.5 0 + + + + + floating + + + + + + true + 0 0 11 0 0 0 + 7.0 + 0.5 0.5 0.5 1 + 0.2 0.2 0.2 1 + + 1000 + 1.0 + 0.0 + 0.0 + + -0.5 0.0 -1.0 + + + + model://lunar_roving_vehicle + lunar_roving_vehicle + 0 0 2.5 0 0 0 + + + + model://apollo15_landing_site_1000x1000_modified + lunar_landing_site + 0 0 -44 0 0 0 + + + + 0 0 -1.62 + + + + \ No newline at end of file