Skip to content

NASA Challenge_[@JasperGrant]_[Apollo 15 Lunar Roving Vehicle Simulation] #62

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
JasperGrant wants to merge 13 commits into
base: main
Choose a base branch
from
Open

NASA Challenge_[@JasperGrant]_[Apollo 15 Lunar Roving Vehicle Simulation] #62

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
13 commits
Select commit Hold shift + click to select a range
25988f8
Initial Mars Rover copy
JasperGrant Aug 22, 2024
b411d69
Add initial basic lunar simulation
JasperGrant Aug 22, 2024
2836ed2
Add controllable rover in basic lunar simulation
JasperGrant Aug 25, 2024
b2205bc
Add simple ROS bridge and simplified GUI based on Mars Rover
JasperGrant Aug 25, 2024
b251da3
Add apollo lunar module to moon.world
JasperGrant Aug 26, 2024
8c13259
Update to modified Apollo 15 landing site in simulations
JasperGrant Aug 27, 2024
48c715f
Shift models up above grid and increase intensity of light
JasperGrant Aug 28, 2024
d7f4bf6
Update real time factor
JasperGrant Sep 2, 2024
320c217
Add image display and image bridges for both cameras
JasperGrant Sep 3, 2024
de180f3
Add simple waypoint follower controller
JasperGrant Sep 3, 2024
7049556
Add IMU
JasperGrant Sep 3, 2024
692c763
Merge branch 'space-ros:main' into summer_sprint
JasperGrant Sep 10, 2024
72c8678
Add readme
JasperGrant Sep 10, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
48 changes: 48 additions & 0 deletions lunar_rover/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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(<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
launch
worlds
DESTINATION share/${PROJECT_NAME}
)

ament_python_install_package(${PROJECT_NAME})

install(PROGRAMS
lunar_rover/waypoints.py
DESTINATION lib/${PROJECT_NAME}
)


ament_package()
40 changes: 40 additions & 0 deletions lunar_rover/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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.

<img src="img/LRV_front_view.png" height="400">
<img src="img/LRV_side_view.png" height="400">


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 |
Binary file added lunar_rover/img/LRV_front_view.png
View file
Open in desktop
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added lunar_rover/img/LRV_side_view.png
View file
Open in desktop
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
73 changes: 73 additions & 0 deletions lunar_rover/launch/lunar_rover.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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,
])
Empty file added lunar_rover/lunar_rover/__init__.py
View file
Open in desktop
Empty file.
106 changes: 106 additions & 0 deletions lunar_rover/lunar_rover/waypoints.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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()

42 changes: 42 additions & 0 deletions lunar_rover/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,42 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>lunar_rover</name>
<version>0.0.1</version>
<description>A Lunar Rover Demo for SpaceROS</description>
<maintainer email="jasper.grant@dal.ca">JasperGrant</maintainer>
<license>Apache-2.0</license>

<buildtool_depend>ament_cmake</buildtool_depend>
<buildtool_depend>ament_cmake_python</buildtool_depend>

<depend>rclcpp</depend>
<depend>rclpy</depend>
<depend>simulation</depend>

<exec_depend>ament_index_python</exec_depend>
<exec_depend>control_msgs</exec_depend>
<exec_depend>diff_drive_controller</exec_depend>
<exec_depend>effort_controllers</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>hardware_interface</exec_depend>
<exec_depend>ign_ros2_control</exec_depend>
<exec_depend>imu_sensor_broadcaster</exec_depend>
<exec_depend>joint_state_broadcaster</exec_depend>
<exec_depend>joint_trajectory_controller</exec_depend>
<exec_depend>launch</exec_depend>
<exec_depend>launch_ros</exec_depend>
<exec_depend>robot_state_publisher</exec_depend>
<exec_depend>ros_ign_gazebo</exec_depend>
<exec_depend>ros2controlcli</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>velocity_controllers</exec_depend>
<exec_depend>xacro</exec_depend>

<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>

<export>
<build_type>ament_cmake</build_type>
</export>
</package>
82 changes: 82 additions & 0 deletions lunar_rover/worlds/moon.world
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,82 @@
<?xml version="1.0" ?>
<sdf version="1.8">
<world name="moon">
<physics name="1ms" type="ignored">
<max_step_size>0.01</max_step_size>
<real_time_factor>1.0</real_time_factor>
</physics>
<plugin
filename="gz-sim-physics-system"
name="gz::sim::systems::Physics">
</plugin>
<plugin
filename="gz-sim-user-commands-system"
name="gz::sim::systems::UserCommands">
</plugin>
<plugin
filename="gz-sim-scene-broadcaster-system"
name="gz::sim::systems::SceneBroadcaster">
</plugin>
<plugin filename="ignition-gazebo-sensors-system" name="ignition::gazebo::systems::Sensors">
<render_engine>ogre2</render_engine>
<background_color>0 0 0</background_color>
</plugin>
<plugin
filename="libignition-gazebo-imu-system.so"
name="ignition::gazebo::systems::Imu">
</plugin>

<gui fullscreen="0">
<plugin filename="GzScene3D" name="3D View">
<ignition-gui>
<title>3D View</title>
<property type="bool" key="showTitleBar">false</property>
<property type="string" key="state">docked</property>
</ignition-gui>
<engine>ogre2</engine>
<scene>scene</scene>
<background_color>0 0 0</background_color>
<ambient>0.001 0.001 0.001 1</ambient> <!-- Reduced ambient light level -->
<camera_pose>-6 0 7 0 0.5 0</camera_pose>

</plugin>
<plugin filename="ImageDisplay" name="Image Display">
<ignition-gui>
<property key="state" type="string">floating</property>
</ignition-gui>
</plugin>
</gui>

<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>
<pose>0 0 11 0 0 0</pose>
<intensity>7.0</intensity>
<diffuse>0.5 0.5 0.5 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<constant>1.0</constant>
<linear>0.0</linear>
<quadratic>0.0</quadratic>
</attenuation>
<direction>-0.5 0.0 -1.0</direction>
</light>

<include>
<uri>model://lunar_roving_vehicle</uri>
<name>lunar_roving_vehicle</name>
<pose>0 0 2.5 0 0 0</pose>
</include>

<include>
<uri>model://apollo15_landing_site_1000x1000_modified</uri>
<name>lunar_landing_site</name>
<pose>0 0 -44 0 0 0</pose>
</include>

<!-- Lunar Gravity-->
<gravity>0 0 -1.62</gravity>


</world>
</sdf>