diff --git a/mars_rover/CMakeLists.txt b/mars_rover/CMakeLists.txt index c2c90051..cf80b88b 100644 --- a/mars_rover/CMakeLists.txt +++ b/mars_rover/CMakeLists.txt @@ -44,7 +44,8 @@ install(PROGRAMS nodes/move_arm nodes/move_mast nodes/move_wheel - nodes/run_demo + nodes/run_demo # To run the Gazebo rover demo + nodes/teleop_rover # To run O3DE rover demo nodes/odom_tf_publisher DESTINATION lib/${PROJECT_NAME} ) diff --git a/mars_rover/README.md b/mars_rover/README.md new file mode 100644 index 00000000..2204fc68 --- /dev/null +++ b/mars_rover/README.md @@ -0,0 +1,14 @@ +# Curiosity Mars Rover ROS2 package + +This packge launches the curiositry mars rover simulation in Gazebo ignition. It is part of the Space ROS demonstration examples. + +**TODO** + + * [ ] Description of fixing the .dae file for the ```chassis``` + * [ ] How I configured it to work with the O3DE project. + * [ ] How to instructions + +**Changelogs** + + * Changed RunDemo class name to TeleOpRover as this class's primary function is to publish Twist message based on user/teleop input. + * odom_tf_broadcaster may need to be turned off??? \ No newline at end of file diff --git a/mars_rover/launch/mars_rover.launch.py b/mars_rover/launch/mars_rover.launch.py index 91e3c99f..6f71b60c 100644 --- a/mars_rover/launch/mars_rover.launch.py +++ b/mars_rover/launch/mars_rover.launch.py @@ -1,85 +1,87 @@ -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 +""" +Bring up nodes, hardware interfaces and spaw curiosity rover in a Gazebo world. -from ament_index_python.packages import get_package_share_directory +Author: +Space ROS -import xacro +Date: TODO +Version: N/A +A.I Tool: ChatGPT 4.o +""" -# . ../spaceros_ws/install/setup.bash && . ../depends_ws/install/setup.bash -# rm -rf build install log && colcon build && . install/setup.bash +# Imports +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import ExecuteProcess, RegisterEventHandler +from launch_ros.actions import Node, SetParameter +from launch.event_handlers import OnProcessExit +import os +from os import environ +import xacro def generate_launch_description(): - + """Generate launch description to bringup curiosity rover simulation.""" + # Define paths to mars_rover and simulation packages mars_rover_demos_path = get_package_share_directory('mars_rover') mars_rover_models_path = get_package_share_directory('simulation') - + # Define 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()} - + # Arm arm_node = Node( package="mars_rover", executable="move_arm", output='screen' ) - + # Sensor mast mast_node = Node( package="mars_rover", executable="move_mast", output='screen' ) - + # Wheel groups wheel_node = Node( package="mars_rover", executable="move_wheel", output='screen' ) - + # Start demo run_node = Node( package="mars_rover", executable="run_demo", output='screen' ) - + # Publish odometry odom_node = Node( package="mars_rover", executable="odom_tf_publisher", output='screen' ) - + # Fire up Gazebo ignition model start_world = ExecuteProcess( cmd=['ign gazebo', mars_world_model, '-r'], output='screen', additional_env=env, shell=True ) - + # Publish various joint positions and link orientation robot_state_publisher = Node( package='robot_state_publisher', executable='robot_state_publisher', name='robot_state_publisher', output='screen', parameters=[robot_description]) - + # Bridge communication between ROS 2 and the Ignition Gazebo simulation ros_gz_bridge = Node( package='ros_gz_bridge', executable='parameter_bridge', @@ -89,13 +91,13 @@ def generate_launch_description(): '/scan@sensor_msgs/msg/LaserScan@ignition.msgs.LaserScan', ], output='screen') - + # Connect image messages from Gazebo to ROS2 image_bridge = Node( package='ros_gz_image', executable='image_bridge', arguments=['/image_raw', '/image_raw'], output='screen') - + # Spawn curiosity rover into the world. spawn = Node( package='ros_ign_gazebo', executable='create', arguments=[ @@ -104,52 +106,49 @@ def generate_launch_description(): '-z', '-7.5' ], output='screen' - ) - - ## Control Components - + # Control Components component_state_msg = '{name: "IgnitionSystem", target_state: {id: 3, label: ""}}' - - ## a hack to resolve current bug + # TODO what bug was resolved with this hack? + # Set hardware_interface state to active set_hardware_interface_active = ExecuteProcess( cmd=['ros2', 'service', 'call', 'controller_manager/set_hardware_component_state', 'controller_manager_msgs/srv/SetHardwareComponentState', component_state_msg] ) - + # Controller to broadcast all joint states load_joint_state_broadcaster = ExecuteProcess( cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'joint_state_broadcaster'], output='screen' ) - + # Controller for arm load_arm_joint_traj_controller = ExecuteProcess( cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'arm_joint_trajectory_controller'], output='screen' ) - + # Controller for sensor mast load_mast_joint_traj_controller = ExecuteProcess( cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'mast_joint_trajectory_controller'], output='screen' ) - + # Controller for wheel load_wheel_joint_traj_controller = ExecuteProcess( cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'wheel_velocity_controller'], output='screen' ) - + # Controller for steering load_steer_joint_traj_controller = ExecuteProcess( cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'steer_position_controller'], output='screen' ) - + # Controller for suspension joints load_suspension_joint_traj_controller = ExecuteProcess( cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'wheel_tree_position_controller'], @@ -175,12 +174,16 @@ def generate_launch_description(): on_exit=[set_hardware_interface_active], ) ), + # After the hardware interface is activated, + # the joint state broadcaster is loaded. RegisterEventHandler( OnProcessExit( target_action=set_hardware_interface_active, on_exit=[load_joint_state_broadcaster], ) ), + # After the joint state broadcaster is loaded, + # all trajectory controllers (arm, mast, wheel, steer, suspension) are loaded and activated. RegisterEventHandler( OnProcessExit( target_action=load_joint_state_broadcaster, diff --git a/mars_rover/launch/mars_rover_o3de.launch.py b/mars_rover/launch/mars_rover_o3de.launch.py new file mode 100644 index 00000000..36201422 --- /dev/null +++ b/mars_rover/launch/mars_rover_o3de.launch.py @@ -0,0 +1,212 @@ +""" +Basic control node for the Curiosity rover in O3DE. + +Author: +Azmyin Md. Kamal, +Ph.D. student in MIE, +Louisiana State University, +Louisiana, USA + +Date: August 29th, 2024 +Version: 1.0 + +AI: ChatGPT 4.o + +""" + +# Imports +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import ExecuteProcess, RegisterEventHandler +from launch_ros.actions import Node, SetParameter +from launch.event_handlers import OnProcessExit +import os +from os import environ +import xacro + +def generate_launch_description(): + """Generate launch description to bringup curiosity rover simulation.""" + # Define paths to mars_rover and simulation packages + # mars_rover_demos_path = get_package_share_directory('mars_rover') + # mars_rover_models_path = get_package_share_directory('simulation') + # # Define + # 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()} + + # Arm + # arm_node = Node( + # package="mars_rover", + # executable="move_arm", + # output='screen' + # ) + # Sensor mast + # mast_node = Node( + # package="mars_rover", + # executable="move_mast", + # output='screen' + # ) + # Wheel groups + # wheel_node = Node( + # package="mars_rover", + # executable="move_wheel", + # output='screen' + # ) + # Node to take teleop + teleop_rover_node = Node( + package="mars_rover", + executable="teleop_rover", + output='screen' + ) + # Publish odometry + # odom_node = Node( + # package="mars_rover", + # executable="odom_tf_publisher", + # output='screen' + # ) + + # Fire up Gazebo Ignition + # start_world = ExecuteProcess( + # cmd=['ign gazebo', mars_world_model, '-r'], + # output='screen', + # additional_env=env, + # shell=True + # ) + # # Publish various joint positions and link orientation + # # This is crucial for simulation + # robot_state_publisher = Node( + # package='robot_state_publisher', + # executable='robot_state_publisher', + # name='robot_state_publisher', + # output='screen', + # parameters=[robot_description]) + # # Bridge communication between ROS 2 and the Ignition Gazebo simulation + # 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') + # # Connect image messages from Gazebo to ROS2 + # image_bridge = Node( + # package='ros_gz_image', + # executable='image_bridge', + # arguments=['/image_raw', '/image_raw'], + # output='screen') + # # Spawn curiosity rover into the world. + # 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: ""}}' + + # # TODO what bug was resolved with this hack? + # # Set hardware_interface state to active + # set_hardware_interface_active = ExecuteProcess( + # cmd=['ros2', 'service', 'call', + # 'controller_manager/set_hardware_component_state', + # 'controller_manager_msgs/srv/SetHardwareComponentState', + # component_state_msg] + # ) + + # # Controller to broadcast all joint states + # load_joint_state_broadcaster = ExecuteProcess( + # cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + # 'joint_state_broadcaster'], + # output='screen' + # ) + + # # Controller for arm + # load_arm_joint_traj_controller = ExecuteProcess( + # cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + # 'arm_joint_trajectory_controller'], + # output='screen' + # ) + + # # Controller for sensor mast + # load_mast_joint_traj_controller = ExecuteProcess( + # cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + # 'mast_joint_trajectory_controller'], + # output='screen' + # ) + + # # Controller for wheel + # load_wheel_joint_traj_controller = ExecuteProcess( + # cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + # 'wheel_velocity_controller'], + # output='screen' + # ) + # # Controller for steering + # load_steer_joint_traj_controller = ExecuteProcess( + # cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + # 'steer_position_controller'], + # output='screen' + # ) + # # Controller for suspension joints + # 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, + teleop_rover_node, # Renamed from run_node + # odom_node, + #ros_gz_bridge, + #image_bridge, + + # RegisterEventHandler( + # OnProcessExit( + # target_action=spawn, + # on_exit=[set_hardware_interface_active], + # ) + # ), + # # After the hardware interface is activated, + # # the joint state broadcaster is loaded. + # RegisterEventHandler( + # OnProcessExit( + # target_action=set_hardware_interface_active, + # on_exit=[load_joint_state_broadcaster], + # ) + # ), + # # After the joint state broadcaster is loaded, + # # all trajectory controllers (arm, mast, wheel, steer, suspension) are loaded and activated. + # 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], + # ) + # ), + ]) diff --git a/mars_rover/nodes/move_arm b/mars_rover/nodes/move_arm old mode 100644 new mode 100755 diff --git a/mars_rover/nodes/move_mast b/mars_rover/nodes/move_mast old mode 100644 new mode 100755 diff --git a/mars_rover/nodes/move_wheel b/mars_rover/nodes/move_wheel old mode 100644 new mode 100755 index c016ad00..83aab4dd --- a/mars_rover/nodes/move_wheel +++ b/mars_rover/nodes/move_wheel @@ -1,32 +1,54 @@ #!/usr/bin/env python3 -from pickle import FALSE +""" +Implementation of the MoveWheel class. + +Primary author: +Space-ROS + +Docstrings and comments added by: +Azmyin Md. Kamal, +Ph.D. student in MIE, iCORE Lab, +Louisiana State University, Louisiana, USA + +Date: August 29th, 2024 +Version: 1.0 +AI: ChatGPT 4.o + +""" + +# Imports +# from pickle import FALSE import rclpy from rclpy.node import Node from builtin_interfaces.msg import Duration - -from std_msgs.msg import String, Float64MultiArray +from std_msgs.msg import Float64MultiArray from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint from geometry_msgs.msg import Twist import math class MoveWheel(Node): + """Node to control the wheel velocity, steering, and suspension.""" def __init__(self): super().__init__('wheel_node') + # Setup publishers 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) self.suspension_publisher_ = self.create_publisher(Float64MultiArray, '/wheel_tree_position_controller/commands',10) - timer_period = 0.1 # seconds + timer_period = 0.1 # seconds, 100ms to update once? self.timer = self.create_timer(timer_period, self.timer_callback) - - - self.vel_sub = self.create_subscription(Twist, '/cmd_vel', self.vel_callback, 10) + # Setup subscribers + # Not used anywhere in this file, + # TODO depricate and delete this subscriberm it is not used anywhere + # self.vel_sub = self.create_subscription(Twist, '/cmd_vel', self.vel_callback, 10) + # Important instance variables self.curr_vel = Twist() self.last_vel = Twist() self.should_steer = False def vel_callback(self, msg): + """Handle incoming velocity messages and updates steering state.""" if abs(self.last_vel.angular.z - self.curr_vel.angular.z) > 0.01 and self.should_steer is False: self.last_vel = Twist() self.last_vel.linear.x = self.curr_vel.linear.x @@ -36,95 +58,74 @@ class MoveWheel(Node): self.curr_vel = msg def set_wheel_common_speed(self, vel): - + """Set the common speed for all wheels and publishes the command.""" target_vel = Float64MultiArray() target_vel.data = [vel, vel*1.5, vel, -vel, -vel*1.5, -vel] - # self.get_logger().info(f'Publishing: "{target_vel.data}"') self.wheel_publisher_.publish(target_vel) - def map_angular_to_steering(self, angular_speed) -> float: + """Map angular speed to a corresponding steering angle.""" if abs(angular_speed) < 1e-3: return 0.0 - - #max 0.6 min -0.6 - angular_speed = min(0.6, max(angular_speed, -0.6)) + angular_speed = min(0.6, max(angular_speed, -0.6)) #max 0.6 min -0.6 return (angular_speed/abs(angular_speed))*(-25 * abs(angular_speed) + 17.5) - def set_steering(self, turn_rad): - #Ideal Ackerman Steering + """Set the steering angles for the wheels using Ideal Ackerman Steering model.""" target_steer = JointTrajectory() # target_steer.header.stamp = self.get_clock().now().to_msg() target_steer.joint_names = ["suspension_steer_F_L_joint", "suspension_steer_B_L_joint", "suspension_steer_F_R_joint", "suspension_steer_B_R_joint"] steer_point = JointTrajectoryPoint() - if abs(turn_rad) < 1e-3: steer_point.positions = [0.0, 0.0, 0.0, 0.0] else: - R = abs(turn_rad) #Turning radius - + R = abs(turn_rad) #Turning radius L = 2.08157 #Chassis Length T = 1.53774 #Chassis Width - alpha_i = math.atan(L/(R - (T/2))) alpha_o = math.atan(L/(R + (T/2))) - if alpha_i > 0.6: alpha_i = 0.6 - if alpha_o > 0.6: alpha_o = 0.6 - alpha_i = round(alpha_i, 2) alpha_o = round(alpha_o, 2) - if turn_rad > 0.0: steer_point.positions = [alpha_i, -alpha_i, alpha_o, -alpha_o] else: steer_point.positions = [-alpha_o, alpha_o, -alpha_i, alpha_i] - - steer_point.time_from_start = Duration(sec=1) - target_steer.points.append(steer_point) - self.steer_publisher_.publish(target_steer) - def set_suspension(self, sus_val=None): - + """Set and publish the suspension values for the rover.""" target_val = Float64MultiArray() if sus_val is None: - target_val.data = [0.3,-0.1,0.3,-0.1] + target_val.data = [0.3,-0.1,0.3,-0.1] # TODO where are these values coming from? self.suspension_publisher_.publish(target_val) - def timer_callback(self): + """Periodic callback to update wheel speed, suspension, and steering.""" self.set_wheel_common_speed(self.curr_vel.linear.x) self.set_suspension() - if self.should_steer: steering_val = self.map_angular_to_steering(self.curr_vel.angular.z) self.set_steering(steering_val) self.should_steer = False - def main(args=None): + """Run main.""" 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() \ No newline at end of file diff --git a/mars_rover/nodes/odom_tf_publisher b/mars_rover/nodes/odom_tf_publisher old mode 100644 new mode 100755 index ca5c95f1..7e452a4a --- a/mars_rover/nodes/odom_tf_publisher +++ b/mars_rover/nodes/odom_tf_publisher @@ -1,16 +1,33 @@ #!/usr/bin/env python3 -# adapted from ROS 2 tutorial -# https://docs.ros.org/en/rolling/Tutorials/Intermediate/Tf2/Writing-A-Tf2-Broadcaster-Py.html +""" +Implementation of the OdomTFBroadcaster class. -import rclpy +Adapted from ROS 2 tutorial https://docs.ros.org/en/rolling/Tutorials/Intermediate/Tf2/Writing-A-Tf2-Broadcaster-Py.html + +Primary author: +Space-ROS + +Docstrings and comments added by: +Azmyin Md. Kamal, +Ph.D. student in MIE, iCORE Lab, +Louisiana State University, Louisiana, USA + +Date: August 29th, 2024 +Version: 1.0 +AI: ChatGPT 4.o + +""" +#imports +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): + """Publish the coordinate transforms between different frames of the rover.""" def __init__(self): super().__init__('odom_tf_publisher') @@ -18,40 +35,37 @@ class OdomTFBroadcaster(Node): # Initialize the transform broadcaster self.tf_broadcaster = TransformBroadcaster(self) + # Setup subscribers + # Gazebo Ignition publishes [odometry data](https://gazebosim.org/api/sim/8/classgz_1_1sim_1_1systems_1_1OdometryPublisher.html#details) + # ! Name changed from /model/curiosity_mars_rover/odometry to ??? # Subscribe to Odometry topic - # TODO: make the topic name a parameter instead of hard-coded - self.subscription = self.create_subscription( - Odometry, - f'/model/curiosity_mars_rover/odometry', - self.handle_odometry, - 1) + sim_odom_topic_name = "/model/curiosity_mars_rover/odometry" # TODO: make the topic name a parameter instead of hardcoded + self.subscription = self.create_subscription(Odometry,sim_odom_topic_name, + self.handle_odometry,1) self.subscription # prevent unused variable warning def handle_odometry(self, msg): + """Process odometry message from simulated world.""" tf = TransformStamped() - # Read message content and assign it to - # corresponding tf variables + # 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 + # 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 + # 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(): + """Run main.""" rclpy.init() node = OdomTFBroadcaster() node.get_logger().info('Starting odometry_tf_publisher node') diff --git a/mars_rover/nodes/teleop_rover b/mars_rover/nodes/teleop_rover new file mode 100755 index 00000000..b1d25531 --- /dev/null +++ b/mars_rover/nodes/teleop_rover @@ -0,0 +1,115 @@ +#!/usr/bin/env python3 + +""" +Implementation of the TeleOpRover class. + +This node acts as an interface between user sent service call to publishing +twist message that is then subscribed too by the controller + +Primary author: +Space-ROS + +Docstrings and comments added by: +Azmyin Md. Kamal, +Ph.D. student in MIE, iCORE Lab, +Louisiana State University, Louisiana, USA + +Date: August 29th, 2024 +Version: 1.0 +AI: ChatGPT 4.o + +""" + +# Imports +import rclpy +from rclpy.node import Node + +# from std_msgs.msg import String, Float64MultiArray +from geometry_msgs.msg import Twist + +# import math +# from random import randint +from std_srvs.srv import Empty + +class TeleOpRover(Node): + """Node that converts user's service request / input device commands to Twist messages.""" + + def __init__(self) -> None: + super().__init__('teleop_node') + # Setup publishers + self.motion_publisher_ = self.create_publisher(Twist, '/cmd_vel', 10) + self.o3de_motion_publisher_ = self.create_publisher(Twist, '/rover/cmd_vel', 10) + # Setup services + 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.stopped = True + timer_period = 0.1 + self.timer = self.create_timer(timer_period, self.timer_callback) + self.curr_action = Twist() + + def timer_callback(self): + """Periodically publish velocity of rover in 3D space as Twist messages.""" + if (not self.stopped): + # Experimental + self.o3de_motion_publisher_.publish(self.curr_action) + # self.motion_publisher_.publish(self.curr_action) + + + def move_forward_callback(self, request, response): + """Print message when moving forward.""" + self.get_logger().info("Moving forward") + action = Twist() + # action.linear.x = 2.0 + action.linear.x = 1.0 # Not sure but negative value makes it move forward + self.curr_action = action + self.stopped = False + return response + + def move_stop_callback(self, request, response): + """Print message when stopped.""" + # 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) + self.o3de_motion_publisher_.publish(self.curr_action) + return response + + def turn_left_callback(self, request, response): + """Print message when turning left.""" + 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): + """Print message when turning right.""" + self.get_logger().info("Turning right") + self.stopped = False + action = Twist() + #action.linear.x = 1.0 + action.linear.x = 0.5 + action.angular.z = -1.5 + self.curr_action = action + self.stopped = False + return response + +def main(args=None): + """Run main.""" + rclpy.init(args=args) + teleop_node = TeleOpRover() + rclpy.spin(teleop_node) + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + teleop_node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/mars_rover/pyproject.toml b/mars_rover/pyproject.toml new file mode 100644 index 00000000..89a386f7 --- /dev/null +++ b/mars_rover/pyproject.toml @@ -0,0 +1,29 @@ +[tool.flake8] +max-line-length = 120 +show-source = true + +[tool.pylint.'MAIN'] +persistent = "no" +[tool.pylint.'VARIABLES'] +ignored-argument-names = "args|kwargs" + +[tool.pydocstyle] +inherit = false +ignore = "D105,D107,D203,D212,D413,D407" + +[tool.ruff] +line-length = 120 +target-version = "py38" +fixable = [] +unfixable = ["ALL"] +select = ["F", "E", "W", "N", "D", "UP", "BLE", "B", "A", "COM", "EM", "PTH", "C4", "ISC"] +ignore = ["D105", "D107", "D203", "D212", "D413", "D407", "COM812"] +# select = [ +# "A", # prevent using keywords that clobber python builtins +# "B", # bugbear: security warnings +# "E", # pycodestyle +# "F", # pyflakes +# "ISC", # implicit string concatenation +# "UP", # alert you when better syntax is available in your python version +# "RUF", # the ruff developer's own rules +# ] \ No newline at end of file diff --git a/o3de_mars_rover/README.md b/o3de_mars_rover/README.md new file mode 100644 index 00000000..041fef41 --- /dev/null +++ b/o3de_mars_rover/README.md @@ -0,0 +1,75 @@ +# o3de_mars_rover + +This directory contains the Dockerfile instructions for creating the ```TODO``` image that showcases Curiosity rover navigating through a clutterd test environment inspired by JPL's Mars Yard testing ground. + +Author: Azmyin Md. Kamal +Version: 1.0 +Date: 09/09/2024 +Landing page: **TODO** after submission. This page will always have the most updated information. + +Version 1: This project was created as a submission to the NASA Space ROS Sim Summer Sprint Challenge 2024. A short review on O3DE, a small video showing the system in action and other related discussion can be found in this [PR](https://github.com/space-ros/demos/pull/64). In this submission only forward movement is supported. + +## Prerequisits + +* ```osrf/space-ros```. I will eventually upgrade the Dockerfile to use the ```openrobotics/moveit2::latest``` from [space-ros/docker/movit2](https://github.com/space-ros/docker/tree/main/moveit2) image + +## Build instruction + +**WARNING** At least have 60 GB of free disk space and since it builds Moveit2 and Demo_ws before building the whole O3DE project, expect at least 3 - 4 hours for the build process to complete. + +* Git clone the top level ```demos``` repository and ```change directory``` into it. If you are just interested in the image, you don't need to clone this repo. Then from a terminal + +```bash +cd /o3de_mars_rover +./build.sh +``` + +* Install Nvidia Container toolkit and run configuration as discussed here: https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/latest/install-guide.html. If you don't have it open a terminal and do the following + +* Make sure you have --runtime nvidia configured. If not this sample example will do it: ```sudo docker run --rm --runtime=nvidia --gpus all ubuntu nvidia-smi``` + +## Run the demo + +* Use ```docker ps -a``` to check if you have a container running ```openrobotics/o3de_curiosity_docker``` running. A useful command that stops and removes all containers is ```docker rm $(docker ps -aq)``` + +We will need three terminals. Lets call them Termianls A, B and C respectively. + +* Terminal A, run the following + +```bash +xhost +local:docker +./run.sh +``` + +* Terminal B, find container id and then attach it + +```bash +docker ps -a +docker exec -it /bin/bash +``` + +* Terminal C, attach it to container + +```bash +docker exec -it /bin/bash +``` + +You should see the following setup + +description + +* In Terminal A, launch the O3DE app: ```./build/linux/bin/profile/Editor``` + +* Hit the play button (right arrow symbol top right) + +* **IMOPRTANT** in Game mode, the mouse dissappears. To get it back , hit ```Ctrl+TAB``` keys. + +* In Terminal B and C, first source DEMO_WS: ```source $DEMO_DIR/install/setup.bash``` +* In Terminal B: ```ros2 launch mars_rover mars_rover_o3de.launch``` +* In Terminal C: ```ros2 service call /move_forward std_srvs/srv/Empty``` + +If you did eveything correclty, you should see the following + +description + +Now the rover will move forward. diff --git a/o3de_mars_rover/bcks/Dockerfile_build_from_space_robot b/o3de_mars_rover/bcks/Dockerfile_build_from_space_robot new file mode 100644 index 00000000..5bf9f95b --- /dev/null +++ b/o3de_mars_rover/bcks/Dockerfile_build_from_space_robot @@ -0,0 +1,248 @@ +# Copyright 2024 Azmyin Md. Kamal +# +# 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 o3de_curiosit_docker image that showcases Open 3D Engine +# in simulating a Curiosity rover traversing through a test ground inspired by the NASA JPL Mars Yard +# O3DE docker file based on https://github.com/husarion/o3de-docker/blob/main/Dockerfile +# Requirements: openrobotics/space_robots_demo 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/space_robots_demo + +# Define arguments used in the metadata definition +ARG VCS_REF +ARG VERSION="preview" +ARG ROS_DISTRO=humble + +# Specify the docker image metadata +LABEL org.label-schema.schema-version="1.0" +LABEL org.label-schema.name="O3DE Curiosity Rover" +LABEL org.label-schema.description="O3DE Curiosity rover demo image" +LABEL org.label-schema.vendor="Nasa Space ROS Sim Summer Spring Challenge 2024" +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/Mechazo11/space-ros-docker" +LABEL org.label-schema.vcs-ref=${VCS_REF} + +# Define a key enviornment variables +ENV MOVEIT2_DIR=${HOME_DIR}/moveit2 +ENV DEMO_DIR=${HOME_DIR}/demos_ws +ENV IGNITION_VERSION fortress +ENV GZ_VERSION fortress +ENV PROJECT_NAME=RobotSim +ENV O3DE_ENGINE=${HOME_DIR}/o3de +ENV O3DE_DIR=${HOME_DIR}/O3DE +ENV O3DE_EXTRAS=${HOME_DIR}/o3de-extras +ENV PROJECT_PATH=${O3DE_DIR}/Projects/${PROJECT_NAME} + +# Disable prompting during package installation +ARG DEBIAN_FRONTEND=noninteractive +ENV DEBIAN_FRONTEND=noninteractive +ENV TZ=Etc/UTC + +RUN sudo sh -c 'echo "deb http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" > /etc/apt/sources.list.d/ros2-latest.list' +RUN curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add - + +### Delete CycloneDDS and install from apt to resolve cmake conflict ### + +# Clear Install folder, we will need to rebuild the workspace again +WORKDIR ${SPACEROS_DIR} +RUN sudo rm -rf install/ + +# Delete problematic packages +WORKDIR ${SPACEROS_DIR}/src +RUN sudo rm -rf rmw_cyclonedds_cpp +RUN sudo rm -rf rmw_implementation +RUN sudo rm -rf rmw_cyclonedds +RUN sudo rm -rf realtime_tools + +WORKDIR ${SPACEROS_DIR} + +# Update rosdep +RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ + --mount=type=cache,target=/var/lib/apt,sharing=locked \ + sudo apt-get update -y +RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ + --mount=type=cache,target=/var/lib/apt,sharing=locked \ + sudo apt-get dist-upgrade -y +RUN rosdep update + +# TODO open a PR to resolve rmw-cyclonedds and rmw-implementation causing . +# Install them back from apt +RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ + --mount=type=cache,target=/var/lib/apt,sharing=locked \ + sudo apt install -y \ + ros-${ROS_DISTRO}-rmw-cyclonedds-cpp \ + ros-${ROS_DISTRO}-rmw-implementation \ + ros-${ROS_DISTRO}-realtime-tools + +# Rebuild Space ROS workspace +RUN /bin/bash -c 'source /opt/ros/humble/setup.bash \ + && colcon build --packages-ignore test_rmw_implementation' + +# Ensure proper permission for all files in SPACEROS_DIR +RUN sudo chown -R ${USERNAME}:${USERNAME} ${SPACEROS_DIR} + +### Delete CycloneDDS and install from apt to resolve cmake conflict ### + +### Install all dependencies for demo_ws and O3DE ### + +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 DEBIAN_FRONTEND=noninteractive apt-get install -y \ + python3-rosinstall-generator \ + libboost-python-dev build-essential cmake libboost-all-dev libois-dev \ + libzzip-dev libfreeimage-dev libfreetype6-dev libx11-dev libxt-dev libxaw7-dev \ + libsdl2-dev libpugixml-dev libglew-dev qtbase5-dev libspdlog-dev libfmt-dev \ + libignition-gazebo6-dev libssl-dev devscripts debian-keyring fakeroot debhelper \ + libboost-dev libsasl2-dev libicu-dev libzstd-dev doxygen libtinyxml-dev \ + libtinyxml2-dev python3-pyqt5 tzdata locales keyboard-configuration + +RUN sudo sed -i -e 's/# en_US.UTF-8 UTF-8/en_US.UTF-8 UTF-8/' /etc/locale.gen \ + && sudo dpkg-reconfigure --frontend=noninteractive locales \ + && sudo update-locale LANG=en_US.UTF-8 + +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 DEBIAN_FRONTEND=noninteractive apt-get install -y \ + bc \ + bind9-utils binutils ca-certificates clang cmake \ + file firewalld git git-lfs jq kbd kmod less \ + lsb-release libglu1-mesa-dev libxcb-xinerama0 libfontconfig1-dev \ + libcurl4-openssl-dev libnvidia-gl-470 libssl-dev libxcb-xkb-dev \ + libxkbcommon-x11-dev libxkbcommon-dev libxcb-xfixes0-dev \ + libxcb-xinput-dev libxcb-xinput0 \ + libpcre2-16-0 lsof net-tools ninja-build pciutils \ + python3-pip software-properties-common \ + sudo tar unzip vim wget xz-utils + # && rm -rf /var/lib/apt/lists/* # TODO need to figure out how to use this correctly + +### Install all dependencies for demo_ws and O3DE ### + +## Delete DEMO_WS repo, redownload correct one build the DEMO_WS again ### +WORKDIR ${DEMO_DIR} +RUN sudo rm -rf install/ build/ log/ + +# WORKDIR ${DEMO_DIR} +# Override what is necessary +# COPY --chown=${USERNAME}:${USERNAME} o3de-robot-sim.repos /tmp/ +# COPY --chown=${USERNAME}:${USERNAME} excluded-pkgs.txt /tmp/ + +# Delete space-ros/demo repo and replace it with Mechazo11/demo repo +# that contains the modified mars_rover launch file to drive rover +# TODO come up with a better solution so as not to disturb other projects in space-ros/demo repo +WORKDIR ${DEMO_DIR}/src +RUN sudo rm -rf demos/ && git clone --branch main --single-branch https://github.com/Mechazo11/space-ros-demos.git +WORKDIR ${DEMO_DIR} + +# Update the ownership of the source files (had to use sudo above to work around +# a possible inherited 'insteadof' from the host that forces use of ssh +RUN sudo chown -R ${USERNAME}:${USERNAME} ${DEMO_DIR} + +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 "/opt/ros/humble/setup.bash"' \ +&& /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_ws +RUN /bin/bash -c 'source ${SPACEROS_DIR}/install/setup.bash && source ${MOVEIT2_DIR}/install/setup.bash \ + && colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release' + +# Git clone RobotSim, NasaMarsRoverGem and MarsYardGem +WORKDIR ${O3DE_DIR}/Projects +RUN git clone --branch nasa_submission https://github.com/Mechazo11/RobotSim.git + +WORKDIR ${O3DE_DIR}/Gems +RUN git clone --branch nasa_submission https://github.com/Mechazo11/NasaCuriosityRoverGem.git +RUN git clone --branch nasa_submission https://github.com/Mechazo11/MarsYardGem.git + +# ## Symlink clang version to non-versioned clang and set cc to clang +RUN sudo DEBIAN_FRONTEND=noninteractive update-alternatives --install /usr/bin/cc cc /usr/bin/clang 100 \ + && sudo DEBIAN_FRONTEND=noninteractive update-alternatives --install /usr/bin/c++ c++ /usr/bin/clang++ 100 + +# #### Install Open 3D Engine #### + +# # Pull latest commit from development and build o3de. Note this does't build the whole engine +WORKDIR ${HOME_DIR} + +# # Install o3de engine (automatically pulls last commit from "development" branch) +RUN git clone --single-branch -b development https://github.com/o3de/o3de.git \ + && cd o3de \ + && git lfs install \ + && git lfs pull \ + && python/get_python.sh + +# # Install o3de-extras (automatically pulls last commit from "development" branch) +# # Contains ROS 2 Gem and other relevant assets + +# WORKDIR ${HOME_DIR} +RUN git clone --single-branch -b development https://github.com/o3de/o3de-extras.git \ + && cd o3de-extras \ + && git lfs install \ + && git lfs pull + +WORKDIR ${HOME_DIR}/o3de + +# Register RobotSim with this engine +#RUN ${O3DE_ENGINE}/scripts/o3de.sh register --this-engine +RUN ./scripts/o3de.sh register --this-engine \ + && ./scripts/o3de.sh register --gem-path ${O3DE_EXTRAS}/Gems/ROS2 \ + && ./scripts/o3de.sh register --gem-path ${O3DE_EXTRAS}/Gems/RosRobotSample \ + && ./scripts/o3de.sh register --gem-path ${O3DE_EXTRAS}/Gems/WarehouseSample \ + && ./scripts/o3de.sh register --gem-path ${O3DE_DIR}/Gems/NasaCuriosityRoverGem \ + && ./scripts/o3de.sh register --gem-path ${O3DE_DIR}/Gems/MarsYardGem \ + && ./scripts/o3de.sh register --gem-path ${HOME_DIR}/o3de/Gems/Terrain + +# Register RobotSim project. +# Project must be registered at the very end after all dependent gems are registered +RUN ./scripts/o3de.sh register --project-path ${O3DE_DIR}/Projects/${PROJECT_NAME} + +# # Move to project directory and run cmake and build project (Warning takes about 10-15 mins) +WORKDIR ${HOME_DIR} + +# Install more essential binaries gazebo-msgs from binary +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 \ + libunwind-dev ros-${ROS_DISTRO}-gazebo-ros-pkgs + +# Move to project directory +WORKDIR ${PROJECT_PATH} + +# Source ROS2, Space Ros, Movit2 and Demo_ws +RUN /bin/bash -c 'source "/opt/ros/humble/setup.bash"' \ + && /bin/bash -c 'source "${SPACEROS_DIR}/install/setup.bash"' \ + && /bin/bash -c 'source "${MOVEIT2_DIR}/install/setup.bash"' + +RUN /bin/bash -c 'source ${DEMO_DIR}/install/setup.bash \ + && cmake -B build/linux -G "Ninja Multi-Config" -DLY_DISABLE_TEST_MODULES=ON -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -DLY_STRIP_DEBUG_SYMBOLS=ON -DAZ_USE_PHYSX5:BOOL=ON' + +RUN cmake --build build/linux --config profile --target ${PROJECT_NAME} Editor ${PROJECT_NAME}.Assets + +# Add the user to the render group so that the user can access /dev/dri/renderD128 +# /dev/dri/renderD128 gives access to hardware-accelerated rendering tool such as GPU +RUN sudo usermod -aG render $USERNAME + +# Setup the entrypoint +COPY ./entrypoint.sh / +ENTRYPOINT ["/entrypoint.sh"] +CMD ["bash"] diff --git a/o3de_mars_rover/bcks/Dockerfile_one_shot_build b/o3de_mars_rover/bcks/Dockerfile_one_shot_build new file mode 100644 index 00000000..494a51b6 --- /dev/null +++ b/o3de_mars_rover/bcks/Dockerfile_one_shot_build @@ -0,0 +1,412 @@ +# Copyright 2024 Azmyin Md. Kamal +# +# 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 o3de_curiosit_docker image that showcases Open 3D Engine +# in simulating a Curiositry rover traversing through a test ground inspired by the NASA JPL Mars Yard +# O3DE docker file based on https://github.com/husarion/o3de-docker/blob/main/Dockerfile +# +# +# 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 osrf/space-ros:latest + +# Define arguments used in the metadata definition +ARG VCS_REF +ARG VERSION="preview" +ARG ROS_DISTRO=humble + +# Specify the docker image metadata +LABEL org.label-schema.schema-version="1.0" +LABEL org.label-schema.name="O3DE Curiosity Rover" +LABEL org.label-schema.description="Curiosity rover demo on a new test enviornment on the Space ROS platform and Open 3D Engine" +LABEL org.label-schema.vendor="Nasa Space ROS Sim Summer Spring Challenge 2024" +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/Mechazo11/space-ros-docker" +LABEL org.label-schema.vcs-ref=${VCS_REF} + +# Define a key enviornment variables +ENV MOVEIT2_DIR=${HOME_DIR}/moveit2 +ENV DEMO_DIR=${HOME_DIR}/demos_ws +ENV IGNITION_VERSION fortress +ENV GZ_VERSION fortress +ENV PROJECT_NAME=RobotSim +ENV O3DE_ENGINE=${HOME_DIR}/o3de +ENV O3DE_DIR=${HOME_DIR}/O3DE +ENV O3DE_EXTRAS=${HOME_DIR}/o3de-extras +ENV PROJECT_PATH=${O3DE_DIR}/Projects/${PROJECT_NAME} + +# Disable prompting during package installation +ARG DEBIAN_FRONTEND=noninteractive +ENV DEBIAN_FRONTEND=noninteractive +ENV TZ=Etc/UTC + +RUN sudo sh -c 'echo "deb http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" > /etc/apt/sources.list.d/ros2-latest.list' +RUN curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add - + +# Clone all space-ros sources +RUN mkdir ${SPACEROS_DIR}/src \ + && vcs import ${SPACEROS_DIR}/src < ${SPACEROS_DIR}/exact.repos + +### Delete CycloneDDS and install from apt to resolve cmake conflict ### + +# Update rosdep +RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ + --mount=type=cache,target=/var/lib/apt,sharing=locked \ + sudo apt-get update -y +RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ + --mount=type=cache,target=/var/lib/apt,sharing=locked \ + sudo apt-get dist-upgrade -y +RUN rosdep update + +# rmw-cyclonedds and rmw-implementation later causes a fatal bug. +# I have removed them in favor of having another top level workspace +# which appears to solve this problem +RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ + --mount=type=cache,target=/var/lib/apt,sharing=locked \ + sudo apt install -y \ + ros-${ROS_DISTRO}-rmw-cyclonedds-cpp \ + ros-${ROS_DISTRO}-rmw-implementation \ + ros-${ROS_DISTRO}-realtime-tools + +# Clear Install folder, we will need to rebuild the workspace again +WORKDIR ${SPACEROS_DIR} +RUN sudo rm -rf install/ + +WORKDIR ${SPACEROS_DIR}/src +RUN sudo rm -rf rmw_cyclonedds_cpp +RUN sudo rm -rf rmw_implementation +RUN sudo rm -rf rmw_cyclonedds +RUN sudo rm -rf realtime_tools + +# Rebuild Space ROS workspace +WORKDIR ${SPACEROS_DIR} +RUN /bin/bash -c 'source /opt/ros/humble/setup.bash \ + && colcon build --packages-ignore test_rmw_implementation' + +### Delete CycloneDDS and install from apt to resolve cmake conflict ### + +#### Install MOVEIT2_WS #### + +# Install the various build and test tools +RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ + --mount=type=cache,target=/var/lib/apt,sharing=locked \ + sudo apt install -y \ + build-essential \ + clang-format \ + cmake \ + git \ + libbullet-dev \ + python3-colcon-common-extensions \ + python3-flake8 \ + python3-pip \ + python3-pytest-cov \ + python3-rosdep \ + python3-setuptools \ + python3-vcstool \ + wget + +# Install some pip packages needed for testing +RUN python3 -m pip install -U \ + argcomplete \ + flake8-blind-except \ + flake8-builtins \ + flake8-class-newline \ + flake8-comprehensions \ + flake8-deprecated \ + flake8-docstrings \ + flake8-import-order \ + flake8-quotes \ + pytest-repeat \ + pytest-rerunfailures \ + pytest + +# # Get the MoveIt2 source code +WORKDIR ${HOME_DIR} +RUN sudo git clone https://github.com/moveit/moveit2.git -b ${ROSDISTRO} moveit2/src +RUN cd ${MOVEIT2_DIR}/src \ + && sudo git clone https://github.com/moveit/moveit2_tutorials.git -b ${ROSDISTRO} + +# Update the ownership of the source files (had to use sudo above to work around +# a possible inherited 'insteadof' from the host that forces use of ssh +RUN sudo chown -R ${USERNAME}:${USERNAME} ${MOVEIT2_DIR} + +# # Get rosinstall_generator +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 + +# # Generate repos file for moveit2 dependencies, excluding packages from Space ROS core. +COPY --chown=${USERNAME}:${USERNAME} moveit2-pkgs.txt /tmp/ +COPY --chown=${USERNAME}:${USERNAME} excluded-pkgs.txt /tmp/ +RUN rosinstall_generator \ + --rosdistro ${ROSDISTRO} \ + --deps \ + --exclude-path ${SPACEROS_DIR}/src \ + --exclude $(cat /tmp/excluded-pkgs.txt) -- \ + -- $(cat /tmp/moveit2-pkgs.txt) \ + > /tmp/moveit2_generated_pkgs.repos + +# # Get the repositories required by MoveIt2, but not included in Space ROS +WORKDIR ${MOVEIT2_DIR} +RUN vcs import src < /tmp/moveit2_generated_pkgs.repos +COPY --chown=${USERNAME}:${USERNAME} moveit2_tutorials.repos /tmp/ +RUN vcs import src < /tmp/moveit2_tutorials.repos + +# TODO open PR this library is missing? +#WORKDIR ${MOVEIT2_DIR}/src +#RUN git clone --branch main --single-branch https://github.com/PickNikRobotics/generate_parameter_library.git + +# Update the ownership of the source files (had to use sudo above to work around +# a possible inherited 'insteadof' from the host that forces use of ssh +WORKDIR ${MOVEIT2_DIR} +RUN sudo chown -R ${USERNAME}:${USERNAME} ${MOVEIT2_DIR} + +# Install system dependencies +RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ + --mount=type=cache,target=/var/lib/apt,sharing=locked \ + /bin/bash -c 'source ${SPACEROS_DIR}/install/setup.bash' \ + && rosdep install --from-paths ../spaceros/src src --ignore-src --rosdistro ${ROSDISTRO} -r -y --skip-keys "console_bridge generate_parameter_library fastcdr fastrtps rti-connext-dds-5.3.1 urdfdom_headers rmw_connextdds ros_testing rmw_connextdds rmw_fastrtps_cpp rmw_fastrtps_dynamic_cpp composition demo_nodes_py lifecycle rosidl_typesupport_fastrtps_cpp rosidl_typesupport_fastrtps_c ikos diagnostic_aggregator diagnostic_updater joy qt_gui rqt_gui rqt_gui_py" + +# Apply a patch to octomap_msgs to work around a build issue +COPY --chown=${USERNAME}:${USERNAME} octomap_fix.diff ./src/octomap_msgs +RUN cd src/octomap_msgs && git apply octomap_fix.diff + +# Unknown issue with generate_parameter_library delete and reinstall +WORKDIR ${MOVEIT2_DIR}/src +RUN sudo rm -rf generate_parameter_library +RUN git clone --branch main --single-branch https://github.com/PickNikRobotics/generate_parameter_library.git + +# Build MoveIt2 +# RUN /bin/bash -c 'source ${SPACEROS_DIR}/install/setup.bash \ +# && colcon build --packages-select generate_parameter_library --cmake-args -DCMAKE_BUILD_TYPE=Release -DCMAKE_EXPORT_COMPILE_COMMANDS=ON --event-handlers desktop_notification- status' + +# Manually fix for permission denined error +RUN sudo chown -R ${USERNAME}:${USERNAME} ${SPACEROS_DIR} + +WORKDIR ${MOVEIT2_DIR} + +# Build MoveIt2 +RUN /bin/bash -c 'source ${SPACEROS_DIR}/install/setup.bash \ + && colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release -DCMAKE_EXPORT_COMPILE_COMMANDS=ON --event-handlers desktop_notification- status-' + +#### Install MOVEIT2_WS #### + + + +#### Install DEMO_WS #### +WORKDIR ${HOME_DIR} + +# Install dependencies +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 DEBIAN_FRONTEND=noninteractive apt-get install -y \ + python3-rosinstall-generator \ + libboost-python-dev build-essential cmake libboost-all-dev libois-dev \ + libzzip-dev libfreeimage-dev libfreetype6-dev libx11-dev libxt-dev libxaw7-dev \ + libsdl2-dev libpugixml-dev libglew-dev qtbase5-dev libspdlog-dev libfmt-dev \ + libignition-gazebo6-dev libssl-dev devscripts debian-keyring fakeroot debhelper \ + libboost-dev libsasl2-dev libicu-dev libzstd-dev doxygen libtinyxml-dev \ + libtinyxml2-dev python3-pyqt5 tzdata locales keyboard-configuration + +RUN sudo sed -i -e 's/# en_US.UTF-8 UTF-8/en_US.UTF-8 UTF-8/' /etc/locale.gen \ + && sudo dpkg-reconfigure --frontend=noninteractive locales \ + && sudo update-locale LANG=en_US.UTF-8 + +# TODO delete +# RUN sed -i -e 's/# en_US.UTF-8 UTF-8/en_US.UTF-8 UTF-8/' /etc/locale.gen \ +# && dpkg-reconfigure --frontend=noninteractive locales \ +# && update-locale LANG=en_US.UTF-8 +# ENV LANG=en_US.UTF-8 + +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 \ + bc \ + bind9-utils \ + binutils \ + ca-certificates \ + clang \ + cmake \ + file \ + firewalld \ + git \ + git-lfs \ + jq \ + kbd \ + kmod \ + less \ + lsb-release \ + libglu1-mesa-dev \ + libxcb-xinerama0 \ + libfontconfig1-dev \ + libcurl4-openssl-dev \ + libnvidia-gl-470 \ + libssl-dev \ + libxcb-xkb-dev \ + libxkbcommon-x11-dev \ + libxkbcommon-dev \ + libxcb-xfixes0-dev \ + libxcb-xinput-dev \ + libxcb-xinput0 \ + libpcre2-16-0 \ + lsof \ + net-tools \ + ninja-build \ + pciutils \ + python3-pip \ + software-properties-common \ + sudo \ + tar \ + unzip \ + vim \ + wget \ + xz-utils + # && rm -rf /var/lib/apt/lists/* # TODO need to figure out how to use this correctly + +# 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 + +# Create demo_ws, setup packages, generate dependencies +RUN mkdir -p ${DEMO_DIR}/src +WORKDIR ${DEMO_DIR} +COPY --chown=${USERNAME}:${USERNAME} o3de-robot-sim.repos /tmp/ +COPY --chown=${USERNAME}:${USERNAME} excluded-pkgs.txt /tmp/ +RUN vcs import ${DEMO_DIR}/src < /tmp/o3de-robot-sim.repos + +# # Generate the repos file for o3de-robot-sim dependencies, excluding packages from Space ROS core +RUN rosinstall_generator \ + --rosdistro ${ROSDISTRO} \ + --deps \ + --exclude-path ${SPACEROS_DIR}/src \ + --exclude $(cat /tmp/excluded-pkgs.txt) -- \ + -- $(cat /tmp/o3de-robot-sim.repos) \ + > /tmp/o3de_robot_sim_generated_pkgs.repos + +# Update the ownership of the source files (had to use sudo above to work around +# a possible inherited 'insteadof' from the host that forces use of ssh +RUN sudo chown -R ${USERNAME}:${USERNAME} ${DEMO_DIR} + +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 "/opt/ros/humble/setup.bash"' \ +&& /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_ws +RUN /bin/bash -c 'source ${SPACEROS_DIR}/install/setup.bash && source ${MOVEIT2_DIR}/install/setup.bash \ + && colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release' + +# # Create O3DE/Projects and O3DE/Gems directory +RUN if [ -d "${O3DE_DIR}/Projects" ]; then rm -rf ${O3DE_DIR}/Gems; fi +RUN mkdir -p ${O3DE_DIR}/Projects + +RUN if [ -d "${O3DE_DIR}/Gems" ]; then rm -rf ${O3DE_DIR}/Gems; fi +RUN mkdir -p ${O3DE_DIR}/Gems + +# Git clone RobotSim, NasaMarsRoverGem and MarsYardGem +WORKDIR ${O3DE_DIR}/Projects +RUN git clone --branch nasa_submission https://github.com/Mechazo11/RobotSim.git + +WORKDIR ${O3DE_DIR}/Gems +RUN git clone --branch nasa_submission https://github.com/Mechazo11/NasaCuriosityRoverGem.git +RUN git clone --branch nasa_submission https://github.com/Mechazo11/MarsYardGem.git + +# ## Symlink clang version to non-versioned clang and set cc to clang +RUN sudo DEBIAN_FRONTEND=noninteractive update-alternatives --install /usr/bin/cc cc /usr/bin/clang 100 \ + && sudo DEBIAN_FRONTEND=noninteractive update-alternatives --install /usr/bin/c++ c++ /usr/bin/clang++ 100 + +# #### Install Open 3D Engine #### + +# # Pull latest commit from development and build o3de. Note this does't build the whole engine +WORKDIR ${HOME_DIR} + +# # Install o3de engine (automatically pulls last commit from "development" branch) +RUN git clone --single-branch -b development https://github.com/o3de/o3de.git \ + && cd o3de \ + && git lfs install \ + && git lfs pull \ + && python/get_python.sh + +# # Install o3de-extras (automatically pulls last commit from "development" branch) +# # Contains ROS 2 Gem and other relevant assets + +# WORKDIR ${HOME_DIR} +RUN git clone --single-branch -b development https://github.com/o3de/o3de-extras.git \ + && cd o3de-extras \ + && git lfs install \ + && git lfs pull + +WORKDIR ${HOME_DIR}/o3de + +# Register RobotSim with this engine +#RUN ${O3DE_ENGINE}/scripts/o3de.sh register --this-engine +RUN ./scripts/o3de.sh register --this-engine +RUN ./scripts/o3de.sh register --gem-path ${O3DE_EXTRAS}/Gems/ROS2 \ + && ./scripts/o3de.sh register --gem-path ${O3DE_EXTRAS}/Gems/RosRobotSample \ + && ./scripts/o3de.sh register --gem-path ${O3DE_EXTRAS}/Gems/WarehouseSample \ + && ./scripts/o3de.sh register --gem-path ${O3DE_DIR}/Gems/NasaCuriosityRoverGem \ + && ./scripts/o3de.sh register --gem-path ${O3DE_DIR}/Gems/MarsYardGem + +# Register the Terrian gem which is located in o3de folder +RUN ./scripts/o3de.sh register --gem-path ${HOME_DIR}/o3de/Gems/Terrain + +# Register RobotSim project. +# Project must be registered at the very end after all dependent gems are registered +RUN ./scripts/o3de.sh register --project-path ${O3DE_DIR}/Projects/${PROJECT_NAME} + +# # Move to project directory and run cmake and build project (Warning takes about 10-15 mins) +WORKDIR ${HOME_DIR} + +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 \ + libunwind-dev + +# Move to project directory +WORKDIR ${PROJECT_PATH} + +# # Source space_ros, movit2 and demo_ws +RUN /bin/bash -c 'source "/opt/ros/humble/setup.bash"' \ + && /bin/bash -c 'source "${SPACEROS_DIR}/install/setup.bash"' \ + && /bin/bash -c 'source "${MOVEIT2_DIR}/install/setup.bash"' + +RUN /bin/bash -c 'source ${DEMO_DIR}/install/setup.bash \ + && cmake -B build/linux -G "Ninja Multi-Config" -DLY_DISABLE_TEST_MODULES=ON -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -DLY_STRIP_DEBUG_SYMBOLS=ON -DAZ_USE_PHYSX5:BOOL=ON' + +RUN cmake --build build/linux --config profile --target ${PROJECT_NAME} Editor ${PROJECT_NAME}.Assets + +# Add the user to the render group so that the user can access /dev/dri/renderD128 +# /dev/dri/renderD128 gives access to hardware-accelerated rendering tool such as GPU +RUN sudo usermod -aG render $USERNAME + +# Setup the entrypoint +COPY ./entrypoint.sh / +ENTRYPOINT ["/entrypoint.sh"] +CMD ["bash"] diff --git a/o3de_mars_rover/bcks/Dockerfile_rwm_conflict_bug b/o3de_mars_rover/bcks/Dockerfile_rwm_conflict_bug new file mode 100644 index 00000000..50cc7304 --- /dev/null +++ b/o3de_mars_rover/bcks/Dockerfile_rwm_conflict_bug @@ -0,0 +1,338 @@ +# Copyright 2024 Azmyin Md. Kamal +# +# 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 o3de_curiosit_docker image that showcases Open 3D Engine +# in simulating a Curiositry rover traversing through a test ground inspired by the NASA JPL Mars Yard +# O3DE docker file based on https://github.com/husarion/o3de-docker/blob/main/Dockerfile +# +# +# 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 osrf/space-ros:latest + +# Note, the above image begins with user in the spaceros ws. + +# Define arguments used in the metadata definition +ARG VCS_REF +ARG VERSION="preview" +ARG ROS_DISTRO=humble + +# Specify the docker image metadata +LABEL org.label-schema.schema-version="1.0" +LABEL org.label-schema.name="O3DE Curiosity Rover" +LABEL org.label-schema.description="Curiosity rover demo on a new test enviornment on the Space ROS platform and Open 3D Engine" +LABEL org.label-schema.vendor="Nasa Space ROS Sim Summer Spring Challenge 2024" +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/Mechazo11/space-ros-docker" +LABEL org.label-schema.vcs-ref=${VCS_REF} + +# Define a key enviornment variables +ENV DEMO_DIR=${HOME_DIR}/demos_ws +ENV IGNITION_VERSION fortress +ENV GZ_VERSION fortress +ENV PROJECT_NAME=RobotSim +ENV O3DE_ENGINE=${HOME_DIR}/o3de +ENV O3DE_DIR=${HOME_DIR}/O3DE +ENV O3DE_EXTRAS=${HOME_DIR}/o3de-extras +ENV PROJECT_PATH=${O3DE_DIR}/Projects/${PROJECT_NAME} + +# Disable prompting during package installation +ARG DEBIAN_FRONTEND=noninteractive +ENV TZ Etc/UTC + +#### Dependencies #### + +# Install dependencies +# 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 DEBIAN_FRONTEND=noninteractive apt-get install -y \ + python3-rosinstall-generator \ + libboost-python-dev build-essential cmake libboost-all-dev libois-dev \ + libzzip-dev libfreeimage-dev libfreetype6-dev libx11-dev libxt-dev libxaw7-dev \ + libsdl2-dev libpugixml-dev libglew-dev qtbase5-dev libspdlog-dev libfmt-dev \ + libignition-gazebo6-dev libssl-dev devscripts debian-keyring fakeroot debhelper \ + libboost-dev libsasl2-dev libicu-dev libzstd-dev doxygen libtinyxml-dev \ + libtinyxml2-dev python3-pyqt5 tzdata locales keyboard-configuration + +RUN sudo sed -i -e 's/# en_US.UTF-8 UTF-8/en_US.UTF-8 UTF-8/' /etc/locale.gen \ + && sudo dpkg-reconfigure --frontend=noninteractive locales \ + && sudo update-locale LANG=en_US.UTF-8 + +ENV LANG=en_US.UTF-8 + +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 \ + bc \ + bind9-utils \ + binutils \ + ca-certificates \ + clang \ + file \ + firewalld \ + git \ + git-lfs \ + jq \ + kbd \ + kmod \ + less \ + lsb-release \ + libglu1-mesa-dev \ + libxcb-xinerama0 \ + libfontconfig1-dev \ + libcurl4-openssl-dev \ + libnvidia-gl-470 \ + libssl-dev \ + libxcb-xkb-dev \ + libxkbcommon-x11-dev \ + libxkbcommon-dev \ + libxcb-xfixes0-dev \ + libxcb-xinput-dev \ + libxcb-xinput0 \ + libpcre2-16-0 \ + lsof \ + net-tools \ + ninja-build \ + pciutils \ + python3-pip \ + software-properties-common \ + sudo \ + tar \ + unzip \ + vim \ + wget \ + xz-utils + # && rm -rf /var/lib/apt/lists/* # TODO need to figure out how to use this correctly + +# 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 + +# Experimental fix +# RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ +# --mount=type=cache,target=/var/lib/apt,sharing=locked \ +# sudo apt-get install -y --no-install-recommends ros-${ROS_DISTRO}-rmw-cyclonedds-cpp +# ENV RMW_IMPLEMENTATION=rmw_cyclonedds_cpp + +# 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 + +#### Space ROS workspace with additional packages #### +# Make sure the latest versions of packages are installed +# 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 sudo DEBIAN_FRONTEND=noninteractive add-apt-repository ppa:ubuntuhandbook1/ppa +RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ + --mount=type=cache,target=/var/lib/apt,sharing=locked \ + sudo apt-get update + +# Install the various build and test tools +RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ + --mount=type=cache,target=/var/lib/apt,sharing=locked \ + sudo apt install -y \ + build-essential \ + clang-format \ + cmake \ + git \ + libbullet-dev \ + libqt4-dev \ + libqt4-opengl-dev \ + python3-colcon-common-extensions \ + python3-flake8 \ + python3-pip \ + python3-pytest-cov \ + python3-rosdep \ + python3-setuptools \ + python3-vcstool \ + wget + +# Install some pip packages needed for testing +RUN python3 -m pip install -U \ + argcomplete \ + flake8-blind-except \ + flake8-builtins \ + flake8-class-newline \ + flake8-comprehensions \ + flake8-deprecated \ + flake8-docstrings \ + flake8-import-order \ + flake8-quotes \ + pytest-repeat \ + pytest-rerunfailures \ + pytest + +# RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ +# --mount=type=cache,target=/var/lib/apt,sharing=locked \ +# sudo apt-get dist-upgrade -y +RUN rosdep update + +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 + +# TODO remove Install folder +# TODO remove realtime_tools from src, install it from apt +# TODO remove cyclone_dds from src and install it from apt +# TODO add colcon ignore for test_rmw_implementation + +# colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release -DCMAKE_EXPORT_COMPILE_COMMANDS=ON --packages-ignore test_rmw_implementation --event-handlers desktop_notification- status- + +# Clone all space-ros sources +# In the earthly build, after building the packages, the src file was deleted +RUN mkdir ${SPACEROS_DIR}/src \ + && vcs import ${SPACEROS_DIR}/src < ${SPACEROS_DIR}/exact.repos + +# Copy the o3de-robot-sim repos +COPY --chown=${USERNAME}:${USERNAME} o3de-robot-sim.repos /tmp/ +COPY --chown=${USERNAME}:${USERNAME} excluded-pkgs.txt /tmp/ +RUN vcs import ${SPACEROS_DIR}/src < /tmp/o3de-robot-sim.repos + +# Generate the repos file for o3de-robot-sim dependencies, excluding packages from Space ROS core +RUN rosinstall_generator \ + --rosdistro ${ROSDISTRO} \ + --deps \ + # --exclude-path ${SPACEROS_DIR}/src \ + --exclude $(cat /tmp/excluded-pkgs.txt) -- \ + -- $(cat /tmp/o3de-robot-sim.repos) \ + > /tmp/o3de_robot_sim_generated_pkgs.repos + +WORKDIR ${SPACEROS_DIR}/src +RUN git clone --branch main --single-branch https://github.com/PickNikRobotics/generate_parameter_library.git + +# Install system dependencies +WORKDIR ${SPACEROS_DIR} +RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ + --mount=type=cache,target=/var/lib/apt,sharing=locked \ + /bin/bash -c 'source ${SPACEROS_DIR}/install/setup.bash' \ + && rosdep install --from-paths src --ignore-src --rosdistro ${ROSDISTRO} -r -y --skip-keys "console_bridge generate_parameter_library fastcdr fastrtps rti-connext-dds-5.3.1 urdfdom_headers rmw_connextdds ros_testing rmw_connextdds rmw_fastrtps_cpp rmw_fastrtps_dynamic_cpp composition demo_nodes_py lifecycle rosidl_typesupport_fastrtps_cpp rosidl_typesupport_fastrtps_c ikos diagnostic_aggregator diagnostic_updater joy qt_gui rqt_gui rqt_gui_py" + + +# Build the demo_ws +WORKDIR ${SPACEROS_DIR} +RUN /bin/bash -c 'source ${SPACEROS_DIR}/install/setup.bash \ + && colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release -DCMAKE_EXPORT_COMPILE_COMMANDS=ON --event-handlers desktop_notification- status-' + +# Create O3DE/Projects +RUN if [ -d "${O3DE_DIR}/Projects" ]; then rm -rf ${O3DE_DIR}/Gems; fi +RUN mkdir -p ${O3DE_DIR}/Projects + +# O3DE/Gems directory +RUN if [ -d "${O3DE_DIR}/Gems" ]; then rm -rf ${O3DE_DIR}/Gems; fi +RUN mkdir -p ${O3DE_DIR}/Gems + +# Git clone RobotSim, NasaMarsRoverGem and MarsYardGem +WORKDIR ${O3DE_DIR}/Projects +RUN git clone --branch nasa_submission https://github.com/Mechazo11/RobotSim.git + +WORKDIR ${O3DE_DIR}/Gems +RUN git clone --branch nasa_submission https://github.com/Mechazo11/NasaCuriosityRoverGem.git +RUN git clone --branch nasa_submission https://github.com/Mechazo11/MarsYardGem.git + +# ## Symlink clang version to non-versioned clang and set cc to clang +RUN sudo DEBIAN_FRONTEND=noninteractive update-alternatives --install /usr/bin/cc cc /usr/bin/clang 100 \ + && sudo DEBIAN_FRONTEND=noninteractive update-alternatives --install /usr/bin/c++ c++ /usr/bin/clang++ 100 + +# #### Install Open 3D Engine #### + +# # Pull latest commit from development and build o3de. Note this does't build the whole engine +WORKDIR ${HOME_DIR} + +# # Install o3de engine (automatically pulls last commit from "development" branch) +RUN git clone --single-branch -b development https://github.com/o3de/o3de.git \ + && cd o3de \ + && git lfs install \ + && git lfs pull \ + && python/get_python.sh + +# # Install o3de-extras (automatically pulls last commit from "development" branch) +# # Contains ROS 2 Gem and other relevant assets + +WORKDIR ${HOME_DIR} +RUN git clone --single-branch -b development https://github.com/o3de/o3de-extras.git \ + && cd o3de-extras \ + && git lfs install \ + && git lfs pull + +WORKDIR ${HOME_DIR}/o3de + +# # Register RobotSim with this engine +RUN ./scripts/o3de.sh register --this-engine + +# Register ROS 2 Gem, Husarion robot, Curiosity rover and Mars Yard gem +RUN ./scripts/o3de.sh register --gem-path ${O3DE_EXTRAS}/Gems/ROS2 \ + && ./scripts/o3de.sh register --gem-path ${O3DE_EXTRAS}/Gems/RosRobotSample \ + && ./scripts/o3de.sh register --gem-path ${O3DE_EXTRAS}/Gems/WarehouseSample \ + && ./scripts/o3de.sh register --gem-path ${O3DE_DIR}/Gems/NasaCuriosityRoverGem \ + && ./scripts/o3de.sh register --gem-path ${O3DE_DIR}/Gems/MarsYardGem + +# # Register the Terrian gem which is located in o3de folder +RUN ./scripts/o3de.sh register --gem-path ${HOME_DIR}/o3de/Gems/Terrain + +# # Register RobotSim project. +# # Project must be registered at the very end after all dependent gems are registered +RUN ./scripts/o3de.sh register --project-path ${O3DE_DIR}/Projects/${PROJECT_NAME} + +# TODO move this dependency up +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 \ + libunwind-dev + +# # Testing build by manually building out a ROS 2 Humble core +# # RUN sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg + +# # 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 \ +# # ros-humble-ros-base + +# # # Move to project directory +WORKDIR ${PROJECT_PATH} + +RUN /bin/bash -c 'source ${SPACEROS_DIR}/install/setup.bash \ + && cmake -B build/linux -G "Ninja Multi-Config" -DLY_DISABLE_TEST_MODULES=ON -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -DLY_STRIP_DEBUG_SYMBOLS=ON -DAZ_USE_PHYSX5:BOOL=ON' + + +# # Source space_ros workspace and build +# RUN /bin/bash -c 'source "${SPACEROS_DIR}/install/setup.bash"' \ +# && /bin/bash -c 'source "${MOVEIT2_DIR}/install/setup.bash"' \ +# && /bin/bash -c 'source "${DEMO_DIR}/install/setup.bash"' +# #&& cmake -B build/linux -G "Ninja Multi-Config" -DLY_DISABLE_TEST_MODULES=ON -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -DLY_STRIP_DEBUG_SYMBOLS=ON -DAZ_USE_PHYSX5:BOOL=ON \ +# #&& cmake --build build/linux --config profile --target ${PROJECT_NAME} Editor ${PROJECT_NAME}.Assets + +# #RUN /bin/bash -c 'source "/opt/ros/humble/setup.bash"' + +# RUN cmake -B build/linux -G "Ninja Multi-Config" -DLY_DISABLE_TEST_MODULES=ON -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -DLY_STRIP_DEBUG_SYMBOLS=ON -DAZ_USE_PHYSX5:BOOL=ON \ +# && cmake --build build/linux --config profile --target ${PROJECT_NAME} Editor ${PROJECT_NAME}.Assets + +# # Add the user to the render group so that the user can access /dev/dri/renderD128 +# # /dev/dri/renderD128 gives access to hardware-accelerated rendering tool such as GPU +# RUN sudo usermod -aG render $USERNAME + +# # Setup the entrypoint +# COPY ./entrypoint.sh / +# ENTRYPOINT ["/entrypoint.sh"] +# CMD ["bash"] diff --git a/o3de_mars_rover/bcks/build_bck_works.sh b/o3de_mars_rover/bcks/build_bck_works.sh new file mode 100755 index 00000000..6f8e47df --- /dev/null +++ b/o3de_mars_rover/bcks/build_bck_works.sh @@ -0,0 +1,23 @@ +#!/usr/bin/env bash + +ORG=openrobotics +IMAGE=o3de_curiosity_docker +TAG=latest + +VCS_REF="" +VERSION=preview + +# Exit script with failure if build fails +set -eo pipefail + +echo "" +echo "##### Building O3DE + Space ROS Demo Docker Image #####" +echo "" + +docker build -t $ORG/$IMAGE:$TAG \ + --build-arg VCS_REF="$VCS_REF" \ + --build-arg VERSION="$VERSION" . + +echo "" +echo "##### Done! #####" + diff --git a/o3de_mars_rover/build.sh b/o3de_mars_rover/build.sh new file mode 100755 index 00000000..615ed12d --- /dev/null +++ b/o3de_mars_rover/build.sh @@ -0,0 +1,50 @@ +#!/usr/bin/env bash + +# This shell script will build openrobotics/moveit2, openrobotics/space_robot_demo and then + +ORG=openrobotics +IMAGE1=moveit2 +IMAGE2=space_robot_demo +IMAGE3=o3de_curiosity_docker +TAG=latest + +VCS_REF="" +VERSION=preview + +# Directory where Dockerfiles are stored +DOCKER_DIR="./docker" + +# Exit script with failure if build fails +set -eo pipefail + +echo "" +echo "##### Building moviet2 image #####" + +docker build -t $ORG/$IMAGE1:$TAG -f $DOCKER_DIR/Dockerfile_moveit2 \ + --build-arg VCS_REF="$VCS_REF" \ + --build-arg VERSION="$VERSION" . + +echo "" +echo "##### Done! #####" + +echo "" +echo "##### Building space_robots_demo image #####" + +docker build -t $ORG/$IMAGE2:$TAG -f $DOCKER_DIR/Dockerfile_space_robots \ + --build-arg VCS_REF="$VCS_REF" \ + --build-arg VERSION="$VERSION" . + +echo "" +echo "##### Done! #####" + +echo "" +echo "##### Building o3de curiosity rover demo docker image #####" +echo "" + +docker build -t $ORG/$IMAGE3:$TAG -f $DOCKER_DIR/Dockerfile_o3de \ + --build-arg VCS_REF="$VCS_REF" \ + --build-arg VERSION="$VERSION" . + +echo "" +echo "##### Done! #####" + diff --git a/o3de_mars_rover/demo_manual_pkgs.repos b/o3de_mars_rover/demo_manual_pkgs.repos new file mode 100644 index 00000000..bd11f4b5 --- /dev/null +++ b/o3de_mars_rover/demo_manual_pkgs.repos @@ -0,0 +1,42 @@ +repositories: + demos: + type: git + url: https://github.com/Mechazo11/space-ros-demos.git + version: main + 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 + simulation: + type: git + url: https://github.com/space-ros/simulation.git + version: main + ros-humble-warehouse-ros-mongo: + type: git + url: https://github.com/moveit/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/o3de_mars_rover/docker/Dockerfile_moveit2 b/o3de_mars_rover/docker/Dockerfile_moveit2 new file mode 100644 index 00000000..6b7db941 --- /dev/null +++ b/o3de_mars_rover/docker/Dockerfile_moveit2 @@ -0,0 +1,207 @@ +# 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 MoveIt2/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 osrf/space-ros: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="MoveIt2" +LABEL org.label-schema.description="Preview version of the MoveIt2/Space ROS platform" +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/docker-images" +LABEL org.label-schema.vcs-ref=${VCS_REF} + +# Disable prompting during package installation +ARG DEBIAN_FRONTEND=noninteractive + +# Patch to solve rmw_cyclone_dds and rmw_implementation bug and control_toolbox finding fastrtps-cpp +# TODO raise Issue, why fastrtps-cpp is available? +# RUN source /opt/ros/humble/setup.bash +# RUN sudo apt-get remove ros-$ROS_DISTRO-rmw-fastrtps-cpp + +# TODO temporary solution, move these away from moveit2 after fix is done to base image +# Clone all space-ros sources +RUN mkdir ${SPACEROS_DIR}/src \ + && vcs import ${SPACEROS_DIR}/src < ${SPACEROS_DIR}/exact.repos + +# Clear Install folder, we will need to rebuild the workspace again +RUN sudo sh -c 'echo "deb http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" > /etc/apt/sources.list.d/ros2-latest.list' +RUN curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add - + +WORKDIR ${SPACEROS_DIR} +RUN sudo rm -rf install/ + +# Delete problematic packages +WORKDIR ${SPACEROS_DIR}/src +RUN sudo rm -rf rmw_cyclonedds_cpp +RUN sudo rm -rf rmw_implementation +RUN sudo rm -rf rmw_cyclonedds +RUN sudo rm -rf realtime_tools + +WORKDIR ${SPACEROS_DIR} + +# Update rosdep +RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ + --mount=type=cache,target=/var/lib/apt,sharing=locked \ + sudo apt-get update -y +RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ + --mount=type=cache,target=/var/lib/apt,sharing=locked \ + sudo apt-get dist-upgrade -y +RUN rosdep update + +# TODO open a PR to resolve rmw-cyclonedds and rmw-implementation causing . +# Install them back from apt +RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ + --mount=type=cache,target=/var/lib/apt,sharing=locked \ + sudo apt install -y \ + ros-humble-rmw-cyclonedds-cpp \ + ros-humble-rmw-implementation \ + ros-humble-realtime-tools + +# Rebuild Space ROS workspace +RUN /bin/bash -c 'source /opt/ros/humble/setup.bash \ + && colcon build --packages-ignore test_rmw_implementation' + +# Ensure proper permission for all files in SPACEROS_DIR +RUN sudo chown -R ${USERNAME}:${USERNAME} ${SPACEROS_DIR} + +# Define key locations +ENV MOVEIT2_DIR=${HOME_DIR}/moveit2 + +# Make sure the latest versions of packages are installed +# 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 +RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ + --mount=type=cache,target=/var/lib/apt,sharing=locked \ + sudo apt-get dist-upgrade -y +RUN rosdep update + +# Install the various build and test tools +RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ + --mount=type=cache,target=/var/lib/apt,sharing=locked \ + sudo apt install -y \ + build-essential \ + clang-format \ + cmake \ + git \ + libbullet-dev \ + python3-colcon-common-extensions \ + python3-flake8 \ + python3-pip \ + python3-pytest-cov \ + python3-rosdep \ + python3-setuptools \ + python3-vcstool \ + wget + +# Install some pip packages needed for testing +RUN python3 -m pip install -U \ + argcomplete \ + flake8-blind-except \ + flake8-builtins \ + flake8-class-newline \ + flake8-comprehensions \ + flake8-deprecated \ + flake8-docstrings \ + flake8-import-order \ + flake8-quotes \ + pytest-repeat \ + pytest-rerunfailures \ + pytest + +# Get the MoveIt2 source code +WORKDIR ${HOME_DIR} +RUN sudo git clone https://github.com/moveit/moveit2.git -b ${ROSDISTRO} moveit2/src +RUN cd ${MOVEIT2_DIR}/src \ + && sudo git clone https://github.com/moveit/moveit2_tutorials.git -b ${ROSDISTRO} + +# Update the ownership of the source files (had to use sudo above to work around +# a possible inherited 'insteadof' from the host that forces use of ssh +RUN sudo chown -R ${USERNAME}:${USERNAME} ${MOVEIT2_DIR} + +# Get rosinstall_generator +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 + +# Generate repos file for moveit2 dependencies, excluding packages from Space ROS core. +COPY --chown=${USERNAME}:${USERNAME} moveit2-pkgs.txt /tmp/ +COPY --chown=${USERNAME}:${USERNAME} excluded-pkgs.txt /tmp/ +RUN rosinstall_generator \ + --rosdistro ${ROSDISTRO} \ + --deps \ + --exclude-path ${SPACEROS_DIR}/src \ + --exclude $(cat /tmp/excluded-pkgs.txt) -- \ + -- $(cat /tmp/moveit2-pkgs.txt) \ + > /tmp/moveit2_generated_pkgs.repos + +# Get the repositories required by MoveIt2, but not included in Space ROS +WORKDIR ${MOVEIT2_DIR} +RUN vcs import src < /tmp/moveit2_generated_pkgs.repos +COPY --chown=${USERNAME}:${USERNAME} moveit2_tutorials.repos /tmp/ +RUN vcs import src < /tmp/moveit2_tutorials.repos + +# Update the ownership of the source files (had to use sudo above to work around +# a possible inherited 'insteadof' from the host that forces use of ssh +RUN sudo chown -R ${USERNAME}:${USERNAME} ${MOVEIT2_DIR} + +# Install system dependencies +RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ + --mount=type=cache,target=/var/lib/apt,sharing=locked \ + /bin/bash -c 'source ${SPACEROS_DIR}/install/setup.bash' \ + && rosdep install --from-paths ../spaceros/src src --ignore-src --rosdistro ${ROSDISTRO} -r -y --skip-keys "console_bridge generate_parameter_library fastcdr fastrtps rti-connext-dds-5.3.1 urdfdom_headers rmw_connextdds ros_testing rmw_connextdds rmw_fastrtps_cpp rmw_fastrtps_dynamic_cpp composition demo_nodes_py lifecycle rosidl_typesupport_fastrtps_cpp rosidl_typesupport_fastrtps_c ikos diagnostic_aggregator diagnostic_updater joy qt_gui rqt_gui rqt_gui_py" + +# Apply a patch to octomap_msgs to work around a build issue +COPY --chown=${USERNAME}:${USERNAME} octomap_fix.diff ./src/octomap_msgs +RUN cd src/octomap_msgs && git apply octomap_fix.diff + +# Build MoveIt2 +# RUN /bin/bash -c 'source ${SPACEROS_DIR}/install/setup.bash \ +# && colcon build --packages-select control_msgs control_toolbox' + +# Build MoveIt2 +RUN /bin/bash -c 'source ${SPACEROS_DIR}/install/setup.bash \ + && colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release -DCMAKE_EXPORT_COMPILE_COMMANDS=ON --event-handlers desktop_notification- status-' + +# # Add a couple sample GUI apps for testing +RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ + --mount=type=cache,target=/var/lib/apt,sharing=locked \ + sudo apt-get install -y \ + firefox \ + glmark2 \ + libcanberra-gtk3-0 \ + libpci-dev \ + xauth \ + xterm + +# Set up the entrypoint +COPY ./entrypoint.sh / +ENTRYPOINT ["/entrypoint.sh"] +CMD ["bash"] diff --git a/o3de_mars_rover/docker/Dockerfile_o3de b/o3de_mars_rover/docker/Dockerfile_o3de new file mode 100644 index 00000000..2cfba472 --- /dev/null +++ b/o3de_mars_rover/docker/Dockerfile_o3de @@ -0,0 +1,171 @@ +# Copyright 2024 Azmyin Md. Kamal +# +# 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 o3de_curiosit_docker image that showcases Open 3D Engine +# in simulating a Curiosity rover traversing through a test ground inspired by the NASA JPL Mars Yard +# O3DE docker file based on https://github.com/husarion/o3de-docker/blob/main/Dockerfile +# Requirements: openrobotics/space_robots_demo 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/space_robot_demo:latest + +# Define arguments used in the metadata definition +ARG VCS_REF +ARG VERSION="preview" +ARG ROS_DISTRO=humble + +# Specify the docker image metadata +LABEL org.label-schema.schema-version="1.0" +LABEL org.label-schema.name="O3DE Curiosity Rover" +LABEL org.label-schema.description="O3DE Curiosity rover demo image" +LABEL org.label-schema.vendor="Nasa Space ROS Sim Summer Spring Challenge 2024" +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/Mechazo11/space-ros-docker" +LABEL org.label-schema.vcs-ref=${VCS_REF} + +# Define a key enviornment variables +ENV MOVEIT2_DIR=${HOME_DIR}/moveit2 +ENV DEMO_DIR=${HOME_DIR}/demos_ws +ENV IGNITION_VERSION fortress +ENV GZ_VERSION fortress +ENV PROJECT_NAME=RobotSim +ENV O3DE_ENGINE=${HOME_DIR}/o3de +ENV O3DE_DIR=${HOME_DIR}/O3DE +ENV O3DE_EXTRAS=${HOME_DIR}/o3de-extras +ENV PROJECT_PATH=${O3DE_DIR}/Projects/${PROJECT_NAME} + +# Disable prompting during package installation +ARG DEBIAN_FRONTEND=noninteractive +ENV DEBIAN_FRONTEND=noninteractive +ENV TZ=Etc/UTC + +### Install all dependencies for demo_ws and O3DE ### + +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 DEBIAN_FRONTEND=noninteractive apt-get install -y \ + python3-rosinstall-generator \ + libboost-python-dev build-essential cmake libboost-all-dev libois-dev \ + libzzip-dev libfreeimage-dev libfreetype6-dev libx11-dev libxt-dev libxaw7-dev \ + libsdl2-dev libpugixml-dev libglew-dev qtbase5-dev libspdlog-dev libfmt-dev \ + libignition-gazebo6-dev libssl-dev devscripts debian-keyring fakeroot debhelper \ + libboost-dev libsasl2-dev libicu-dev libzstd-dev doxygen libtinyxml-dev \ + libtinyxml2-dev python3-pyqt5 tzdata locales keyboard-configuration + +RUN sudo sed -i -e 's/# en_US.UTF-8 UTF-8/en_US.UTF-8 UTF-8/' /etc/locale.gen \ + && sudo dpkg-reconfigure --frontend=noninteractive locales \ + && sudo update-locale LANG=en_US.UTF-8 + +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 DEBIAN_FRONTEND=noninteractive apt-get install -y \ + bc \ + bind9-utils binutils ca-certificates clang cmake \ + file firewalld git git-lfs jq kbd kmod less \ + lsb-release libglu1-mesa-dev libxcb-xinerama0 libfontconfig1-dev \ + libcurl4-openssl-dev libnvidia-gl-470 libssl-dev libxcb-xkb-dev \ + libxkbcommon-x11-dev libxkbcommon-dev libxcb-xfixes0-dev \ + libxcb-xinput-dev libxcb-xinput0 \ + libpcre2-16-0 lsof net-tools ninja-build pciutils \ + python3-pip software-properties-common \ + sudo tar unzip vim wget xz-utils + # && rm -rf /var/lib/apt/lists/* # TODO need to figure out how to use this correctly + +### Install all dependencies for O3DE ### + +# Git clone RobotSim, NasaMarsRoverGem and MarsYardGem +WORKDIR ${O3DE_DIR}/Projects +RUN git clone --branch nasa_submission https://github.com/Mechazo11/RobotSim.git + +WORKDIR ${O3DE_DIR}/Gems +RUN git clone --branch nasa_submission https://github.com/Mechazo11/NasaCuriosityRoverGem.git +RUN git clone --branch nasa_submission https://github.com/Mechazo11/MarsYardGem.git + +# ## Symlink clang version to non-versioned clang and set cc to clang +RUN sudo DEBIAN_FRONTEND=noninteractive update-alternatives --install /usr/bin/cc cc /usr/bin/clang 100 \ + && sudo DEBIAN_FRONTEND=noninteractive update-alternatives --install /usr/bin/c++ c++ /usr/bin/clang++ 100 + +# #### Install Open 3D Engine #### + +# # Pull latest commit from development and build o3de. Note this does't build the whole engine +WORKDIR ${HOME_DIR} + +# # Install o3de engine (automatically pulls last commit from "development" branch) +RUN git clone --single-branch -b development https://github.com/o3de/o3de.git \ + && cd o3de \ + && git lfs install \ + && git lfs pull \ + && python/get_python.sh + +# # Install o3de-extras (automatically pulls last commit from "development" branch) +# # Contains ROS 2 Gem and other relevant assets + +# WORKDIR ${HOME_DIR} +RUN git clone --single-branch -b development https://github.com/o3de/o3de-extras.git \ + && cd o3de-extras \ + && git lfs install \ + && git lfs pull + +WORKDIR ${HOME_DIR}/o3de + +# Register RobotSim with this engine +#RUN ${O3DE_ENGINE}/scripts/o3de.sh register --this-engine +RUN ./scripts/o3de.sh register --this-engine \ + && ./scripts/o3de.sh register --gem-path ${O3DE_EXTRAS}/Gems/ROS2 \ + && ./scripts/o3de.sh register --gem-path ${O3DE_EXTRAS}/Gems/RosRobotSample \ + && ./scripts/o3de.sh register --gem-path ${O3DE_EXTRAS}/Gems/WarehouseSample \ + && ./scripts/o3de.sh register --gem-path ${O3DE_DIR}/Gems/NasaCuriosityRoverGem \ + && ./scripts/o3de.sh register --gem-path ${O3DE_DIR}/Gems/MarsYardGem \ + && ./scripts/o3de.sh register --gem-path ${HOME_DIR}/o3de/Gems/Terrain + +# Register RobotSim project. +# Project must be registered at the very end after all dependent gems are registered +RUN ./scripts/o3de.sh register --project-path ${O3DE_DIR}/Projects/${PROJECT_NAME} + +# # Move to project directory and run cmake and build project (Warning takes about 10-15 mins) +WORKDIR ${HOME_DIR} + +# Install more essential binaries gazebo-msgs from binary +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 \ + libunwind-dev ros-${ROS_DISTRO}-gazebo-ros-pkgs + +# Move to project directory +WORKDIR ${PROJECT_PATH} + +# Source ROS2, Space Ros, Movit2 and Demo_ws +RUN /bin/bash -c 'source "/opt/ros/humble/setup.bash"' \ + && /bin/bash -c 'source "${SPACEROS_DIR}/install/setup.bash"' \ + && /bin/bash -c 'source "${MOVEIT2_DIR}/install/setup.bash"' + +RUN /bin/bash -c 'source ${DEMO_DIR}/install/setup.bash \ + && cmake -B build/linux -G "Ninja Multi-Config" -DLY_DISABLE_TEST_MODULES=ON -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -DLY_STRIP_DEBUG_SYMBOLS=ON -DAZ_USE_PHYSX5:BOOL=ON' + +RUN cmake --build build/linux --config profile --target ${PROJECT_NAME} Editor ${PROJECT_NAME}.Assets + +# Add the user to the render group so that the user can access /dev/dri/renderD128 +# /dev/dri/renderD128 gives access to hardware-accelerated rendering tool such as GPU +RUN sudo usermod -aG render $USERNAME + +# Setup the entrypoint +COPY ./entrypoint.sh / +ENTRYPOINT ["/entrypoint.sh"] +CMD ["bash"] diff --git a/o3de_mars_rover/docker/Dockerfile_space_robots b/o3de_mars_rover/docker/Dockerfile_space_robots new file mode 100644 index 00000000..9dedcbf4 --- /dev/null +++ b/o3de_mars_rover/docker/Dockerfile_space_robots @@ -0,0 +1,100 @@ +# 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="Curiosity Rover" +LABEL org.label-schema.description="Curiosity rover demo on the Space ROS platform" +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/docker" +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 +ENV DEBIAN_FRONTEND=noninteractive + +# 09/15 The patch to fix rmw conflict and control_toolbox added to moveit2 image + +# 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} demo_manual_pkgs.repos /tmp/ +# WARNING the "demo" repo is referring Mechazo11/space-ros-demos not space-ros/demos +RUN vcs import src < /tmp/demo_manual_pkgs.repos && /bin/bash -c 'source "${SPACEROS_DIR}/install/setup.bash"' + +# Update the ownership of the source files (had to use sudo above to work around +# a possible inherited 'insteadof' from the host that forces use of ssh +RUN sudo chown -R ${USERNAME}:${USERNAME} ${DEMO_DIR} + +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 "/opt/ros/humble/setup.bash"' \ +&& /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 ./entrypoint.sh / +ENTRYPOINT ["/entrypoint.sh"] +CMD ["bash"] diff --git a/o3de_mars_rover/docs/output.png b/o3de_mars_rover/docs/output.png new file mode 100644 index 00000000..1d86c6d5 Binary files /dev/null and b/o3de_mars_rover/docs/output.png differ diff --git a/o3de_mars_rover/docs/terminals.png b/o3de_mars_rover/docs/terminals.png new file mode 100644 index 00000000..511ac118 Binary files /dev/null and b/o3de_mars_rover/docs/terminals.png differ diff --git a/o3de_mars_rover/entrypoint.sh b/o3de_mars_rover/entrypoint.sh new file mode 100755 index 00000000..5eeb5475 --- /dev/null +++ b/o3de_mars_rover/entrypoint.sh @@ -0,0 +1,9 @@ +#!/bin/bash +set -e + +# Source all relevant worksapces +source "/opt/ros/humble/setup.bash" +source "${SPACEROS_DIR}/install/setup.bash" +source "${MOVEIT2_DIR}/install/setup.bash" +source "${DEMO_DIR}/install/setup.bash" +exec "$@" diff --git a/o3de_mars_rover/excluded-pkgs.txt b/o3de_mars_rover/excluded-pkgs.txt new file mode 100644 index 00000000..7a57a8ae --- /dev/null +++ b/o3de_mars_rover/excluded-pkgs.txt @@ -0,0 +1,10 @@ +fastrtps +fastcdr +generate_parameter_library +rmw_fastrtps_cpp +rmw_fastrtps_dynamic_cpp +rmw_fastrtps_shared_cpp +rmw_connextdds +rosidl_typesupport_fastrtps_c +rosidl_typesupport_fastrtps_cpp +fastrtps_cmake_module diff --git a/o3de_mars_rover/moveit2-pkgs.txt b/o3de_mars_rover/moveit2-pkgs.txt new file mode 100644 index 00000000..7aa8a566 --- /dev/null +++ b/o3de_mars_rover/moveit2-pkgs.txt @@ -0,0 +1,38 @@ +ackermann_msgs +angles +backward_ros +control_msgs +control_toolbox +eigen_stl_containers +geometric_shapes +graph_msgs +ignition/ignition_cmake2_vendor +ignition/ignition_math6_vendor +image_common +interactive_markers +joint_state_publisher +laser_geometry +launch_param_builder +moveit_msgs +moveit_resources +moveit_task_constructor +navigation_msgs +object_recognition_msgs +octomap +ompl +orocos_kinematics_dynamics +py_binding_tools +python_qt_binding +random_numbers +realtime_tools +resource_retriever +ros2_control +ros2_controllers +ruckig +rviz2 +srdfdom +urdf_parser_py +vision_opencv +warehouse_ros +xacro +yaml_cpp_vendor diff --git a/o3de_mars_rover/moveit2_tutorials.repos b/o3de_mars_rover/moveit2_tutorials.repos new file mode 100644 index 00000000..41d4774b --- /dev/null +++ b/o3de_mars_rover/moveit2_tutorials.repos @@ -0,0 +1,17 @@ +repositories: + moveit_task_constructor: + type: git + url: https://github.com/moveit/moveit_task_constructor.git + version: humble + moveit_visual_tools: + type: git + url: https://github.com/moveit/moveit_visual_tools + version: ros2 + rosparam_shortcuts: + type: git + url: https://github.com/PickNikRobotics/rosparam_shortcuts + version: ros2 + rviz_visual_tools: + type: git + url: https://github.com/PickNikRobotics/rviz_visual_tools.git + version: ros2 diff --git a/o3de_mars_rover/o3de-robot-sim-one-shot.repos b/o3de_mars_rover/o3de-robot-sim-one-shot.repos new file mode 100644 index 00000000..a7249fe6 --- /dev/null +++ b/o3de_mars_rover/o3de-robot-sim-one-shot.repos @@ -0,0 +1,107 @@ +repositories: + demos: + type: git + url: https://github.com/Mechazo11/space-ros-demos.git + version: main + 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-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 + simulation: + type: git + url: https://github.com/Mechazo11/space-ros-simulation.git + version: fix_chassis_dae + gazebo_ros_pkgs: + type: git + url: https://github.com/ros-simulation/gazebo_ros_pkgs.git + version: ros2 + image_common: + type: git + url: https://github.com/ros-perception/image_common.git + version: humble + warehouse_ros: + type: git + url: https://github.com/ros-planning/warehouse_ros.git + version: ros2 + angles: + type: git + url: https://github.com/ros/angles.git + version: ros2 + yaml_cpp_vendor: + type: git + url: https://github.com/ros2/yaml_cpp_vendor.git + version: humble + rviz: + type: git + url: https://github.com/ros2/rviz + version: humble + resource_retriever: + type: git + url: https://github.com/ros/resource_retriever.git + version: humble + control_toolbox: + type: git + url: https://github.com/ros-controls/control_toolbox.git + version: ros2-master + filters: + type: git + url: https://github.com/ros/filters.git + version: ros2 + realtime_tools: + type: git + url: https://github.com/ros-controls/realtime_tools.git + version: master + geometric_shapes: + type: git + url: https://github.com/moveit/geometric_shapes.git + version: ros2 + RSL: + type: git + url: https://github.com/PickNikRobotics/RSL.git + version: main + ros2_control: + type: git + url: https://github.com/ros-controls/ros2_control + version: humble + srdfdom: + type: git + url: https://github.com/moveit/srdfdom + version: ros2 + interactive_markers: + type: git + url: https://github.com/ros-visualization/interactive_markers + version: humble + laser_geometry: + type: git + url: https://github.com/ros-perception/laser_geometry.git + version: humble + ros_gz: + type: git + url: https://github.com/gazebosim/ros_gz.git + version: humble + diff --git a/o3de_mars_rover/o3de-robot-sim.repos b/o3de_mars_rover/o3de-robot-sim.repos new file mode 100644 index 00000000..ce00ced6 --- /dev/null +++ b/o3de_mars_rover/o3de-robot-sim.repos @@ -0,0 +1,6 @@ +repositories: + demos: + type: git + url: https://github.com/Mechazo11/space-ros-demos.git + version: main + \ No newline at end of file diff --git a/o3de_mars_rover/octomap_fix.diff b/o3de_mars_rover/octomap_fix.diff new file mode 100644 index 00000000..ea500e04 --- /dev/null +++ b/o3de_mars_rover/octomap_fix.diff @@ -0,0 +1,13 @@ +diff --git a/CMakeLists.txt b/CMakeLists.txt +index cd3112a..72403c5 100644 +--- a/CMakeLists.txt ++++ b/CMakeLists.txt +@@ -36,7 +36,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} + ) + + install(DIRECTORY include/ +- DESTINATION include ++ DESTINATION include/${PROJECT_NAME} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE) + diff --git a/o3de_mars_rover/run.sh b/o3de_mars_rover/run.sh new file mode 100755 index 00000000..309f70b3 --- /dev/null +++ b/o3de_mars_rover/run.sh @@ -0,0 +1,28 @@ +#!/usr/bin/env bash + +# Runs a docker container with the image created by build.bash +# Requires: +# docker +# an X server + +IMG_NAME=openrobotics/o3de_curiosity_docker + +# Replace `/` with `_` to comply with docker container naming +# And append `_runtime` +CONTAINER_NAME="$(tr '/' '_' <<< "$IMG_NAME")" + +# TODO this needs to be updated with +# Start the container +# docker run --rm -it --name $CONTAINER_NAME --network host \ +# -e DISPLAY -e TERM -e QT_X11_NO_MITSHM=1 $IMG_NAME + +# Will automatically remove the container once exited +docker run --rm -it --name $CONTAINER_NAME \ +--runtime nvidia \ +--network host \ +-v /tmp/.X11-unix:/tmp/.X11-unix:rw \ +-e DISPLAY=${DISPLAY} \ +-e NVIDIA_VISIBLE_DEVICES=all \ +-e NVIDIA_DRIVER_CAPABILITIES=all \ +-e DISPLAY -e TERM -e QT_X11_NO_MITSHM=1 $IMG_NAME +