├── .github └── workflows │ └── docker-image.yml ├── LICENSE ├── README.md ├── canadarm2 ├── .defaults.yaml ├── .gitignore ├── Dockerfile ├── Dockerfile.GUI ├── README.md ├── build.sh ├── canadarm_demo │ ├── CMakeLists.txt │ ├── action │ │ └── MoveJoint.action │ ├── canadarm │ │ └── __init__.py │ ├── launch │ │ └── canadarm.launch.py │ ├── nodes │ │ ├── move_arm │ │ └── move_joint_server │ └── package.xml ├── canadarm_description │ ├── CMakeLists.txt │ ├── models │ │ ├── config │ │ │ └── canadarm_control.yaml │ │ ├── meshes │ │ │ ├── ee.dae │ │ │ ├── ee.stl │ │ │ ├── ee_tex_v1_3.png │ │ │ ├── joint_v3_0.dae │ │ │ ├── joint_v3_0.stl │ │ │ ├── joint_v3_0_tex.png │ │ │ ├── land_ocean_ice_2.png │ │ │ ├── link_joint.png │ │ │ ├── link_joint_v2_1.dae │ │ │ ├── link_joint_v2_1.stl │ │ │ ├── long_link_1_v3_0.dae │ │ │ ├── long_link_1_v3_0.stl │ │ │ ├── long_link_1_v3_0_tex.png │ │ │ ├── long_link_2_v3_0.dae │ │ │ ├── long_link_2_v3_0.stl │ │ │ └── long_link_2_v3_2_tex.png │ │ ├── model.config │ │ ├── model.sdf │ │ └── urdf │ │ │ ├── SSRMS_Canadarm2.gazebo.xacro │ │ │ └── SSRMS_Canadarm2.urdf.xacro │ └── package.xml ├── canadarm_gazebo │ ├── CMakeLists.txt │ ├── launch │ │ └── canadarm.launch.py │ ├── models │ │ └── canadarm │ │ │ └── meshes │ │ │ ├── earth.dae │ │ │ ├── earth.png │ │ │ ├── earth.stl │ │ │ ├── iss.dae │ │ │ └── iss.stl │ ├── package.xml │ └── worlds │ │ └── simple.world ├── canadarm_moveit_config │ ├── .setup_assistant │ ├── CMakeLists.txt │ ├── config │ │ ├── SSRMS_Canadarm2.ros2_control.xacro │ │ ├── SSRMS_Canadarm2.srdf │ │ ├── SSRMS_Canadarm2.urdf.xacro │ │ ├── initial_positions.yaml │ │ ├── joint_limits.yaml │ │ ├── kinematics.yaml │ │ ├── moveit.rviz │ │ ├── moveit_controllers.yaml │ │ ├── pilz_cartesian_limits.yaml │ │ └── ros2_controllers.yaml │ ├── launch │ │ ├── demo.launch.py │ │ ├── move_group.launch.py │ │ ├── moveit_rviz.launch.py │ │ ├── rsp.launch.py │ │ ├── setup_assistant.launch.py │ │ ├── spawn_controllers.launch.py │ │ ├── static_virtual_joint_tfs.launch.py │ │ └── warehouse_db.launch.py │ └── package.xml ├── docker-compose.yml └── run.sh ├── curiosity_rover ├── .defaults.yaml ├── .gitignore ├── Dockerfile ├── Dockerfile.GUI ├── README.md ├── build.sh ├── curiosity_description │ ├── CMakeLists.txt │ ├── models │ │ ├── config │ │ │ └── mars_rover_control.yaml │ │ ├── meshes │ │ │ ├── CuriosityQR.stl │ │ │ ├── MSL_dirty_mod_chassis.dae │ │ │ ├── MSL_dirty_mod_chassis_fixed.dae │ │ │ ├── MSL_dirty_mod_chassis_stuff.dae │ │ │ ├── MSL_dirty_mod_chassis_stuff_fixed.dae │ │ │ ├── MSL_dirty_mod_chassis_stuff_joint_fixed.dae │ │ │ ├── MSL_dirty_mod_chassis_stuff_joint_fixed_v2.dae │ │ │ ├── arm_01.dae │ │ │ ├── arm_01_fixed.dae │ │ │ ├── arm_02.dae │ │ │ ├── arm_02_fixed.dae │ │ │ ├── arm_02_fixed2.dae │ │ │ ├── arm_03.dae │ │ │ ├── arm_03_fixed.dae │ │ │ ├── arm_04.dae │ │ │ ├── arm_04_fixed.dae │ │ │ ├── arm_tools.dae │ │ │ ├── arm_tools_fixed.dae │ │ │ ├── chassis_full.dae │ │ │ ├── chassis_full_fixed.dae │ │ │ ├── chassis_full_fixed_glitch.dae │ │ │ ├── chassis_full_fixed_glitch_v2.dae │ │ │ ├── chassis_full_fixed_v1.dae │ │ │ ├── chassis_full_fixed_v2.dae │ │ │ ├── left_axis.dae │ │ │ ├── left_axis_jointfix.dae │ │ │ ├── left_axis_jointfix_v2.dae │ │ │ ├── mast_02.dae │ │ │ ├── mast_02_fixed.dae │ │ │ ├── mast_cameras.dae │ │ │ ├── mast_p.dae │ │ │ ├── mast_p_fixed.dae │ │ │ ├── parts_AO.png │ │ │ ├── right_axis.dae │ │ │ ├── right_axis_jointfix.dae │ │ │ ├── right_axis_jointfix_v2.dae │ │ │ ├── suspension_arm_B2_L.dae │ │ │ ├── suspension_arm_B2_L_fixed.dae │ │ │ ├── suspension_arm_B2_R.dae │ │ │ ├── suspension_arm_B2_R_fixed.dae │ │ │ ├── suspension_arm_B_L.dae │ │ │ ├── suspension_arm_B_L_fixed.dae │ │ │ ├── suspension_arm_B_R.dae │ │ │ ├── suspension_arm_B_R_fixed.dae │ │ │ ├── suspension_arm_F_L.dae │ │ │ ├── suspension_arm_F_L_fixed.dae │ │ │ ├── suspension_arm_F_R.dae │ │ │ ├── suspension_arm_F_R_fixed.dae │ │ │ ├── suspension_steer_B_L.dae │ │ │ ├── suspension_steer_B_L_fixed.dae │ │ │ ├── suspension_steer_B_R.dae │ │ │ ├── suspension_steer_B_R_fixed.dae │ │ │ ├── suspension_steer_F_L.dae │ │ │ ├── suspension_steer_F_L_fixed.dae │ │ │ ├── suspension_steer_F_R.dae │ │ │ ├── suspension_steer_F_R_fixed.dae │ │ │ ├── tex_01.png │ │ │ ├── tex_02.png │ │ │ ├── tex_03.png │ │ │ ├── tex_03_n.png │ │ │ ├── tex_03_s.png │ │ │ ├── tex_04.png │ │ │ ├── tex_05.png │ │ │ ├── wheel.dae │ │ │ └── wheel_fixed.dae │ │ └── urdf │ │ │ ├── arm.xacro │ │ │ ├── chassis.xacro │ │ │ ├── curiosity_mars_rover.gazebo │ │ │ ├── curiosity_mars_rover.xacro │ │ │ ├── curiosity_mars_rover_properties.xacro │ │ │ ├── left_wheel_group.xacro │ │ │ ├── macros.xacro │ │ │ ├── right_wheel_group.xacro │ │ │ ├── sensor_mast.xacro │ │ │ └── wheel.xacro │ └── package.xml ├── curiosity_gazebo │ ├── CMakeLists.txt │ ├── config │ │ └── mars_rover_control.yaml │ ├── curiosity_gazebo │ │ └── __init__.py │ ├── launch │ │ └── curiosity_gazebo.launch.py │ ├── models │ │ └── curiosity_path │ │ │ ├── meshes │ │ │ ├── curiosity_path.stl │ │ │ └── mars_path_simple1.dae │ │ │ ├── model.config │ │ │ └── model.sdf │ ├── nodes │ │ └── odom_tf_publisher │ ├── package.xml │ └── worlds │ │ └── mars_curiosity.world ├── curiosity_rover_demo │ ├── CMakeLists.txt │ ├── curiosity_rover_demo │ │ └── __init__.py │ ├── launch │ │ └── mars_rover.launch.py │ ├── nodes │ │ ├── move_arm │ │ ├── move_mast │ │ ├── move_wheel │ │ ├── run_demo │ │ └── test_node │ └── package.xml ├── docker-compose.yml └── run.sh ├── ros_trick ├── .gitignore ├── LICENSE ├── README.md ├── build.sh ├── canadarm_ros_trick_demo.Dockerfile ├── docker-compose.yml ├── docs │ ├── moveit_rviz.png │ ├── ros_trick_bridge_architecture.png │ ├── rviz_joints.png │ ├── rviz_motion.png │ ├── trick_main.png │ ├── trick_velocity_plot.png │ └── trick_view_variables.png ├── ros_src │ ├── canadarm_wrench_publisher │ │ ├── canadarm_wrench_publisher │ │ │ ├── __init__.py │ │ │ └── canadarm_wrench_publisher_node.py │ │ ├── package.xml │ │ ├── resource │ │ │ └── canadarm_wrench_publisher │ │ ├── setup.cfg │ │ └── setup.py │ ├── ros_trick_bridge │ │ ├── launch │ │ │ └── ros_trick_bridge.launch.py │ │ ├── package.xml │ │ ├── params │ │ │ └── ros_trick_bridge_example_params.yaml │ │ ├── resource │ │ │ └── ros_trick_bridge │ │ ├── ros_trick_bridge │ │ │ ├── __init__.py │ │ │ ├── ros_trick_bridge_node.py │ │ │ └── trick_variable_server_client.py │ │ ├── setup.cfg │ │ └── setup.py │ ├── trick_canadarm_moveit_config │ │ ├── .setup_assistant │ │ ├── CMakeLists.txt │ │ ├── config │ │ │ ├── SSRMS_Canadarm2.ros2_control.xacro │ │ │ ├── SSRMS_Canadarm2.srdf │ │ │ ├── SSRMS_Canadarm2.urdf.xacro │ │ │ ├── initial_positions.yaml │ │ │ ├── joint_limits.yaml │ │ │ ├── kinematics.yaml │ │ │ ├── moveit.rviz │ │ │ ├── moveit_controllers.yaml │ │ │ ├── pilz_cartesian_limits.yaml │ │ │ └── ros2_controllers.yaml │ │ ├── launch │ │ │ ├── demo.launch.py │ │ │ ├── move_group.launch.py │ │ │ ├── moveit_rviz.launch.py │ │ │ ├── rsp.launch.py │ │ │ ├── setup_assistant.launch.py │ │ │ ├── spawn_controllers.launch.py │ │ │ ├── static_virtual_joint_tfs.launch.py │ │ │ └── warehouse_db.launch.py │ │ └── package.xml │ └── trick_ros2_control │ │ ├── CMakeLists.txt │ │ ├── include │ │ └── trick_ros2_control │ │ │ ├── socket.hpp │ │ │ ├── trick_system.hpp │ │ │ ├── utils.hpp │ │ │ └── visibility_control.h │ │ ├── package.xml │ │ ├── src │ │ ├── socket.cpp │ │ └── trick_system.cpp │ │ └── trick_ros2_control.xml ├── ros_trick_bridge.Dockerfile ├── run.sh ├── spaceros_docker_repo.patch └── trick_src │ └── SIM_trick_canadarm │ ├── RUN_2DPlanar │ └── input.py │ ├── S_define │ ├── S_overrides.mk │ └── models │ └── manipulator │ ├── manipulator.cc │ └── manipulator.hh └── space_ros_memory_allocation_demo ├── CMakeLists.txt ├── COLCON_IGNORE ├── LICENSE ├── include └── space_ros_memory_allocation_demo │ ├── memory_allocator.hpp │ ├── memory_resource_helper.hpp │ └── visibility_control.h ├── launch └── talker_listener.launch.xml ├── package.xml └── src ├── common_main.hpp ├── expected_push_pop.hpp ├── listener.hpp ├── listener_library.cpp ├── listener_main.cpp ├── talker.hpp ├── talker_library.cpp └── talker_main.cpp /.github/workflows/docker-image.yml: -------------------------------------------------------------------------------- 1 | name: Docker Image CI 2 | 3 | on: 4 | push: 5 | branches: [ "main" ] 6 | pull_request: 7 | branches: [ "main" ] 8 | 9 | jobs: 10 | 11 | build: 12 | 13 | runs-on: ubuntu-latest 14 | 15 | steps: 16 | - name: Checkout 17 | uses: actions/checkout@v3 18 | - name: Run the build.sh scripts 19 | run: | 20 | # find the build.sh scripts and execute them 21 | echo "Running all build.sh scripts in repo" 22 | path=$(pwd) 23 | for f in $(find . -iname 'build.sh') 24 | do 25 | (cd $(dirname $f) && ./build.sh && cd $path) || (echo $f "Failed" && exit 1) 26 | done 27 | echo "Done running build.sh scripts" 28 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Space ROS Demos 2 | 3 | This repository provides examples for running robots using Space ROS 4 | 5 | Submissions to this repo should include: 6 | 1) A Dockerfile or docker-compose.yaml file for building on top of the `osrf/space-ros:latest` 7 | 2) A `build.sh` script for building the docker image 8 | - This is required for CI to build the image 9 | - See example here: https://github.com/space-ros/docker/blob/main/space_robots/build.sh 10 | 3) A `run.sh` script for running the demo 11 | - - See example here: https://github.com/space-ros/docker/blob/main/space_robots/run.sh 12 | 13 | Please refer to the [dockerfile repo](https://github.com/space-ros/docker/tree/main/space_robots) for instructions on running the existing demos 14 | 15 | ## Demos 16 | 17 | 1. [Canadarm2](canadarm2/README.md) 18 | 2. [Curiosity Rover](curiosity_rover/README.md) 19 | -------------------------------------------------------------------------------- /canadarm2/.defaults.yaml: -------------------------------------------------------------------------------- 1 | { 2 | "build": 3 | { 4 | "symlink-install": true, 5 | "cmake_args": "-DCMAKE_BUILD_TYPE=Release -Wno-dev", 6 | }, 7 | } 8 | -------------------------------------------------------------------------------- /canadarm2/.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | install/ 3 | log/ 4 | -------------------------------------------------------------------------------- /canadarm2/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM osrf/space-ros:jazzy-2025.04.0 2 | 3 | ENV RMW_IMPLEMENTATION=rmw_cyclonedds_cpp 4 | ENV ROS_DISTRO=jazzy 5 | ENV HOME=/home/spaceros-user 6 | 7 | # Install dependencies 8 | # Related to space-ros/space-ros#195 and space-ros/space-ros#196 9 | RUN sudo apt update && sudo apt install -y ros-${ROS_DISTRO}-control-msgs \ 10 | ros-${ROS_DISTRO}-rmw-cyclonedds-cpp 11 | 12 | # Prepare the workspace 13 | SHELL ["bash", "-c"] 14 | RUN mkdir -p ${HOME}/canadarm_ws/src 15 | WORKDIR ${HOME}/canadarm_ws 16 | 17 | COPY ./canadarm_demo src/canadarm_demo 18 | COPY .defaults.yaml .defaults.yaml 19 | 20 | # Build the workspace 21 | RUN source /opt/ros/${ROS_DISTRO}/setup.bash \ 22 | && source "${SPACEROS_DIR}"/install/setup.bash \ 23 | && colcon build 24 | 25 | # Source the workspace 26 | RUN echo "source ${HOME}/canadarm_ws/install/setup.bash" >> ${HOME}/.bashrc 27 | -------------------------------------------------------------------------------- /canadarm2/Dockerfile.GUI: -------------------------------------------------------------------------------- 1 | FROM ros:jazzy-ros-core-noble 2 | 3 | ARG DEBIAN_FRONTEND=noninteractive 4 | 5 | ENV RMW_IMPLEMENTATION=rmw_cyclonedds_cpp 6 | ENV ROS_DISTRO=jazzy 7 | ENV USERNAME=spaceros-user 8 | ENV HOME=/home/spaceros-user 9 | ENV GZ_VERSION=harmonic 10 | 11 | # Install dependencies 12 | RUN apt update && apt install -y ros-${ROS_DISTRO}-rmw-cyclonedds-cpp \ 13 | wget \ 14 | curl \ 15 | && apt clean \ 16 | && rm -rf /var/lib/apt/lists/* 17 | 18 | RUN wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg\ 19 | && echo "deb [arch=amd64 signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" | tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null 20 | 21 | # Install Gazebo 22 | RUN apt-get update -qq \ 23 | && apt-get install -y \ 24 | gz-${GZ_VERSION} \ 25 | libgz-sim8 \ 26 | libgz-transport13 \ 27 | libgz-gui8 \ 28 | build-essential\ 29 | ros-${ROS_DISTRO}-rcl-interfaces\ 30 | ros-${ROS_DISTRO}-rclcpp\ 31 | ros-${ROS_DISTRO}-builtin-interfaces\ 32 | ros-${ROS_DISTRO}-ros-gz\ 33 | ros-${ROS_DISTRO}-sdformat-urdf\ 34 | ros-${ROS_DISTRO}-vision-msgs\ 35 | ros-${ROS_DISTRO}-actuator-msgs\ 36 | ros-${ROS_DISTRO}-image-transport\ 37 | ros-${ROS_DISTRO}-xacro\ 38 | && rm -rf /var/lib/apt/lists/* 39 | 40 | 41 | # Install Rviz2 42 | RUN apt-get update -qq \ 43 | && apt-get install -y \ 44 | ros-${ROS_DISTRO}-rviz2 \ 45 | ros-${ROS_DISTRO}-moveit-ros-visualization \ 46 | ros-${ROS_DISTRO}-nav2-rviz-plugins \ 47 | && rm -rf /var/lib/apt/lists/* 48 | 49 | # Demo package 50 | RUN mkdir -p ${HOME}/canadarm2_ws/src 51 | WORKDIR ${HOME}/canadarm2_ws/src 52 | COPY ./canadarm_description ./canadarm_description 53 | COPY ./canadarm_gazebo ./canadarm_gazebo 54 | COPY .defaults.yaml ${HOME}/canadarm2_ws/.defaults.yaml 55 | 56 | RUN apt update \ 57 | && apt install -y python3-rosdep python3-colcon-common-extensions \ 58 | && apt clean \ 59 | && rm -rf /var/lib/apt/lists/* \ 60 | && rosdep init \ 61 | && rosdep update 62 | 63 | RUN . /opt/ros/jazzy/setup.sh \ 64 | && cd ${HOME}/canadarm2_ws \ 65 | && apt update \ 66 | && rosdep install --from-paths src --ignore-src -r -y \ 67 | && colcon build 68 | 69 | WORKDIR ${HOME}/canadarm2_ws 70 | 71 | RUN apt update && apt install -y gz-harmonic 72 | RUN echo "source /opt/ros/jazzy/setup.bash" >> ${HOME}/.bashrc 73 | RUN echo "source ${HOME}/canadarm2_ws/install/setup.bash" >> ${HOME}/.bashrc 74 | 75 | CMD gz sim 76 | -------------------------------------------------------------------------------- /canadarm2/README.md: -------------------------------------------------------------------------------- 1 | # SSRMS CanadArm2 - Demo 2 | 3 | This is a simple demo of controlling the canadarm using spaceROS. 4 | 5 | ### Installation 6 | 7 | To start the demo, there are few dependencies that need to be installed. The following steps will guide you through the installation process. 8 | 9 | 1. You will need docker installed on your system. If not, you can follow the instructions [here](https://docs.docker.com/get-docker/). 10 | 11 | ### How to run the demo 12 | 13 | 1. Clone the demo repository 14 | ```bash 15 | git clone https://github.com/space-ros/demos.git 16 | ``` 17 | 2. cd into the `canadarm2` directory 18 | ```bash 19 | cd canadarm2 20 | ``` 21 | 3. To build the demo, you can use the following command. 22 | ```bash 23 | # To build the demos for the canadarm2 24 | ./build.sh 25 | ``` 26 | 4. To run the demo, you can use the following command. 27 | ```bash 28 | # To run the demos for the canadarm2 29 | ./run.sh 30 | ``` 31 | 32 | This will start the demo in one terminal and gazebo in another terminal. To control the canadarm2, we provide ros2 services for the demo. You can control the rover using the following services. 33 | 34 | 1. Control arm of the canadarm 35 | 36 | ```bash 37 | # To open an arm in outstretched position 38 | ros2 service call /open_arm std_srvs/srv/Empty 39 | 40 | # To move the arm to its default close pose of all zeros 41 | ros2 service call /close_arm std_srvs/srv/Empty 42 | 43 | # To move the arm to a random pose 44 | ros2 service call /random_arm std_srvs/srv/Empty 45 | ``` 46 | -------------------------------------------------------------------------------- /canadarm2/build.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -e 4 | 5 | docker compose build 6 | -------------------------------------------------------------------------------- /canadarm2/canadarm_demo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.22) 2 | project(canadarm_demo) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(ament_cmake_python REQUIRED) 11 | find_package(rclpy REQUIRED) 12 | find_package(rosidl_default_generators REQUIRED) 13 | 14 | rosidl_generate_interfaces(${PROJECT_NAME} 15 | "action/MoveJoint.action" 16 | ) 17 | 18 | 19 | if(BUILD_TESTING) 20 | find_package(ament_lint_auto REQUIRED) 21 | # the following line skips the linter which checks for copyrights 22 | # comment the line when a copyright and license is added to all source files 23 | set(ament_cmake_copyright_FOUND TRUE) 24 | # the following line skips cpplint (only works in a git repo) 25 | # comment the line when this package is in a git repo and when 26 | # a copyright and license is added to all source files 27 | set(ament_cmake_cpplint_FOUND TRUE) 28 | ament_lint_auto_find_test_dependencies() 29 | endif() 30 | 31 | 32 | install(DIRECTORY 33 | action 34 | launch 35 | DESTINATION share/${PROJECT_NAME} 36 | ) 37 | 38 | 39 | install(PROGRAMS 40 | nodes/move_joint_server 41 | nodes/move_arm 42 | DESTINATION lib/${PROJECT_NAME} 43 | ) 44 | 45 | 46 | ament_package() 47 | -------------------------------------------------------------------------------- /canadarm2/canadarm_demo/action/MoveJoint.action: -------------------------------------------------------------------------------- 1 | float32[7] target 2 | --- 3 | int32 result 4 | --- 5 | float32[7] joint_states -------------------------------------------------------------------------------- /canadarm2/canadarm_demo/canadarm/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/space-ros/demos/d82db232718d4a8e07f593c54f1f2137a84c0886/canadarm2/canadarm_demo/canadarm/__init__.py -------------------------------------------------------------------------------- /canadarm2/canadarm_demo/launch/canadarm.launch.py: -------------------------------------------------------------------------------- 1 | """Canadarm2 demo launch file.""" 2 | 3 | from launch import LaunchDescription # type: ignore 4 | 5 | from launch_ros.actions import Node # type: ignore 6 | 7 | 8 | def generate_launch_description(): 9 | """Generate launch description with multiple components.""" 10 | 11 | # run_node = Node( 12 | # package="canadarm", 13 | # executable="move_joint_server", 14 | # output='screen' 15 | # ) 16 | 17 | run_move_arm = Node(package="canadarm_demo", executable="move_arm", output="screen") 18 | 19 | return LaunchDescription( 20 | [ 21 | # run_node, 22 | run_move_arm, 23 | ] 24 | ) 25 | -------------------------------------------------------------------------------- /canadarm2/canadarm_demo/nodes/move_arm: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | """Move the arm to a specific position.""" 3 | 4 | import rclpy # type: ignore 5 | from rclpy.node import Node # type: ignore 6 | from builtin_interfaces.msg import Duration # type: ignore 7 | 8 | from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint # type: ignore 9 | from std_srvs.srv import Empty # type: ignore 10 | 11 | 12 | class MoveArm(Node): 13 | """Move the arm to a specific position.""" 14 | 15 | def __init__(self): 16 | super().__init__("arm_node") 17 | self.arm_publisher_ = self.create_publisher( 18 | JointTrajectory, 19 | "/canadarm_joint_trajectory_controller/joint_trajectory", 20 | 10, 21 | ) 22 | self.open_srv = self.create_service(Empty, "open_arm", self.open_arm_callback) 23 | self.close_srv = self.create_service( 24 | Empty, "close_arm", self.close_arm_callback 25 | ) 26 | self.random_srv = self.create_service( 27 | Empty, "random_arm", self.random_arm_callback 28 | ) 29 | 30 | def open_arm_callback(self, request, response): # pylint: disable=unused-argument 31 | """Open the arm.""" 32 | traj = JointTrajectory() 33 | traj.joint_names = [ 34 | "Base_Joint", 35 | "Shoulder_Roll", 36 | "Shoulder_Yaw", 37 | "Elbow_Pitch", 38 | "Wrist_Pitch", 39 | "Wrist_Yaw", 40 | "Wrist_Roll", 41 | ] 42 | 43 | point1 = JointTrajectoryPoint() 44 | point1.positions = [0.0, 0.0, 0.0, -3.1416, 0.0, 0.0, 0.0] 45 | point1.time_from_start = Duration(sec=4) 46 | 47 | traj.points.append(point1) 48 | self.arm_publisher_.publish(traj) 49 | 50 | return response 51 | 52 | def close_arm_callback(self, request, response): # pylint: disable=unused-argument 53 | """Close the arm.""" 54 | traj = JointTrajectory() 55 | traj.joint_names = [ 56 | "Base_Joint", 57 | "Shoulder_Roll", 58 | "Shoulder_Yaw", 59 | "Elbow_Pitch", 60 | "Wrist_Pitch", 61 | "Wrist_Yaw", 62 | "Wrist_Roll", 63 | ] 64 | 65 | point1 = JointTrajectoryPoint() 66 | point1.positions = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 67 | point1.time_from_start = Duration(sec=4) 68 | 69 | traj.points.append(point1) 70 | self.arm_publisher_.publish(traj) 71 | 72 | return response 73 | 74 | def random_arm_callback(self, request, response): # pylint: disable=unused-argument 75 | """Move the arm to a random position.""" 76 | traj = JointTrajectory() 77 | traj.joint_names = [ 78 | "Base_Joint", 79 | "Shoulder_Roll", 80 | "Shoulder_Yaw", 81 | "Elbow_Pitch", 82 | "Wrist_Pitch", 83 | "Wrist_Yaw", 84 | "Wrist_Roll", 85 | ] 86 | 87 | point1 = JointTrajectoryPoint() 88 | point1.positions = [1.0, -1.5, 2.0, -3.2, 0.8, 0.5, -1.0] 89 | point1.time_from_start = Duration(sec=4) 90 | 91 | traj.points.append(point1) 92 | self.arm_publisher_.publish(traj) 93 | 94 | return response 95 | 96 | 97 | def main(args=None): 98 | """Main function.""" 99 | rclpy.init(args=args) 100 | 101 | arm_node = MoveArm() 102 | 103 | rclpy.spin(arm_node) 104 | # Destroy the node explicitly 105 | # (optional - otherwise it will be done automatically 106 | # when the garbage collector destroys the node object) 107 | arm_node.destroy_node() 108 | rclpy.shutdown() 109 | 110 | 111 | if __name__ == "__main__": 112 | main() 113 | -------------------------------------------------------------------------------- /canadarm2/canadarm_demo/nodes/move_joint_server: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | """Create an action server to move the arm to a specific position.""" 3 | 4 | import rclpy # type: ignore 5 | from rclpy.action import ActionServer # type: ignore 6 | from rclpy.node import Node # type: ignore 7 | 8 | from std_msgs.msg import Float64MultiArray # type: ignore 9 | 10 | from canadarm_demo.action import MoveJoint 11 | 12 | 13 | class MoveJointActionServer(Node): 14 | """Action server to move the arm to a specific position.""" 15 | 16 | def __init__(self): 17 | super().__init__("move_joint_node") 18 | self.arm_publisher_ = self.create_publisher( 19 | Float64MultiArray, "canadarm_joint_controller/commands", 10 20 | ) 21 | 22 | self._action_server = ActionServer( 23 | self, MoveJoint, "movejoint", self.movejoint_callback 24 | ) 25 | 26 | timer_period = 0.01 # seconds 27 | self.timer = self.create_timer(timer_period, self.timer_callback) 28 | 29 | self.goal_joint_pos = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 30 | 31 | def movejoint_callback(self, goal_handle): 32 | """Move the arm to a specific position.""" 33 | self.get_logger().info("Executing joint goal...") 34 | self.goal_joint_pos = [float(i) for i in goal_handle.request.target] 35 | goal_handle.succeed() 36 | result = MoveJoint.Result() 37 | return result 38 | 39 | def move_joint(self): 40 | """Move the arm to the goal position.""" 41 | target_val = Float64MultiArray() 42 | target_val.data = self.goal_joint_pos 43 | self.arm_publisher_.publish(target_val) 44 | 45 | def timer_callback(self): 46 | """Timer callback to move the arm to the goal position.""" 47 | self.move_joint() 48 | 49 | 50 | def main(args=None): 51 | """Main function.""" 52 | rclpy.init(args=args) 53 | 54 | move_joint_node = MoveJointActionServer() 55 | 56 | rclpy.spin(move_joint_node) 57 | 58 | move_joint_node.destroy_node() 59 | rclpy.shutdown() 60 | 61 | 62 | if __name__ == "__main__": 63 | main() 64 | -------------------------------------------------------------------------------- /canadarm2/canadarm_demo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | canadarm_demo 5 | 1.0.0 6 | CanadArm2 Control demo using SpaceROS 7 | root 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | ament_cmake_python 12 | rosidl_default_generators 13 | 14 | rclcpp 15 | rclpy 16 | action_msgs 17 | 18 | ament_index_python 19 | geometry_msgs 20 | launch 21 | launch_ros 22 | std_msgs 23 | 24 | rosidl_interface_packages 25 | 26 | 27 | ament_lint_auto 28 | ament_lint_common 29 | ament_cmake 30 | 31 | 32 | -------------------------------------------------------------------------------- /canadarm2/canadarm_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.22) 2 | project(canadarm_description) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | 11 | install(DIRECTORY 12 | ${CMAKE_CURRENT_SOURCE_DIR}/models 13 | DESTINATION share/${PROJECT_NAME}/ 14 | ) 15 | 16 | if(BUILD_TESTING) 17 | find_package(ament_lint_auto REQUIRED) 18 | # the following line skips the linter which checks for copyrights 19 | # comment the line when a copyright and license is added to all source files 20 | set(ament_cmake_copyright_FOUND TRUE) 21 | # the following line skips cpplint (only works in a git repo) 22 | # comment the line when this package is in a git repo and when 23 | # a copyright and license is added to all source files 24 | set(ament_cmake_cpplint_FOUND TRUE) 25 | ament_lint_auto_find_test_dependencies() 26 | endif() 27 | 28 | ament_package() 29 | -------------------------------------------------------------------------------- /canadarm2/canadarm_description/models/config/canadarm_control.yaml: -------------------------------------------------------------------------------- 1 | controller_manager: 2 | ros__parameters: 3 | update_rate: 100 4 | 5 | canadarm_joint_controller: 6 | type: effort_controllers/JointGroupPositionController 7 | 8 | canadarm_joint_trajectory_controller: 9 | type: joint_trajectory_controller/JointTrajectoryController 10 | 11 | joint_state_broadcaster: 12 | type: joint_state_broadcaster/JointStateBroadcaster 13 | 14 | canadarm_joint_trajectory_controller: 15 | ros__parameters: 16 | joints: 17 | - Base_Joint 18 | - Shoulder_Roll 19 | - Shoulder_Yaw 20 | - Elbow_Pitch 21 | - Wrist_Pitch 22 | - Wrist_Yaw 23 | - Wrist_Roll 24 | interface_name: position 25 | command_interfaces: 26 | - position 27 | state_interfaces: 28 | - position 29 | - velocity 30 | 31 | canadarm_joint_controller: 32 | ros__parameters: 33 | joints: 34 | - Base_Joint 35 | - Shoulder_Roll 36 | - Shoulder_Yaw 37 | - Elbow_Pitch 38 | - Wrist_Pitch 39 | - Wrist_Yaw 40 | - Wrist_Roll 41 | command_interfaces: 42 | - effort 43 | state_interfaces: 44 | - position 45 | - velocity 46 | - effort 47 | gains: 48 | Base_Joint: 49 | p: 50.0 50 | i: 100.0 51 | d: 2000.0 52 | Shoulder_Roll: 53 | p: 50.0 54 | i: 100.0 55 | d: 2000.0 56 | Shoulder_Yaw: 57 | p: 50.0 58 | i: 100.0 59 | d: 2000.0 60 | Elbow_Pitch: 61 | p: 50.0 62 | i: 100.0 63 | d: 2000.0 64 | Wrist_Pitch: 65 | p: 50.0 66 | i: 100.0 67 | d: 2000.0 68 | Wrist_Yaw: 69 | p: 50.0 70 | i: 10.0 71 | d: 200.0 72 | Wrist_Roll: 73 | p: 50.0 74 | i: 10.0 75 | d: 200.0 76 | -------------------------------------------------------------------------------- /canadarm2/canadarm_description/models/meshes/ee.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/space-ros/demos/d82db232718d4a8e07f593c54f1f2137a84c0886/canadarm2/canadarm_description/models/meshes/ee.stl -------------------------------------------------------------------------------- /canadarm2/canadarm_description/models/meshes/ee_tex_v1_3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/space-ros/demos/d82db232718d4a8e07f593c54f1f2137a84c0886/canadarm2/canadarm_description/models/meshes/ee_tex_v1_3.png -------------------------------------------------------------------------------- /canadarm2/canadarm_description/models/meshes/joint_v3_0.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/space-ros/demos/d82db232718d4a8e07f593c54f1f2137a84c0886/canadarm2/canadarm_description/models/meshes/joint_v3_0.stl -------------------------------------------------------------------------------- /canadarm2/canadarm_description/models/meshes/joint_v3_0_tex.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/space-ros/demos/d82db232718d4a8e07f593c54f1f2137a84c0886/canadarm2/canadarm_description/models/meshes/joint_v3_0_tex.png -------------------------------------------------------------------------------- /canadarm2/canadarm_description/models/meshes/land_ocean_ice_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/space-ros/demos/d82db232718d4a8e07f593c54f1f2137a84c0886/canadarm2/canadarm_description/models/meshes/land_ocean_ice_2.png -------------------------------------------------------------------------------- /canadarm2/canadarm_description/models/meshes/link_joint.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/space-ros/demos/d82db232718d4a8e07f593c54f1f2137a84c0886/canadarm2/canadarm_description/models/meshes/link_joint.png -------------------------------------------------------------------------------- /canadarm2/canadarm_description/models/meshes/link_joint_v2_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/space-ros/demos/d82db232718d4a8e07f593c54f1f2137a84c0886/canadarm2/canadarm_description/models/meshes/link_joint_v2_1.stl -------------------------------------------------------------------------------- /canadarm2/canadarm_description/models/meshes/long_link_1_v3_0.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/space-ros/demos/d82db232718d4a8e07f593c54f1f2137a84c0886/canadarm2/canadarm_description/models/meshes/long_link_1_v3_0.stl -------------------------------------------------------------------------------- /canadarm2/canadarm_description/models/meshes/long_link_1_v3_0_tex.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/space-ros/demos/d82db232718d4a8e07f593c54f1f2137a84c0886/canadarm2/canadarm_description/models/meshes/long_link_1_v3_0_tex.png -------------------------------------------------------------------------------- /canadarm2/canadarm_description/models/meshes/long_link_2_v3_0.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/space-ros/demos/d82db232718d4a8e07f593c54f1f2137a84c0886/canadarm2/canadarm_description/models/meshes/long_link_2_v3_0.stl -------------------------------------------------------------------------------- /canadarm2/canadarm_description/models/meshes/long_link_2_v3_2_tex.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/space-ros/demos/d82db232718d4a8e07f593c54f1f2137a84c0886/canadarm2/canadarm_description/models/meshes/long_link_2_v3_2_tex.png -------------------------------------------------------------------------------- /canadarm2/canadarm_description/models/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | model.sdf 4 | -------------------------------------------------------------------------------- /canadarm2/canadarm_description/models/urdf/SSRMS_Canadarm2.gazebo.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | gz_ros2_control/GazeboSimSystem 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | robot_description 58 | robot_state_publisher 59 | $(find canadarm_description)/models/config/canadarm_control.yaml 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | -------------------------------------------------------------------------------- /canadarm2/canadarm_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | canadarm_description 5 | 1.0.0 6 | Simulation Description for Canadarm2 7 | franklinselva 8 | GPL 9 | 10 | ament_cmake 11 | 12 | ament_lint_auto 13 | ament_lint_common 14 | 15 | 16 | ament_cmake 17 | 18 | 19 | -------------------------------------------------------------------------------- /canadarm2/canadarm_gazebo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.22) 2 | project(canadarm_gazebo) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(canadarm_description REQUIRED) 11 | 12 | install(DIRECTORY 13 | launch 14 | models 15 | worlds 16 | DESTINATION share/${PROJECT_NAME} 17 | ) 18 | 19 | if(BUILD_TESTING) 20 | find_package(ament_lint_auto REQUIRED) 21 | # the following line skips the linter which checks for copyrights 22 | # comment the line when a copyright and license is added to all source files 23 | set(ament_cmake_copyright_FOUND TRUE) 24 | # the following line skips cpplint (only works in a git repo) 25 | # comment the line when this package is in a git repo and when 26 | # a copyright and license is added to all source files 27 | set(ament_cmake_cpplint_FOUND TRUE) 28 | ament_lint_auto_find_test_dependencies() 29 | endif() 30 | 31 | ament_package() 32 | -------------------------------------------------------------------------------- /canadarm2/canadarm_gazebo/launch/canadarm.launch.py: -------------------------------------------------------------------------------- 1 | """Canadarm2 Gazebo launch file.""" 2 | 3 | import os 4 | from launch import LaunchDescription # type: ignore 5 | from launch.actions import ExecuteProcess, RegisterEventHandler, IncludeLaunchDescription, SetEnvironmentVariable # type: ignore 6 | from launch.event_handlers import OnProcessExit # type: ignore 7 | from launch_ros.actions import Node # type: ignore 8 | from launch.substitutions import PathJoinSubstitution 9 | 10 | from ament_index_python.packages import get_package_share_directory # type: ignore 11 | 12 | import xacro # type: ignore 13 | 14 | 15 | def generate_launch_description(): 16 | """Generate launch description with multiple components.""" 17 | 18 | canadarm_gazebo_path = get_package_share_directory("canadarm_gazebo") 19 | 20 | simulation_models_path = get_package_share_directory("canadarm_description") 21 | 22 | env_gz_plugin = SetEnvironmentVariable('GZ_SIM_SYSTEM_PLUGIN_PATH', 23 | os.pathsep.join( 24 | [ 25 | os.environ.get("GZ_SIM_SYSTEM_PLUGIN_PATH", default=""), 26 | os.environ.get("LD_LIBRARY_PATH", default="") 27 | ] 28 | )) 29 | env_gz_resource = SetEnvironmentVariable("GZ_SIM_RESOURCE_PATH", 30 | os.pathsep.join( 31 | [ 32 | os.environ.get("GZ_SIM_RESOURCE_PATH", default=""), 33 | canadarm_gazebo_path + "/models", 34 | ] 35 | )) 36 | 37 | 38 | urdf_model_path = os.path.join( 39 | simulation_models_path, 40 | "models", 41 | "urdf", 42 | "SSRMS_Canadarm2.urdf.xacro", 43 | ) 44 | leo_model = os.path.join(canadarm_gazebo_path, "worlds", "simple.world") 45 | 46 | doc = xacro.process_file( 47 | urdf_model_path, mappings={"xyz": "1.0 0.0 1.5", "rpy": "3.1416 0.0 0.0"} 48 | ) 49 | robot_description = {"robot_description": doc.toxml()} 50 | 51 | start_world = IncludeLaunchDescription( 52 | PathJoinSubstitution([get_package_share_directory('ros_gz_sim'), 'launch', 'gz_sim.launch.py']), 53 | launch_arguments = [ 54 | ('gz_args', [ 55 | leo_model, 56 | ' -r', 57 | ' -v 4' 58 | ]) 59 | ] 60 | ) 61 | 62 | robot_state_publisher = Node( 63 | package="robot_state_publisher", 64 | executable="robot_state_publisher", 65 | name="robot_state_publisher", 66 | output="screen", 67 | parameters=[robot_description], 68 | ) 69 | 70 | spawn = Node( 71 | package="ros_gz_sim", 72 | executable="create", 73 | arguments=[ 74 | "-name", 75 | "canadarm", 76 | "-topic", 77 | robot_description, 78 | ], 79 | output="screen", 80 | ) 81 | 82 | # Control 83 | load_joint_state_broadcaster = ExecuteProcess( 84 | cmd=[ 85 | "ros2", 86 | "control", 87 | "load_controller", 88 | "--set-state", 89 | "active", 90 | "joint_state_broadcaster", 91 | ], 92 | output="screen", 93 | ) 94 | 95 | load_canadarm_joint_controller = ExecuteProcess( 96 | cmd=[ 97 | "ros2", 98 | "control", 99 | "load_controller", 100 | "--set-state", 101 | "active", 102 | "canadarm_joint_trajectory_controller", 103 | ], 104 | output="screen", 105 | ) 106 | 107 | gz_sim_bridge = Node( 108 | package="ros_gz_bridge", 109 | executable="parameter_bridge", 110 | arguments=[ 111 | "/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock", 112 | ], 113 | output="screen", 114 | ) 115 | 116 | return LaunchDescription( 117 | [ 118 | env_gz_plugin, 119 | env_gz_resource, 120 | start_world, 121 | robot_state_publisher, 122 | spawn, 123 | RegisterEventHandler( 124 | OnProcessExit( 125 | target_action=spawn, 126 | on_exit=[load_joint_state_broadcaster], 127 | ) 128 | ), 129 | RegisterEventHandler( 130 | OnProcessExit( 131 | target_action=load_joint_state_broadcaster, 132 | on_exit=[load_canadarm_joint_controller], 133 | ) 134 | ), 135 | gz_sim_bridge 136 | ] 137 | ) 138 | 139 | -------------------------------------------------------------------------------- /canadarm2/canadarm_gazebo/models/canadarm/meshes/earth.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/space-ros/demos/d82db232718d4a8e07f593c54f1f2137a84c0886/canadarm2/canadarm_gazebo/models/canadarm/meshes/earth.png -------------------------------------------------------------------------------- /canadarm2/canadarm_gazebo/models/canadarm/meshes/earth.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/space-ros/demos/d82db232718d4a8e07f593c54f1f2137a84c0886/canadarm2/canadarm_gazebo/models/canadarm/meshes/earth.stl -------------------------------------------------------------------------------- /canadarm2/canadarm_gazebo/models/canadarm/meshes/iss.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/space-ros/demos/d82db232718d4a8e07f593c54f1f2137a84c0886/canadarm2/canadarm_gazebo/models/canadarm/meshes/iss.stl -------------------------------------------------------------------------------- /canadarm2/canadarm_gazebo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | canadarm_gazebo 5 | 0.1.0 6 | Simulation description for CanadArm2 7 | franklinselva 8 | GPL 9 | 10 | ament_cmake 11 | canadarm_description 12 | ros_gz 13 | 14 | gz_ros2_control 15 | joint_state_broadcaster 16 | robot_state_publisher 17 | control_msgs 18 | ros2_control 19 | ros2_controllers 20 | xacro 21 | 22 | ament_lint_auto 23 | ament_lint_common 24 | 25 | ament_lint_auto 26 | ament_lint_common 27 | 28 | 29 | ament_cmake 30 | 31 | 32 | -------------------------------------------------------------------------------- /canadarm2/canadarm_moveit_config/.setup_assistant: -------------------------------------------------------------------------------- 1 | moveit_setup_assistant_config: 2 | urdf: 3 | package: simulation 4 | relative_path: models/canadarm/urdf/SSRMS_Canadarm2.urdf 5 | srdf: 6 | relative_path: config/SSRMS_Canadarm2.srdf 7 | package_settings: 8 | author_name: Dharini Dutia 9 | author_email: dharini@openrobotics.org 10 | generated_timestamp: 1672757808 11 | control_xacro: 12 | command: 13 | - position 14 | - velocity 15 | state: 16 | - position 17 | - velocity 18 | modified_urdf: 19 | xacros: 20 | - control_xacro 21 | control_xacro: 22 | command: 23 | - position 24 | - velocity 25 | state: 26 | - position 27 | - velocity -------------------------------------------------------------------------------- /canadarm2/canadarm_moveit_config/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.22) 2 | project(canadarm_moveit_config) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | 6 | ament_package() 7 | 8 | install(DIRECTORY launch DESTINATION share/${PROJECT_NAME} 9 | PATTERN "setup_assistant.launch" EXCLUDE) 10 | install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) 11 | install(FILES .setup_assistant DESTINATION share/${PROJECT_NAME}) 12 | -------------------------------------------------------------------------------- /canadarm2/canadarm_moveit_config/config/SSRMS_Canadarm2.ros2_control.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | mock_components/GenericSystem 10 | 11 | 12 | 13 | 14 | 15 | ${initial_positions['Base_Joint']} 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | ${initial_positions['Shoulder_Roll']} 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | ${initial_positions['Shoulder_Yaw']} 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | ${initial_positions['Elbow_Pitch']} 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | ${initial_positions['Wrist_Pitch']} 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | ${initial_positions['Wrist_Yaw']} 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | ${initial_positions['Wrist_Roll']} 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | -------------------------------------------------------------------------------- /canadarm2/canadarm_moveit_config/config/SSRMS_Canadarm2.srdf: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /canadarm2/canadarm_moveit_config/config/SSRMS_Canadarm2.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /canadarm2/canadarm_moveit_config/config/initial_positions.yaml: -------------------------------------------------------------------------------- 1 | # Default initial positions for SSRMS_Canadarm2's ros2_control fake system 2 | 3 | initial_positions: 4 | Base_Joint: 0 5 | Elbow_Pitch: 0 6 | Shoulder_Roll: 0 7 | Shoulder_Yaw: 0 8 | Wrist_Pitch: 0 9 | Wrist_Roll: 0 10 | Wrist_Yaw: 0 -------------------------------------------------------------------------------- /canadarm2/canadarm_moveit_config/config/joint_limits.yaml: -------------------------------------------------------------------------------- 1 | # joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed 2 | 3 | # For beginners, we downscale velocity and acceleration limits. 4 | # You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. 5 | default_velocity_scaling_factor: 0.1 6 | default_acceleration_scaling_factor: 0.1 7 | 8 | # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] 9 | # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] 10 | joint_limits: 11 | Base_Joint: 12 | has_velocity_limits: true 13 | max_velocity: 0.069813200000000006 14 | has_acceleration_limits: false 15 | max_acceleration: 0 16 | Elbow_Pitch: 17 | has_velocity_limits: true 18 | max_velocity: 0.069813200000000006 19 | has_acceleration_limits: false 20 | max_acceleration: 0 21 | Shoulder_Roll: 22 | has_velocity_limits: true 23 | max_velocity: 0.069813200000000006 24 | has_acceleration_limits: false 25 | max_acceleration: 0 26 | Shoulder_Yaw: 27 | has_velocity_limits: true 28 | max_velocity: 0.069813200000000006 29 | has_acceleration_limits: false 30 | max_acceleration: 0 31 | Wrist_Pitch: 32 | has_velocity_limits: true 33 | max_velocity: 0.069813200000000006 34 | has_acceleration_limits: false 35 | max_acceleration: 0 36 | Wrist_Roll: 37 | has_velocity_limits: true 38 | max_velocity: 0.069813200000000006 39 | has_acceleration_limits: false 40 | max_acceleration: 0 41 | Wrist_Yaw: 42 | has_velocity_limits: true 43 | max_velocity: 0.069813200000000006 44 | has_acceleration_limits: false 45 | max_acceleration: 0 -------------------------------------------------------------------------------- /canadarm2/canadarm_moveit_config/config/kinematics.yaml: -------------------------------------------------------------------------------- 1 | canadarm: 2 | kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin 3 | kinematics_solver_search_resolution: 0.0050000000000000001 4 | kinematics_solver_timeout: 0.0050000000000000001 5 | canadarm_planning_group: 6 | kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin 7 | kinematics_solver_search_resolution: 0.0050000000000000001 8 | kinematics_solver_timeout: 0.0050000000000000001 -------------------------------------------------------------------------------- /canadarm2/canadarm_moveit_config/config/moveit.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Name: Displays 4 | Property Tree Widget: 5 | Expanded: 6 | - /MotionPlanning1 7 | - Class: rviz_common/Help 8 | Name: Help 9 | - Class: rviz_common/Views 10 | Name: Views 11 | Visualization Manager: 12 | Displays: 13 | - Class: rviz_default_plugins/Grid 14 | Name: Grid 15 | Value: true 16 | - Class: moveit_rviz_plugin/MotionPlanning 17 | Name: MotionPlanning 18 | Planned Path: 19 | Loop Animation: true 20 | State Display Time: 0.05 s 21 | Trajectory Topic: display_planned_path 22 | Planning Scene Topic: monitored_planning_scene 23 | Robot Description: robot_description 24 | Scene Geometry: 25 | Scene Alpha: 1 26 | Scene Robot: 27 | Robot Alpha: 0.5 28 | Value: true 29 | Global Options: 30 | Fixed Frame: world 31 | Tools: 32 | - Class: rviz_default_plugins/Interact 33 | - Class: rviz_default_plugins/MoveCamera 34 | - Class: rviz_default_plugins/Select 35 | Value: true 36 | Views: 37 | Current: 38 | Class: rviz_default_plugins/Orbit 39 | Distance: 2.0 40 | Focal Point: 41 | X: -0.1 42 | Y: 0.25 43 | Z: 0.30 44 | Name: Current View 45 | Pitch: 0.5 46 | Target Frame: world 47 | Yaw: -0.623 48 | Window Geometry: 49 | Height: 975 50 | QMainWindow State: 000000ff00000000fd0000000100000000000002b400000375fc0200000005fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb000000100044006900730070006c006100790073010000003d00000123000000c900fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000166000001910000018800fffffffb0000000800480065006c0070000000029a0000006e0000006e00fffffffb0000000a0056006900650077007301000002fd000000b5000000a400ffffff000001f60000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 51 | Width: 1200 52 | -------------------------------------------------------------------------------- /canadarm2/canadarm_moveit_config/config/moveit_controllers.yaml: -------------------------------------------------------------------------------- 1 | # MoveIt uses this configuration for controller management 2 | 3 | moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager 4 | 5 | moveit_simple_controller_manager: 6 | controller_names: 7 | - canadarm_controller 8 | 9 | canadarm_controller: 10 | type: FollowJointTrajectory 11 | action_ns: follow_joint_trajectory 12 | default: true 13 | joints: 14 | - Base_Joint 15 | - Shoulder_Roll 16 | - Shoulder_Yaw 17 | - Elbow_Pitch 18 | - Wrist_Pitch 19 | - Wrist_Yaw 20 | - Wrist_Roll -------------------------------------------------------------------------------- /canadarm2/canadarm_moveit_config/config/pilz_cartesian_limits.yaml: -------------------------------------------------------------------------------- 1 | # Limits for the Pilz planner 2 | cartesian_limits: 3 | max_trans_vel: 1.0 4 | max_trans_acc: 2.25 5 | max_trans_dec: -5.0 6 | max_rot_vel: 1.57 7 | -------------------------------------------------------------------------------- /canadarm2/canadarm_moveit_config/config/ros2_controllers.yaml: -------------------------------------------------------------------------------- 1 | # This config file is used by ros2_control 2 | controller_manager: 3 | ros__parameters: 4 | update_rate: 100 # Hz 5 | 6 | canadarm_controller: 7 | type: joint_trajectory_controller/JointTrajectoryController 8 | 9 | 10 | joint_state_broadcaster: 11 | type: joint_state_broadcaster/JointStateBroadcaster 12 | 13 | canadarm_controller: 14 | ros__parameters: 15 | joints: 16 | - Base_Joint 17 | - Shoulder_Roll 18 | - Shoulder_Yaw 19 | - Elbow_Pitch 20 | - Wrist_Pitch 21 | - Wrist_Yaw 22 | - Wrist_Roll 23 | command_interfaces: 24 | - position 25 | - velocity 26 | state_interfaces: 27 | - position 28 | - velocity -------------------------------------------------------------------------------- /canadarm2/canadarm_moveit_config/launch/demo.launch.py: -------------------------------------------------------------------------------- 1 | from moveit_configs_utils import MoveItConfigsBuilder 2 | from moveit_configs_utils.launches import generate_demo_launch 3 | 4 | 5 | def generate_launch_description(): 6 | moveit_config = MoveItConfigsBuilder("SSRMS_Canadarm2", package_name="canadarm_moveit_config").to_moveit_configs() 7 | return generate_demo_launch(moveit_config) 8 | -------------------------------------------------------------------------------- /canadarm2/canadarm_moveit_config/launch/move_group.launch.py: -------------------------------------------------------------------------------- 1 | from moveit_configs_utils import MoveItConfigsBuilder 2 | from moveit_configs_utils.launches import generate_move_group_launch 3 | 4 | 5 | def generate_launch_description(): 6 | moveit_config = MoveItConfigsBuilder("SSRMS_Canadarm2", package_name="canadarm_moveit_config").to_moveit_configs() 7 | return generate_move_group_launch(moveit_config) 8 | -------------------------------------------------------------------------------- /canadarm2/canadarm_moveit_config/launch/moveit_rviz.launch.py: -------------------------------------------------------------------------------- 1 | from moveit_configs_utils import MoveItConfigsBuilder 2 | from moveit_configs_utils.launches import generate_moveit_rviz_launch 3 | 4 | 5 | def generate_launch_description(): 6 | moveit_config = MoveItConfigsBuilder("SSRMS_Canadarm2", package_name="canadarm_moveit_config").to_moveit_configs() 7 | return generate_moveit_rviz_launch(moveit_config) 8 | -------------------------------------------------------------------------------- /canadarm2/canadarm_moveit_config/launch/rsp.launch.py: -------------------------------------------------------------------------------- 1 | from moveit_configs_utils import MoveItConfigsBuilder 2 | from moveit_configs_utils.launches import generate_rsp_launch 3 | 4 | 5 | def generate_launch_description(): 6 | moveit_config = MoveItConfigsBuilder("SSRMS_Canadarm2", package_name="canadarm_moveit_config").to_moveit_configs() 7 | return generate_rsp_launch(moveit_config) 8 | -------------------------------------------------------------------------------- /canadarm2/canadarm_moveit_config/launch/setup_assistant.launch.py: -------------------------------------------------------------------------------- 1 | from moveit_configs_utils import MoveItConfigsBuilder 2 | from moveit_configs_utils.launches import generate_setup_assistant_launch 3 | 4 | 5 | def generate_launch_description(): 6 | moveit_config = MoveItConfigsBuilder("SSRMS_Canadarm2", package_name="canadarm_moveit_config").to_moveit_configs() 7 | return generate_setup_assistant_launch(moveit_config) 8 | -------------------------------------------------------------------------------- /canadarm2/canadarm_moveit_config/launch/spawn_controllers.launch.py: -------------------------------------------------------------------------------- 1 | from moveit_configs_utils import MoveItConfigsBuilder 2 | from moveit_configs_utils.launches import generate_spawn_controllers_launch 3 | 4 | 5 | def generate_launch_description(): 6 | moveit_config = MoveItConfigsBuilder("SSRMS_Canadarm2", package_name="canadarm_moveit_config").to_moveit_configs() 7 | return generate_spawn_controllers_launch(moveit_config) 8 | -------------------------------------------------------------------------------- /canadarm2/canadarm_moveit_config/launch/static_virtual_joint_tfs.launch.py: -------------------------------------------------------------------------------- 1 | from moveit_configs_utils import MoveItConfigsBuilder 2 | from moveit_configs_utils.launches import generate_static_virtual_joint_tfs_launch 3 | 4 | 5 | def generate_launch_description(): 6 | moveit_config = MoveItConfigsBuilder("SSRMS_Canadarm2", package_name="canadarm_moveit_config").to_moveit_configs() 7 | return generate_static_virtual_joint_tfs_launch(moveit_config) 8 | -------------------------------------------------------------------------------- /canadarm2/canadarm_moveit_config/launch/warehouse_db.launch.py: -------------------------------------------------------------------------------- 1 | from moveit_configs_utils import MoveItConfigsBuilder 2 | from moveit_configs_utils.launches import generate_warehouse_db_launch 3 | 4 | 5 | def generate_launch_description(): 6 | moveit_config = MoveItConfigsBuilder("SSRMS_Canadarm2", package_name="canadarm_moveit_config").to_moveit_configs() 7 | return generate_warehouse_db_launch(moveit_config) 8 | -------------------------------------------------------------------------------- /canadarm2/canadarm_moveit_config/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | canadarm_moveit_config 5 | 0.3.0 6 | 7 | An automatically generated package with all the configuration and launch files for using the SSRMS_Canadarm2 with the MoveIt Motion Planning Framework 8 | 9 | Dharini Dutia 10 | 11 | BSD 12 | 13 | http://moveit.ros.org/ 14 | https://github.com/ros-planning/moveit2/issues 15 | https://github.com/ros-planning/moveit2 16 | 17 | Dharini Dutia 18 | 19 | ament_cmake 20 | 21 | moveit_ros_move_group 22 | moveit_kinematics 23 | moveit_planners 24 | moveit_simple_controller_manager 25 | joint_state_publisher 26 | joint_state_publisher_gui 27 | tf2_ros 28 | xacro 29 | 31 | 32 | 33 | controller_manager 34 | moveit_configs_utils 35 | moveit_ros_move_group 36 | moveit_ros_visualization 37 | moveit_setup_assistant 38 | robot_state_publisher 39 | rviz2 40 | rviz_common 41 | rviz_default_plugins 42 | simulation 43 | tf2_ros 44 | xacro 45 | 46 | 47 | 48 | ament_cmake 49 | 50 | 51 | -------------------------------------------------------------------------------- /canadarm2/docker-compose.yml: -------------------------------------------------------------------------------- 1 | version: "3" 2 | 3 | services: 4 | canadarm_gui: 5 | image: osrf/space-ros:canadarm_gui 6 | build: 7 | context: ./ 8 | dockerfile: ./Dockerfile.GUI 9 | # runtime: nvidia 10 | environment: 11 | - PYTHONUNBUFFERED=1 # important to show error messages if a ros service crashes! 12 | - DISPLAY # for UI 13 | - QT_X11_NO_MITSHM=1 #fix some QT bugs 14 | - NVIDIA_VISIBLE_DEVICES=all # for docker-nvidia2 15 | - NVIDIA_DRIVER_CAPABILITIES=all # for docker-nvidia2 16 | - XDG_RUNTIME_DIR 17 | - __NV_PRIME_RENDER_OFFLOAD=1 18 | - __GLX_VENDOR_LIBRARY_NAME=nvidia 19 | - XAUTHORITY=$XAUTHORITY 20 | - RMW_IMPLEMENTATION=rmw_cyclonedds_cpp 21 | volumes: 22 | - /tmp/.X11-unix:/tmp/.X11-unix:rw # for UI 23 | - /run/user:/run/user:ro # for UI 24 | command: 25 | [ 26 | "bash", 27 | "-c", 28 | "source /opt/ros/jazzy/setup.bash && source ~/canadarm2_ws/install/setup.bash && ros2 launch canadarm_gazebo canadarm.launch.py", 29 | ] 30 | network_mode: host 31 | privileged: true # for UI 32 | canadarm_demo: 33 | image: osrf/space-ros:canadarm_demo 34 | build: 35 | context: ./ 36 | dockerfile: ./Dockerfile 37 | environment: 38 | - PYTHONUNBUFFERED=1 # important to show error messages if a ros service crashes! 39 | - RMW_IMPLEMENTATION=rmw_cyclonedds_cpp 40 | network_mode: host 41 | command: 42 | [ 43 | "bash", 44 | "-c", 45 | "source /home/spaceros-user/canadarm_ws/install/setup.bash && ros2 launch canadarm_demo canadarm.launch.py", 46 | ] 47 | -------------------------------------------------------------------------------- /canadarm2/run.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | docker compose down 4 | docker compose up -d 5 | -------------------------------------------------------------------------------- /curiosity_rover/.defaults.yaml: -------------------------------------------------------------------------------- 1 | { 2 | "build": { 3 | "symlink-install": true, 4 | "cmake_args": "-DCMAKE_BUILD_TYPE=Release" 5 | } 6 | } 7 | -------------------------------------------------------------------------------- /curiosity_rover/.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | install/ 3 | log/ 4 | -------------------------------------------------------------------------------- /curiosity_rover/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM osrf/space-ros:jazzy-2025.04.0 2 | 3 | ENV RMW_IMPLEMENTATION=rmw_cyclonedds_cpp 4 | ENV ROS_DISTRO=jazzy 5 | ENV HOME=/home/spaceros-user 6 | 7 | # Install dependencies 8 | # Related to space-ros/space-ros#195 9 | RUN sudo apt update && sudo apt install -y ros-${ROS_DISTRO}-control-msgs \ 10 | ros-${ROS_DISTRO}-rmw-cyclonedds-cpp 11 | 12 | # Prepare the workspace 13 | SHELL ["bash", "-c"] 14 | RUN mkdir -p ${HOME}/curiosity_ws/src 15 | WORKDIR ${HOME}/curiosity_ws 16 | 17 | COPY ./curiosity_rover_demo src/curiosity_rover_demo 18 | COPY .defaults.yaml .defaults.yaml 19 | 20 | # Build the workspace 21 | RUN source /opt/ros/${ROS_DISTRO}/setup.bash \ 22 | && source "${SPACEROS_DIR}"/install/setup.bash \ 23 | && colcon build --symlink-install 24 | 25 | # Source the workspace 26 | RUN echo "source ${HOME}/curiosity_ws/install/setup.bash" >> ${HOME}/.bashrc 27 | -------------------------------------------------------------------------------- /curiosity_rover/Dockerfile.GUI: -------------------------------------------------------------------------------- 1 | FROM ros:jazzy-ros-core-noble 2 | 3 | ARG DEBIAN_FRONTEND=noninteractive 4 | 5 | ENV RMW_IMPLEMENTATION=rmw_cyclonedds_cpp 6 | ENV ROS_DISTRO=jazzy 7 | ENV USERNAME=spaceros-user 8 | ENV HOME=/home/spaceros-user 9 | ENV GZ_VERSION=harmonic 10 | 11 | # Install dependencies 12 | RUN apt update && apt install -y ros-${ROS_DISTRO}-rmw-cyclonedds-cpp \ 13 | wget \ 14 | curl \ 15 | && apt clean \ 16 | && rm -rf /var/lib/apt/lists/* 17 | 18 | RUN wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg\ 19 | && echo "deb [arch=amd64 signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" | tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null 20 | 21 | # Install Gazebo 22 | RUN apt-get update -qq \ 23 | && apt-get install -y \ 24 | gz-${GZ_VERSION} \ 25 | libgz-sim8 \ 26 | libgz-transport13 \ 27 | libgz-gui8 \ 28 | build-essential\ 29 | ros-${ROS_DISTRO}-rcl-interfaces\ 30 | ros-${ROS_DISTRO}-rclcpp\ 31 | ros-${ROS_DISTRO}-builtin-interfaces\ 32 | ros-${ROS_DISTRO}-ros-gz\ 33 | ros-${ROS_DISTRO}-sdformat-urdf\ 34 | ros-${ROS_DISTRO}-vision-msgs\ 35 | ros-${ROS_DISTRO}-actuator-msgs\ 36 | ros-${ROS_DISTRO}-image-transport\ 37 | ros-${ROS_DISTRO}-xacro\ 38 | && rm -rf /var/lib/apt/lists/* 39 | 40 | 41 | # Install Rviz2 42 | RUN apt-get update -qq \ 43 | && apt-get install -y \ 44 | ros-${ROS_DISTRO}-rviz2 \ 45 | ros-${ROS_DISTRO}-moveit-ros-visualization \ 46 | ros-${ROS_DISTRO}-nav2-rviz-plugins \ 47 | && rm -rf /var/lib/apt/lists/* 48 | 49 | # Demo package 50 | RUN mkdir -p ${HOME}/curiosity_ws/src 51 | WORKDIR ${HOME}/curiosity_ws/src 52 | COPY ./curiosity_description ./curiosity_description 53 | COPY ./curiosity_gazebo ./curiosity_gazebo 54 | COPY .defaults.yaml ${HOME}/curiosity_ws/.defaults.yaml 55 | 56 | RUN apt update \ 57 | && apt install -y python3-rosdep python3-colcon-common-extensions \ 58 | && apt clean \ 59 | && rm -rf /var/lib/apt/lists/* \ 60 | && rosdep init \ 61 | && rosdep update 62 | 63 | RUN . /opt/ros/jazzy/setup.sh \ 64 | && cd ${HOME}/curiosity_ws \ 65 | && apt update \ 66 | && rosdep install --from-paths src --ignore-src -r -y \ 67 | && colcon build 68 | 69 | WORKDIR ${HOME}/curiosity_ws 70 | 71 | RUN apt update && apt install -y gz-harmonic 72 | RUN echo "source /opt/ros/jazzy/setup.bash" >> ${HOME}/.bashrc 73 | RUN echo "source ${HOME}/curiosity_ws/install/setup.bash" >> ${HOME}/.bashrc 74 | 75 | CMD gz sim 76 | -------------------------------------------------------------------------------- /curiosity_rover/README.md: -------------------------------------------------------------------------------- 1 | # Curiosity Rover - Demo 2 | 3 | This is a simple demo of controlling the curiosity rover using spaceROS. 4 | 5 | ### Installation 6 | 7 | To start the demo, there are few dependencies that need to be installed. The following steps will guide you through the installation process. 8 | 9 | 1. You will need docker installed on your system. If not, you can follow the instructions [here](https://docs.docker.com/get-docker/). 10 | 11 | ### How to run the demo 12 | 13 | 1. Clone the demo repository 14 | ```bash 15 | git clone https://github.com/space-ros/demos.git 16 | ``` 17 | 2. cd into the `curiosity_rover` directory 18 | ```bash 19 | cd curiosity_rover 20 | ``` 21 | 3. To build the demo, you can use the following command. 22 | ```bash 23 | # To build all the demos for the canadarm2 24 | ./build.sh 25 | ``` 26 | 4. To run the demo, you can use the following command. 27 | ```bash 28 | # To run the demo 29 | ./run.sh 30 | ``` 31 | 32 | This will start the demo in one terminal and gazebo in another terminal. To control the rover, we provide ros2 services for the demo. You can control the rover using the following services. 33 | 34 | 1. To move around, you can use the `move` service. The service takes two arguments, `linear` and `angular` velocities. You can call the service using the following command. 35 | ```bash 36 | # Move forward 37 | ros2 service call /move_forward std_srvs/srv/Empty 38 | 39 | # Turn left 40 | ros2 service call /turn_left std_srvs/srv/Empty 41 | 42 | # Turn right 43 | ros2 service call /turn_right std_srvs/srv/Empty 44 | 45 | # Stop 46 | ros2 service call /stop std_srvs/srv/Empty 47 | ``` 48 | 2. To control the arm of the rover, you can run the following services, 49 | ```bash 50 | # Open the arm 51 | ros2 service call /open_arm std_srvs/srv/Empty 52 | 53 | # Close the arm 54 | ros2 service call /close_arm std_srvs/srv/Empty 55 | ``` 56 | 3. To control the mast arm of the rover, you can run the following services, 57 | ```bash 58 | # Open the mast arm 59 | ros2 service call /mast_open std_srvs/srv/Empty 60 | 61 | # Close the mast arm 62 | ros2 service call /mast_close std_srvs/srv/Empty 63 | 64 | # To rotate the mast arm 65 | ros2 service call /mast_rotate std_srvs/srv/Empty 66 | ``` -------------------------------------------------------------------------------- /curiosity_rover/build.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -e 4 | 5 | docker compose build 6 | -------------------------------------------------------------------------------- /curiosity_rover/curiosity_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.22) 2 | project(curiosity_description) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | # uncomment the following section in order to fill in 11 | # further dependencies manually. 12 | # find_package( REQUIRED) 13 | 14 | install(DIRECTORY 15 | ${CMAKE_CURRENT_SOURCE_DIR}/models 16 | DESTINATION share/${PROJECT_NAME}/ 17 | ) 18 | 19 | 20 | if(BUILD_TESTING) 21 | find_package(ament_lint_auto REQUIRED) 22 | # the following line skips the linter which checks for copyrights 23 | # comment the line when a copyright and license is added to all source files 24 | set(ament_cmake_copyright_FOUND TRUE) 25 | # the following line skips cpplint (only works in a git repo) 26 | # comment the line when this package is in a git repo and when 27 | # a copyright and license is added to all source files 28 | set(ament_cmake_cpplint_FOUND TRUE) 29 | ament_lint_auto_find_test_dependencies() 30 | endif() 31 | 32 | ament_package() 33 | -------------------------------------------------------------------------------- /curiosity_rover/curiosity_description/models/config/mars_rover_control.yaml: -------------------------------------------------------------------------------- 1 | controller_manager: 2 | ros__parameters: 3 | update_rate: 100 4 | 5 | arm_joint_trajectory_controller: 6 | type: joint_trajectory_controller/JointTrajectoryController 7 | 8 | mast_joint_trajectory_controller: 9 | type: joint_trajectory_controller/JointTrajectoryController 10 | 11 | wheel_velocity_controller: 12 | type: velocity_controllers/JointGroupVelocityController 13 | 14 | steer_position_controller: 15 | type: joint_trajectory_controller/JointTrajectoryController 16 | 17 | wheel_tree_position_controller: 18 | type: effort_controllers/JointGroupEffortController 19 | 20 | joint_state_broadcaster: 21 | type: joint_state_broadcaster/JointStateBroadcaster 22 | 23 | 24 | arm_joint_trajectory_controller: 25 | ros__parameters: 26 | joints: 27 | - arm_01_joint 28 | - arm_02_joint 29 | - arm_03_joint 30 | - arm_04_joint 31 | - arm_tools_joint 32 | interface_name: position 33 | command_interfaces: 34 | - position 35 | state_interfaces: 36 | - position 37 | - velocity 38 | 39 | mast_joint_trajectory_controller: 40 | ros__parameters: 41 | joints: 42 | - mast_p_joint 43 | - mast_02_joint 44 | - mast_cameras_joint 45 | interface_name: position 46 | command_interfaces: 47 | - position 48 | state_interfaces: 49 | - position 50 | - velocity 51 | 52 | wheel_velocity_controller: 53 | ros__parameters: 54 | joints: 55 | - front_wheel_L_joint 56 | - middle_wheel_L_joint 57 | - back_wheel_L_joint 58 | - front_wheel_R_joint 59 | - middle_wheel_R_joint 60 | - back_wheel_R_joint 61 | interface_name: velocity 62 | 63 | 64 | steer_position_controller: 65 | ros__parameters: 66 | joints: 67 | - suspension_steer_F_L_joint 68 | - suspension_steer_B_L_joint 69 | - suspension_steer_F_R_joint 70 | - suspension_steer_B_R_joint 71 | interface_name: position 72 | command_interfaces: 73 | - position 74 | state_interfaces: 75 | - position 76 | - velocity 77 | 78 | wheel_tree_position_controller: 79 | ros__parameters: 80 | joints: 81 | - suspension_arm_F_L_joint 82 | - suspension_arm_B_L_joint 83 | - suspension_arm_F_R_joint 84 | - suspension_arm_B_R_joint 85 | command_interfaces: 86 | - effort 87 | state_interfaces: 88 | - position 89 | - velocity 90 | - effort 91 | gains: 92 | suspension_arm_F_L_joint: 93 | p: 2200.0 94 | i: 10.0 95 | d: 10.0 96 | suspension_arm_B_L_joint: 97 | p: 4200.0 98 | i: 10.0 99 | d: 10.0 100 | suspension_arm_F_R_joint: 101 | p: 2200.0 102 | i: 10.0 103 | d: 10.0 104 | suspension_arm_B_R_joint: 105 | p: 4200.0 106 | i: 10.0 107 | d: 10.0 108 | enable: 109 | suspension_arm_F_L_joint: 110 | status: 1 111 | suspension_arm_B_L_joint: 112 | status: 1 113 | suspension_arm_F_R_joint: 114 | status: 1 115 | suspension_arm_B_R_joint: 116 | status: 1 117 | -------------------------------------------------------------------------------- /curiosity_rover/curiosity_description/models/meshes/CuriosityQR.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/space-ros/demos/d82db232718d4a8e07f593c54f1f2137a84c0886/curiosity_rover/curiosity_description/models/meshes/CuriosityQR.stl -------------------------------------------------------------------------------- /curiosity_rover/curiosity_description/models/meshes/parts_AO.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/space-ros/demos/d82db232718d4a8e07f593c54f1f2137a84c0886/curiosity_rover/curiosity_description/models/meshes/parts_AO.png -------------------------------------------------------------------------------- /curiosity_rover/curiosity_description/models/meshes/tex_01.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/space-ros/demos/d82db232718d4a8e07f593c54f1f2137a84c0886/curiosity_rover/curiosity_description/models/meshes/tex_01.png -------------------------------------------------------------------------------- /curiosity_rover/curiosity_description/models/meshes/tex_02.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/space-ros/demos/d82db232718d4a8e07f593c54f1f2137a84c0886/curiosity_rover/curiosity_description/models/meshes/tex_02.png -------------------------------------------------------------------------------- /curiosity_rover/curiosity_description/models/meshes/tex_03.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/space-ros/demos/d82db232718d4a8e07f593c54f1f2137a84c0886/curiosity_rover/curiosity_description/models/meshes/tex_03.png -------------------------------------------------------------------------------- /curiosity_rover/curiosity_description/models/meshes/tex_03_n.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/space-ros/demos/d82db232718d4a8e07f593c54f1f2137a84c0886/curiosity_rover/curiosity_description/models/meshes/tex_03_n.png -------------------------------------------------------------------------------- /curiosity_rover/curiosity_description/models/meshes/tex_03_s.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/space-ros/demos/d82db232718d4a8e07f593c54f1f2137a84c0886/curiosity_rover/curiosity_description/models/meshes/tex_03_s.png -------------------------------------------------------------------------------- /curiosity_rover/curiosity_description/models/meshes/tex_04.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/space-ros/demos/d82db232718d4a8e07f593c54f1f2137a84c0886/curiosity_rover/curiosity_description/models/meshes/tex_04.png -------------------------------------------------------------------------------- /curiosity_rover/curiosity_description/models/meshes/tex_05.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/space-ros/demos/d82db232718d4a8e07f593c54f1f2137a84c0886/curiosity_rover/curiosity_description/models/meshes/tex_05.png -------------------------------------------------------------------------------- /curiosity_rover/curiosity_description/models/urdf/chassis.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /curiosity_rover/curiosity_description/models/urdf/curiosity_mars_rover.xacro: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /curiosity_rover/curiosity_description/models/urdf/macros.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | 8 | 9 | 10 | 11 | 19 | 20 | 21 | 22 | 23 | 31 | 32 | 33 | 34 | 35 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /curiosity_rover/curiosity_description/models/urdf/wheel.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 100.0 34 | 0.001 35 | 1.0 36 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | transmission_interface/SimpleTransmission 51 | 52 | hardware_interface/EffortJointInterface 53 | 54 | 55 | hardware_interface/EffortJointInterface 56 | 1 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /curiosity_rover/curiosity_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | curiosity_description 5 | 1.0.0 6 | Simulation description for Curiosity Mars Rover 7 | franklinselva 8 | GPL 9 | 10 | ament_cmake 11 | 12 | ament_lint_auto 13 | ament_lint_common 14 | 15 | 16 | ament_cmake 17 | 18 | 19 | -------------------------------------------------------------------------------- /curiosity_rover/curiosity_gazebo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.22) 2 | project(curiosity_gazebo) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(ament_cmake_python REQUIRED) 11 | find_package(control_msgs REQUIRED) 12 | find_package(curiosity_description REQUIRED) 13 | find_package(rclcpp REQUIRED) 14 | find_package(ament_cmake REQUIRED) 15 | 16 | install(DIRECTORY 17 | config 18 | launch 19 | models 20 | worlds 21 | DESTINATION share/${PROJECT_NAME} 22 | ) 23 | 24 | ament_python_install_package(${PROJECT_NAME}) 25 | 26 | install(PROGRAMS 27 | nodes/odom_tf_publisher 28 | DESTINATION lib/${PROJECT_NAME} 29 | ) 30 | 31 | if(BUILD_TESTING) 32 | find_package(ament_lint_auto REQUIRED) 33 | # the following line skips the linter which checks for copyrights 34 | # comment the line when a copyright and license is added to all source files 35 | set(ament_cmake_copyright_FOUND TRUE) 36 | # the following line skips cpplint (only works in a git repo) 37 | # comment the line when this package is in a git repo and when 38 | # a copyright and license is added to all source files 39 | set(ament_cmake_cpplint_FOUND TRUE) 40 | ament_lint_auto_find_test_dependencies() 41 | endif() 42 | 43 | ament_package() 44 | -------------------------------------------------------------------------------- /curiosity_rover/curiosity_gazebo/config/mars_rover_control.yaml: -------------------------------------------------------------------------------- 1 | controller_manager: 2 | ros__parameters: 3 | update_rate: 100 4 | 5 | arm_joint_trajectory_controller: 6 | type: joint_trajectory_controller/JointTrajectoryController 7 | 8 | mast_joint_trajectory_controller: 9 | type: joint_trajectory_controller/JointTrajectoryController 10 | 11 | wheel_velocity_controller: 12 | type: velocity_controllers/JointGroupVelocityController 13 | 14 | steer_position_controller: 15 | type: joint_trajectory_controller/JointTrajectoryController 16 | 17 | wheel_tree_position_controller: 18 | type: effort_controllers/JointGroupPositionController 19 | 20 | joint_state_broadcaster: 21 | type: joint_state_broadcaster/JointStateBroadcaster 22 | 23 | 24 | arm_joint_trajectory_controller: 25 | ros__parameters: 26 | joints: 27 | - arm_01_joint 28 | - arm_02_joint 29 | - arm_03_joint 30 | - arm_04_joint 31 | - arm_tools_joint 32 | interface_name: position 33 | command_interfaces: 34 | - position 35 | state_interfaces: 36 | - position 37 | - velocity 38 | 39 | mast_joint_trajectory_controller: 40 | ros__parameters: 41 | joints: 42 | - mast_p_joint 43 | - mast_02_joint 44 | - mast_cameras_joint 45 | interface_name: position 46 | command_interfaces: 47 | - position 48 | state_interfaces: 49 | - position 50 | - velocity 51 | 52 | wheel_velocity_controller: 53 | ros__parameters: 54 | joints: 55 | - front_wheel_L_joint 56 | - middle_wheel_L_joint 57 | - back_wheel_L_joint 58 | - front_wheel_R_joint 59 | - middle_wheel_R_joint 60 | - back_wheel_R_joint 61 | interface_name: velocity 62 | 63 | 64 | steer_position_controller: 65 | ros__parameters: 66 | joints: 67 | - suspension_steer_F_L_joint 68 | - suspension_steer_B_L_joint 69 | - suspension_steer_F_R_joint 70 | - suspension_steer_B_R_joint 71 | interface_name: position 72 | command_interfaces: 73 | - position 74 | state_interfaces: 75 | - position 76 | - velocity 77 | 78 | wheel_tree_position_controller: 79 | ros__parameters: 80 | joints: 81 | - suspension_arm_F_L_joint 82 | - suspension_arm_B_L_joint 83 | - suspension_arm_F_R_joint 84 | - suspension_arm_B_R_joint 85 | command_interfaces: 86 | - effort 87 | state_interfaces: 88 | - position 89 | - velocity 90 | - effort 91 | gains: 92 | suspension_arm_F_L_joint: 93 | p: 2200.0 94 | i: 10.0 95 | d: 10.0 96 | suspension_arm_B_L_joint: 97 | p: 4200.0 98 | i: 10.0 99 | d: 10.0 100 | suspension_arm_F_R_joint: 101 | p: 2200.0 102 | i: 10.0 103 | d: 10.0 104 | suspension_arm_B_R_joint: 105 | p: 4200.0 106 | i: 10.0 107 | d: 10.0 108 | enable: 109 | suspension_arm_F_L_joint: 110 | status: 1 111 | suspension_arm_B_L_joint: 112 | status: 1 113 | suspension_arm_F_R_joint: 114 | status: 1 115 | suspension_arm_B_R_joint: 116 | status: 1 117 | -------------------------------------------------------------------------------- /curiosity_rover/curiosity_gazebo/curiosity_gazebo/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/space-ros/demos/d82db232718d4a8e07f593c54f1f2137a84c0886/curiosity_rover/curiosity_gazebo/curiosity_gazebo/__init__.py -------------------------------------------------------------------------------- /curiosity_rover/curiosity_gazebo/models/curiosity_path/meshes/curiosity_path.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/space-ros/demos/d82db232718d4a8e07f593c54f1f2137a84c0886/curiosity_rover/curiosity_gazebo/models/curiosity_path/meshes/curiosity_path.stl -------------------------------------------------------------------------------- /curiosity_rover/curiosity_gazebo/models/curiosity_path/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | curiosity_path 4 | 0.1.0 5 | 6 | Miguel Angel Rodriguez 7 | duckfrost@theconstructsim.com 8 | 9 | model.sdf 10 | 11 | NASA's most advanced Mars rover Curiosity has landed on the Red Planet.Source 3D Models: https://nasa3d.arc.nasa.gov/detail/curiosity-path 12 | 13 | 14 | -------------------------------------------------------------------------------- /curiosity_rover/curiosity_gazebo/models/curiosity_path/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 1 5 | 6 | 7 | 0.25 8 | 9 | 0.00015 10 | 0.000000 11 | 0.000000 12 | 0.00015 13 | 0.000000 14 | 0.00015 15 | 16 | 17 | 18 | 19 | 20 | model://curiosity_path/meshes/curiosity_path.stl 21 | 1 1 1 22 | 23 | 24 | 25 | 26 | 27 | 30.0 28 | 30.0 29 | 30 | 31 | 32 | 33 | 1000000.0 34 | 100.0 35 | 1.0 36 | 0.002 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | model://curiosity_path/meshes/mars_path_simple1.dae 45 | 1 1 1 46 | 47 | 48 | 56 | 57 | 58 | 0.000000 59 | 0.000000 60 | 61 | 0 62 | 0 63 | 1 64 | 65 | 66 | 67 | -------------------------------------------------------------------------------- /curiosity_rover/curiosity_gazebo/nodes/odom_tf_publisher: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # adapted from ROS 2 tutorial 4 | # https://docs.ros.org/en/rolling/Tutorials/Intermediate/Tf2/Writing-A-Tf2-Broadcaster-Py.html 5 | """Odometry TF Broadcaster for Curiosity Mars Rovers""" 6 | 7 | import rclpy # type: ignore 8 | 9 | from nav_msgs.msg import Odometry # type: ignore 10 | from geometry_msgs.msg import TransformStamped # type: ignore 11 | from rclpy.node import Node # type: ignore 12 | from tf2_ros import TransformBroadcaster # type: ignore 13 | 14 | 15 | class OdomTFBroadcaster(Node): 16 | """Odometry TF Broadcaster for Curiosity Mars Rovers""" 17 | 18 | def __init__(self): 19 | super().__init__("odom_tf_publisher") 20 | 21 | # Declare the parameter for the topic name 22 | self.declare_parameter("odom_topic", "/model/curiosity_mars_rover/odometry") 23 | 24 | # Get the parameter value 25 | odom_topic = self.get_parameter("odom_topic").get_parameter_value().string_value 26 | 27 | # Initialize the transform broadcaster 28 | self.tf_broadcaster = TransformBroadcaster(self) 29 | 30 | # Subscribe to Odometry topic 31 | self.subscription = self.create_subscription( 32 | Odometry, odom_topic, self.handle_odometry, 1 33 | ) 34 | 35 | def handle_odometry(self, msg): 36 | """Handle incoming Odometry message and broadcast it as a TF""" 37 | tf = TransformStamped() 38 | 39 | # Read message content and assign it to 40 | # corresponding tf variables 41 | tf.header.stamp = msg.header.stamp 42 | tf.header.frame_id = msg.header.frame_id 43 | tf.child_frame_id = msg.child_frame_id 44 | 45 | # get translation coordinates from the message pose 46 | tf.transform.translation.x = msg.pose.pose.position.x 47 | tf.transform.translation.y = msg.pose.pose.position.y 48 | tf.transform.translation.z = msg.pose.pose.position.z 49 | 50 | # get rotation from the message pose 51 | tf.transform.rotation.x = msg.pose.pose.orientation.x 52 | tf.transform.rotation.y = msg.pose.pose.orientation.y 53 | tf.transform.rotation.z = msg.pose.pose.orientation.z 54 | tf.transform.rotation.w = msg.pose.pose.orientation.w 55 | 56 | # Send the transformation 57 | self.tf_broadcaster.sendTransform(tf) 58 | 59 | 60 | def main(): 61 | """Main function to start the node""" 62 | rclpy.init() 63 | node = OdomTFBroadcaster() 64 | node.get_logger().info("Starting odometry_tf_publisher node") 65 | try: 66 | rclpy.spin(node) 67 | except KeyboardInterrupt: 68 | pass 69 | 70 | rclpy.shutdown() 71 | 72 | 73 | if __name__ == "__main__": 74 | main() 75 | -------------------------------------------------------------------------------- /curiosity_rover/curiosity_gazebo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | curiosity_gazebo 5 | 0.1.0 6 | Gazebo worlds and ROS2 launch descriptions for Curiosity Mars Demo 7 | franklinselva 8 | GPL 9 | 10 | ament_cmake 11 | 12 | control_msgs 13 | curiosity_description 14 | rclcpp 15 | ament_cmake 16 | ament_cmake_python 17 | ros_gz 18 | 19 | gz_ros2_control 20 | ros2_control 21 | ros2_controllers 22 | 23 | ament_lint_auto 24 | ament_lint_common 25 | 26 | 27 | ament_cmake 28 | 29 | 30 | -------------------------------------------------------------------------------- /curiosity_rover/curiosity_gazebo/worlds/mars_curiosity.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 0.4 0.4 0.4 1 6 | 0.9 0.753 0.66 1 7 | false 8 | 9 | 10 | 11 | 12 | 13 | 14 | 3D View 15 | false 16 | docked 17 | 18 | 19 | ogre2 20 | scene 21 | -5.0 0.0 -6.0 0.0 0.0 0.0 22 | 23 | 24 | 25 | false 26 | 5 27 | 5 28 | floating 29 | false 30 | 31 | 32 | 33 | 34 | false 35 | 5 36 | 5 37 | floating 38 | false 39 | 40 | 41 | 42 | 43 | 44 | floating 45 | 46 | 47 | 48 | 49 | 50 | 53 | 54 | 55 | 58 | 59 | 60 | 63 | 64 | 65 | 66 | 67 | true 68 | 0 0 10 0 0 0 69 | 0.8 0.8 0.8 1 70 | 0.2 0.2 0.2 1 71 | 72 | 1000 73 | 0.9 74 | 0.01 75 | 0.001 76 | 77 | -0.5 0.1 -0.9 78 | 79 | 80 | 81 | 0 0 -3.711 82 | 83 | 84 | model://curiosity_path 85 | curiosity_path 86 | 0 0 0 0 0 0 87 | 88 | 89 | 90 | 91 | 92 | -------------------------------------------------------------------------------- /curiosity_rover/curiosity_rover_demo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.22) 2 | project(curiosity_rover_demo) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(ament_cmake_python REQUIRED) 11 | find_package(control_msgs REQUIRED) 12 | find_package(geometry_msgs REQUIRED) 13 | find_package(rclcpp REQUIRED) 14 | find_package(rclcpp_action REQUIRED) 15 | find_package(rclpy REQUIRED) 16 | find_package(std_msgs REQUIRED) 17 | 18 | 19 | if(BUILD_TESTING) 20 | find_package(ament_lint_auto REQUIRED) 21 | # the following line skips the linter which checks for copyrights 22 | # comment the line when a copyright and license is added to all source files 23 | set(ament_cmake_copyright_FOUND TRUE) 24 | # the following line skips cpplint (only works in a git repo) 25 | # comment the line when this package is in a git repo and when 26 | # a copyright and license is added to all source files 27 | set(ament_cmake_cpplint_FOUND TRUE) 28 | ament_lint_auto_find_test_dependencies() 29 | endif() 30 | 31 | install(DIRECTORY 32 | launch 33 | DESTINATION share/${PROJECT_NAME} 34 | ) 35 | 36 | ament_python_install_package(${PROJECT_NAME}) 37 | 38 | install(PROGRAMS 39 | nodes/move_arm 40 | nodes/move_mast 41 | nodes/move_wheel 42 | nodes/run_demo 43 | DESTINATION lib/${PROJECT_NAME} 44 | ) 45 | 46 | ament_package() 47 | -------------------------------------------------------------------------------- /curiosity_rover/curiosity_rover_demo/curiosity_rover_demo/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/space-ros/demos/d82db232718d4a8e07f593c54f1f2137a84c0886/curiosity_rover/curiosity_rover_demo/curiosity_rover_demo/__init__.py -------------------------------------------------------------------------------- /curiosity_rover/curiosity_rover_demo/launch/mars_rover.launch.py: -------------------------------------------------------------------------------- 1 | """Launch the curiosity rover demo.""" 2 | 3 | from launch import LaunchDescription # type: ignore 4 | from launch_ros.actions import Node, SetParameter # type: ignore 5 | 6 | 7 | def generate_launch_description(): 8 | """Launch the curiosity rover demo.""" 9 | 10 | arm_node = Node( 11 | package="curiosity_rover_demo", executable="move_arm", output="screen" 12 | ) 13 | 14 | mast_node = Node( 15 | package="curiosity_rover_demo", executable="move_mast", output="screen" 16 | ) 17 | 18 | wheel_node = Node( 19 | package="curiosity_rover_demo", executable="move_wheel", output="screen" 20 | ) 21 | 22 | run_node = Node( 23 | package="curiosity_rover_demo", executable="run_demo", output="screen" 24 | ) 25 | 26 | return LaunchDescription( 27 | [ 28 | SetParameter(name="use_sim_time", value=True), 29 | arm_node, 30 | mast_node, 31 | wheel_node, 32 | run_node, 33 | ] 34 | ) 35 | -------------------------------------------------------------------------------- /curiosity_rover/curiosity_rover_demo/nodes/move_arm: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | """Move the arm of the curiosity rover.""" 3 | 4 | import rclpy # type: ignore 5 | from rclpy.node import Node # type: ignore 6 | from builtin_interfaces.msg import Duration # type: ignore 7 | 8 | from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint # type: ignore 9 | from std_srvs.srv import Empty # type: ignore 10 | 11 | 12 | class MoveArm(Node): 13 | """Move the arm of the curiosity rover.""" 14 | 15 | def __init__(self): 16 | super().__init__("arm_node") 17 | self.arm_publisher_ = self.create_publisher( 18 | JointTrajectory, "/arm_joint_trajectory_controller/joint_trajectory", 10 19 | ) 20 | self.open_srv = self.create_service(Empty, "open_arm", self.open_arm_callback) 21 | self.close_srv = self.create_service( 22 | Empty, "close_arm", self.close_arm_callback 23 | ) 24 | 25 | self.open = True 26 | 27 | def open_arm_callback(self, request, response): # pylint: disable=unused-argument 28 | """Open the arm of the curiosity rover.""" 29 | self.open = True 30 | traj = JointTrajectory() 31 | traj.joint_names = [ 32 | "arm_01_joint", 33 | "arm_02_joint", 34 | "arm_03_joint", 35 | "arm_04_joint", 36 | "arm_tools_joint", 37 | ] 38 | 39 | point1 = JointTrajectoryPoint() 40 | point1.positions = [0.0, -0.5, 3.0, 1.0, 0.0] 41 | point1.time_from_start = Duration(sec=4) 42 | 43 | traj.points.append(point1) 44 | self.arm_publisher_.publish(traj) 45 | 46 | return response 47 | 48 | def close_arm_callback(self, request, response): # pylint: disable=unused-argument 49 | """Close the arm of the curiosity rover.""" 50 | self.open = False 51 | traj = JointTrajectory() 52 | traj.joint_names = [ 53 | "arm_01_joint", 54 | "arm_02_joint", 55 | "arm_03_joint", 56 | "arm_04_joint", 57 | "arm_tools_joint", 58 | ] 59 | 60 | point1 = JointTrajectoryPoint() 61 | point1.positions = [-1.57, -0.4, -1.1, -1.57, -1.57] 62 | point1.time_from_start = Duration(sec=4) 63 | 64 | traj.points.append(point1) 65 | self.arm_publisher_.publish(traj) 66 | 67 | return response 68 | 69 | 70 | def main(args=None): 71 | """Main function""" 72 | rclpy.init(args=args) 73 | 74 | arm_node = MoveArm() 75 | 76 | rclpy.spin(arm_node) 77 | # Destroy the node explicitly 78 | # (optional - otherwise it will be done automatically 79 | # when the garbage collector destroys the node object) 80 | arm_node.destroy_node() 81 | rclpy.shutdown() 82 | 83 | 84 | if __name__ == "__main__": 85 | main() 86 | -------------------------------------------------------------------------------- /curiosity_rover/curiosity_rover_demo/nodes/move_mast: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | """Move the mast arm of the curiosity rover.""" 3 | 4 | import rclpy # type: ignore 5 | from rclpy.node import Node # type: ignore 6 | from builtin_interfaces.msg import Duration # type: ignore 7 | 8 | from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint # type: ignore 9 | from std_srvs.srv import Empty # type: ignore 10 | 11 | 12 | class MastArm(Node): 13 | """Move the mast arm of the curiosity rover.""" 14 | 15 | def __init__(self): 16 | super().__init__("mast_node") 17 | self.mast_publisher_ = self.create_publisher( 18 | JointTrajectory, "/mast_joint_trajectory_controller/joint_trajectory", 10 19 | ) 20 | self.mast_open_srv = self.create_service( 21 | Empty, "mast_open", self.mast_open_callback 22 | ) 23 | self.mast_close_srv = self.create_service( 24 | Empty, "mast_close", self.mast_close_callback 25 | ) 26 | self.mast_rotate_srv = self.create_service( 27 | Empty, "mast_rotate", self.mast_rotate_callback 28 | ) 29 | 30 | def mast_open_callback(self, request, response): # pylint: disable=unused-argument 31 | """Move the mast arm to open position.""" 32 | traj = JointTrajectory() 33 | traj.joint_names = ["mast_p_joint", "mast_02_joint", "mast_cameras_joint"] 34 | 35 | point = JointTrajectoryPoint() 36 | point.positions = [0.0, -0.5, 0.0] 37 | point.time_from_start = Duration(sec=1) 38 | 39 | traj.points.append(point) 40 | self.mast_publisher_.publish(traj) 41 | return response 42 | 43 | def mast_close_callback(self, request, response): # pylint: disable=unused-argument 44 | """Move the mast arm to close position.""" 45 | traj = JointTrajectory() 46 | traj.joint_names = ["mast_p_joint", "mast_02_joint", "mast_cameras_joint"] 47 | 48 | point = JointTrajectoryPoint() 49 | point.positions = [1.57, -1.57, 0.0] 50 | point.time_from_start = Duration(sec=1) 51 | 52 | traj.points.append(point) 53 | self.mast_publisher_.publish(traj) 54 | return response 55 | 56 | def mast_rotate_callback( 57 | self, request, response 58 | ): # pylint: disable=unused-argument 59 | """Rotate the mast arm.""" 60 | 61 | traj = JointTrajectory() 62 | traj.joint_names = ["mast_p_joint", "mast_02_joint", "mast_cameras_joint"] 63 | 64 | point1 = JointTrajectoryPoint() 65 | point1.positions = [0.0, -1.57, 0.0] 66 | point1.time_from_start = Duration(sec=2) 67 | traj.points.append(point1) 68 | 69 | point2 = JointTrajectoryPoint() 70 | point2.positions = [0.0, -3.14, 0.0] 71 | point2.time_from_start = Duration(sec=4) 72 | traj.points.append(point2) 73 | 74 | point3 = JointTrajectoryPoint() 75 | point3.positions = [0.0, -6.28, 0.0] 76 | point3.time_from_start = Duration(sec=6) 77 | traj.points.append(point3) 78 | 79 | self.mast_publisher_.publish(traj) 80 | return response 81 | 82 | 83 | def main(args=None): 84 | """Main function.""" 85 | rclpy.init(args=args) 86 | 87 | mast_node = MastArm() 88 | 89 | rclpy.spin(mast_node) 90 | 91 | # Destroy the node explicitly 92 | # (optional - otherwise it will be done automatically 93 | # when the garbage collector destroys the node object) 94 | mast_node.destroy_node() 95 | rclpy.shutdown() 96 | 97 | 98 | if __name__ == "__main__": 99 | main() 100 | -------------------------------------------------------------------------------- /curiosity_rover/curiosity_rover_demo/nodes/run_demo: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | """Combine motions from different moving parts of the Curiosity rover.""" 3 | 4 | 5 | import rclpy # type: ignore 6 | from rclpy.node import Node # type: ignore 7 | 8 | from geometry_msgs.msg import Twist # type: ignore 9 | from std_srvs.srv import Empty # type: ignore 10 | 11 | 12 | # A node for performing combination of motions from different moving parts of the Curiosity rover. 13 | class RunDemo(Node): 14 | """Main node for combining motions from different moving parts of the Curiosity rover.""" 15 | 16 | def __init__(self) -> None: 17 | super().__init__("run_node") 18 | self.motion_publisher_ = self.create_publisher(Twist, "/cmd_vel", 10) 19 | self.forward_srv = self.create_service( 20 | Empty, "move_forward", self.move_forward_callback 21 | ) 22 | self.stop_srv = self.create_service(Empty, "move_stop", self.move_stop_callback) 23 | self.left_srv = self.create_service(Empty, "turn_left", self.turn_left_callback) 24 | self.right_srv = self.create_service( 25 | Empty, "turn_right", self.turn_right_callback 26 | ) 27 | self.stopped = True 28 | 29 | timer_period = 0.1 30 | self.timer = self.create_timer(timer_period, self.timer_callback) 31 | 32 | self.curr_action = Twist() 33 | 34 | def timer_callback(self): 35 | """Timer callback to publish the current action.""" 36 | if not self.stopped: 37 | self.motion_publisher_.publish(self.curr_action) 38 | 39 | def move_forward_callback( 40 | self, request, response 41 | ): # pylint: disable=unused-argument 42 | """Move the rover forward.""" 43 | self.get_logger().info("Moving forward") 44 | action = Twist() 45 | action.linear.x = 2.0 46 | self.curr_action = action 47 | self.stopped = False 48 | return response 49 | 50 | def move_stop_callback(self, request, response): # pylint: disable=unused-argument 51 | """Move the rover forward.""" 52 | 53 | # stop timer from publishing 54 | self.stopped = True 55 | self.get_logger().info("Stopping") 56 | self.curr_action = Twist() 57 | # publish once to ensure we stop 58 | self.motion_publisher_.publish(self.curr_action) 59 | return response 60 | 61 | def turn_left_callback(self, request, response): # pylint: disable=unused-argument 62 | """Turn the rover left.""" 63 | 64 | self.get_logger().info("Turning left") 65 | action = Twist() 66 | action.linear.x = 1.0 67 | action.angular.z = 0.4 68 | self.curr_action = action 69 | self.stopped = False 70 | return response 71 | 72 | def turn_right_callback(self, request, response): # pylint: disable=unused-argument 73 | """Turn the rover right.""" 74 | 75 | self.get_logger().info("Turning right") 76 | self.stopped = False 77 | action = Twist() 78 | action.linear.x = 1.0 79 | action.angular.z = -0.4 80 | self.curr_action = action 81 | self.stopped = False 82 | return response 83 | 84 | 85 | def main(args=None): 86 | """Main function to run the demo.""" 87 | rclpy.init(args=args) 88 | 89 | run_demo = RunDemo() 90 | 91 | rclpy.spin(run_demo) 92 | 93 | # Destroy the node explicitly 94 | # (optional - otherwise it will be done automatically 95 | # when the garbage collector destroys the node object) 96 | run_demo.destroy_node() 97 | rclpy.shutdown() 98 | 99 | 100 | if __name__ == "__main__": 101 | main() 102 | -------------------------------------------------------------------------------- /curiosity_rover/curiosity_rover_demo/nodes/test_node: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import rclpy 4 | from rclpy.node import Node 5 | 6 | from std_msgs.msg import String 7 | 8 | 9 | class MinimalPublisher(Node): 10 | 11 | def __init__(self): 12 | super().__init__('minimal_publisher') 13 | self.publisher_ = self.create_publisher(String, 'topic', 10) 14 | timer_period = 0.5 # seconds 15 | self.timer = self.create_timer(timer_period, self.timer_callback) 16 | self.i = 0 17 | 18 | def timer_callback(self): 19 | msg = String() 20 | msg.data = 'Hello World: %d' % self.i 21 | self.publisher_.publish(msg) 22 | self.get_logger().info('Publishing: "%s"' % msg.data) 23 | self.i += 1 24 | 25 | 26 | def main(args=None): 27 | rclpy.init(args=args) 28 | 29 | minimal_publisher = MinimalPublisher() 30 | 31 | rclpy.spin(minimal_publisher) 32 | 33 | # Destroy the node explicitly 34 | # (optional - otherwise it will be done automatically 35 | # when the garbage collector destroys the node object) 36 | minimal_publisher.destroy_node() 37 | rclpy.shutdown() 38 | 39 | 40 | if __name__ == '__main__': 41 | main() -------------------------------------------------------------------------------- /curiosity_rover/curiosity_rover_demo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | curiosity_rover_demo 5 | 0.0.1 6 | A Mars Rover Demo for SpaceROS 7 | tianyu 8 | Apache-2.0 9 | 10 | ament_cmake 11 | ament_cmake_python 12 | 13 | rclcpp 14 | rclpy 15 | 16 | ament_index_python 17 | control_msgs 18 | diff_drive_controller 19 | effort_controllers 20 | geometry_msgs 21 | hardware_interface 22 | imu_sensor_broadcaster 23 | joint_state_broadcaster 24 | joint_trajectory_controller 25 | launch 26 | launch_ros 27 | robot_state_publisher 28 | ros2controlcli 29 | std_msgs 30 | velocity_controllers 31 | 32 | ament_lint_auto 33 | ament_lint_common 34 | 35 | 36 | ament_cmake 37 | 38 | 39 | -------------------------------------------------------------------------------- /curiosity_rover/docker-compose.yml: -------------------------------------------------------------------------------- 1 | version: "3" 2 | 3 | services: 4 | curiosity_gui: 5 | image: osrf/space-ros:curiosity_gui 6 | build: 7 | context: ./ 8 | dockerfile: ./Dockerfile.GUI 9 | # runtime: nvidia 10 | environment: 11 | - PYTHONUNBUFFERED=1 # important to show error messages if a ros service crashes! 12 | - DISPLAY # for UI 13 | - QT_X11_NO_MITSHM=1 #fix some QT bugs 14 | - NVIDIA_VISIBLE_DEVICES=all # for docker-nvidia2 15 | - NVIDIA_DRIVER_CAPABILITIES=all # for docker-nvidia2 16 | - XDG_RUNTIME_DIR 17 | - __NV_PRIME_RENDER_OFFLOAD=1 18 | - __GLX_VENDOR_LIBRARY_NAME=nvidia 19 | - XAUTHORITY=$XAUTHORITY 20 | - RMW_IMPLEMENTATION=rmw_cyclonedds_cpp 21 | volumes: 22 | - /tmp/.X11-unix:/tmp/.X11-unix:rw # for UI 23 | - /run/user:/run/user:ro # for UI 24 | command: 25 | [ 26 | "bash", 27 | "-c", 28 | "source /opt/ros/jazzy/setup.bash && source ~/curiosity_ws/install/setup.bash && ros2 launch curiosity_gazebo curiosity_gazebo.launch.py", 29 | ] 30 | network_mode: host 31 | privileged: true # for UI 32 | curiosity_demo: 33 | image: osrf/space-ros:curiosity_demo 34 | build: 35 | context: ./ 36 | dockerfile: ./Dockerfile 37 | environment: 38 | - PYTHONUNBUFFERED=1 # important to show error messages if a ros service crashes! 39 | - RMW_IMPLEMENTATION=rmw_cyclonedds_cpp 40 | network_mode: host 41 | command: 42 | [ 43 | "bash", 44 | "-c", 45 | "source /home/spaceros-user/curiosity_ws/install/setup.bash && ros2 launch curiosity_rover_demo mars_rover.launch.py", 46 | ] 47 | -------------------------------------------------------------------------------- /curiosity_rover/run.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | docker compose down 4 | docker compose up -d 5 | -------------------------------------------------------------------------------- /ros_trick/.gitignore: -------------------------------------------------------------------------------- 1 | .vscode 2 | docker_repo/ 3 | -------------------------------------------------------------------------------- /ros_trick/build.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -e 4 | 5 | TAG=latest 6 | ROS_TRICK_BRIDGE_IMAGE_NAME=ros_trick_bridge 7 | CANADARM_ROS_TRICK_DEMO_NAME=canadarm_ros_trick_demo 8 | export SPACE_ROS_IMAGE=osrf/space-ros:humble-2024.10.0 9 | CURRENT_PATH=$(pwd) 10 | SPACEROS_DOCKER_REPO_PATH="${CURRENT_PATH}/docker_repo" 11 | 12 | if [ ! -d ${SPACEROS_DOCKER_REPO_PATH} ]; then 13 | mkdir -p ${SPACEROS_DOCKER_REPO_PATH} 14 | git clone -b humble-2024.10.0 https://github.com/space-ros/docker.git ${SPACEROS_DOCKER_REPO_PATH} 15 | cd ${SPACEROS_DOCKER_REPO_PATH} 16 | git apply ${CURRENT_PATH}/spaceros_docker_repo.patch 17 | cd ${CURRENT_PATH} 18 | fi 19 | cd ${SPACEROS_DOCKER_REPO_PATH}/moveit2 && ./build.sh 20 | cd ${SPACEROS_DOCKER_REPO_PATH}/space_robots && ./build.sh 21 | cd ${CURRENT_PATH} 22 | 23 | docker build -t ${ROS_TRICK_BRIDGE_IMAGE_NAME}:${TAG} -f ./ros_trick_bridge.Dockerfile . 24 | docker build -t ${CANADARM_ROS_TRICK_DEMO_NAME}:${TAG} -f ./canadarm_ros_trick_demo.Dockerfile . 25 | -------------------------------------------------------------------------------- /ros_trick/canadarm_ros_trick_demo.Dockerfile: -------------------------------------------------------------------------------- 1 | # Copyright 2024 Blazej Fiderek (xfiderek) 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # https://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | # The base image is built from 16 | # https://github.com/space-ros/docker/blob/humble-2024.10.0/space_robots/ 17 | # Git tag: humble-2024.10.0. 18 | FROM openrobotics/space_robots_demo:latest 19 | 20 | # Rviz does not work inside the newest space_robots_demo, so we have to upgrade 21 | # the following packages. These packages are already present in the base image 22 | # and here we are upgrading them to latest versions. 23 | RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ 24 | --mount=type=cache,target=/var/lib/apt,sharing=locked \ 25 | sudo apt-get update && sudo apt-get install -y \ 26 | mesa-libgallium \ 27 | libdrm-amdgpu1 \ 28 | libdrm-common \ 29 | libdrm-intel1 \ 30 | libdrm-radeon1 \ 31 | libdrm2 \ 32 | libegl-mesa0 \ 33 | libgbm1 \ 34 | libgl1-mesa-dev \ 35 | libglapi-mesa \ 36 | libglx-mesa0 \ 37 | liborc-0.4-0 \ 38 | libpq5 \ 39 | linux-libc-dev \ 40 | mesa-va-drivers \ 41 | mesa-vdpau-drivers \ 42 | mesa-vulkan-drivers 43 | 44 | ENV TRICK_DEMO_WS=${HOME_DIR}/ros_trick_demo_ws 45 | 46 | RUN mkdir ${TRICK_DEMO_WS} 47 | WORKDIR ${TRICK_DEMO_WS} 48 | COPY --chown=spaceros-user:spaceros-user ros_src ${TRICK_DEMO_WS}/src 49 | 50 | RUN /bin/bash -c 'source ${DEMO_DIR}/install/setup.bash \ 51 | && colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release -DCMAKE_EXPORT_COMPILE_COMMANDS=ON --event-handlers desktop_notification- status-' 52 | RUN rm -rf build log src 53 | -------------------------------------------------------------------------------- /ros_trick/docker-compose.yml: -------------------------------------------------------------------------------- 1 | # Copyright 2024 Blazej Fiderek (xfiderek) 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # https://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | services: 16 | ros_trick_bridge: 17 | container_name: ros_trick_bridge 18 | command: [ "bash", "-c", ". ~/.bashrc && . install/setup.bash && ros2 launch ros_trick_bridge ros_trick_bridge.launch.py" ] 19 | build: 20 | context: . 21 | dockerfile: ros_trick_bridge.Dockerfile 22 | image: ros_trick_bridge:latest 23 | environment: 24 | - DISPLAY=${DISPLAY} 25 | - TERM=${TERM} 26 | - QT_X11_NO_MITSHM=1 27 | network_mode: host 28 | volumes: 29 | - /tmp/.X11-unix:/tmp/.X11-unix 30 | 31 | canadarm_ros_trick_demo: 32 | container_name: canadarm_ros_trick_demo 33 | build: 34 | context: . 35 | dockerfile: canadarm_ros_trick_demo.Dockerfile 36 | image: canadarm_ros_trick_demo:latest 37 | network_mode: host 38 | environment: 39 | - DISPLAY=${DISPLAY} 40 | - TERM=${TERM} 41 | - QT_X11_NO_MITSHM=1 42 | volumes: 43 | - /tmp/.X11-unix:/tmp/.X11-unix 44 | command: [ "bash", "-c", ". ~/.bashrc && . install/setup.bash && ros2 launch trick_canadarm_moveit_config demo.launch.py" ] 45 | -------------------------------------------------------------------------------- /ros_trick/docs/moveit_rviz.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/space-ros/demos/d82db232718d4a8e07f593c54f1f2137a84c0886/ros_trick/docs/moveit_rviz.png -------------------------------------------------------------------------------- /ros_trick/docs/ros_trick_bridge_architecture.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/space-ros/demos/d82db232718d4a8e07f593c54f1f2137a84c0886/ros_trick/docs/ros_trick_bridge_architecture.png -------------------------------------------------------------------------------- /ros_trick/docs/rviz_joints.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/space-ros/demos/d82db232718d4a8e07f593c54f1f2137a84c0886/ros_trick/docs/rviz_joints.png -------------------------------------------------------------------------------- /ros_trick/docs/rviz_motion.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/space-ros/demos/d82db232718d4a8e07f593c54f1f2137a84c0886/ros_trick/docs/rviz_motion.png -------------------------------------------------------------------------------- /ros_trick/docs/trick_main.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/space-ros/demos/d82db232718d4a8e07f593c54f1f2137a84c0886/ros_trick/docs/trick_main.png -------------------------------------------------------------------------------- /ros_trick/docs/trick_velocity_plot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/space-ros/demos/d82db232718d4a8e07f593c54f1f2137a84c0886/ros_trick/docs/trick_velocity_plot.png -------------------------------------------------------------------------------- /ros_trick/docs/trick_view_variables.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/space-ros/demos/d82db232718d4a8e07f593c54f1f2137a84c0886/ros_trick/docs/trick_view_variables.png -------------------------------------------------------------------------------- /ros_trick/ros_src/canadarm_wrench_publisher/canadarm_wrench_publisher/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright 2024 Blazej Fiderek (xfiderek) 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # https://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | -------------------------------------------------------------------------------- /ros_trick/ros_src/canadarm_wrench_publisher/canadarm_wrench_publisher/canadarm_wrench_publisher_node.py: -------------------------------------------------------------------------------- 1 | # Copyright 2024 Blazej Fiderek (xfiderek) 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # https://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | 16 | import time 17 | 18 | import rclpy 19 | from geometry_msgs.msg import WrenchStamped 20 | from rclpy.executors import SingleThreadedExecutor 21 | from rclpy.node import Node 22 | from ros_trick_bridge.trick_variable_server_client import TrickVariableServerClient 23 | from std_msgs.msg import Header 24 | 25 | 26 | class WrenchPublisher(Node): 27 | def __init__(self): 28 | super().__init__("wrench_publisher") 29 | self._trick_host = "localhost" 30 | self._trick_port = 49765 31 | self._ndof = 7 32 | self._trick_variable_names = [ 33 | item 34 | for i in range(self._ndof) 35 | for item in [ 36 | f"CanadarmManip.manip.applied_torque[{i}]", 37 | f"CanadarmManip.manip.friction_torque[{i}]", 38 | ] 39 | ] 40 | 41 | # let everything start 42 | time.sleep(5.0) 43 | 44 | self.get_logger().info("starting trick variable server") 45 | self._trick_variable_server_client = TrickVariableServerClient( 46 | host=self._trick_host, 47 | port=self._trick_port, 48 | client_tag="wrench_pub", 49 | trick_variable_names=self._trick_variable_names, 50 | trick_var_cycle_time=0.1, 51 | on_receive_callback=self.trick_data_callback, 52 | ) 53 | self.link_frames = ["B1", "B2", "B3", "B4", "B5", "B6", "EE_SSRMS"] 54 | self.rot_axes = [ 55 | [0.0, 0.0, 1.0], 56 | [1.0, 0.0, 0.0], 57 | [0.0, 0.0, 1.0], 58 | [0.0, 0.0, 1.0], 59 | [0.0, 0.0, -1.0], 60 | [1.0, 0.0, 0.0], 61 | [0.0, 0.0, 1.0], 62 | ] 63 | self.get_logger().info("spawning wrench publishers") 64 | self.wrench_publishers = [ 65 | publisher 66 | for link_name in self.link_frames 67 | for publisher in [ 68 | self.create_publisher( 69 | WrenchStamped, f"wrench_topic/{link_name}/total_torque", 10 70 | ), 71 | self.create_publisher( 72 | WrenchStamped, f"wrench_topic/{link_name}/friction_torque", 10 73 | ), 74 | ] 75 | ] 76 | 77 | self._trick_variable_server_client.start_listening() 78 | 79 | def trick_data_callback(self, trick_data): 80 | for i in range(self._ndof): 81 | total_torque_var_name = self._trick_variable_names[2 * i] 82 | friction_torque_var_name = self._trick_variable_names[2 * i + 1] 83 | total_torque_val = float(trick_data[total_torque_var_name]) 84 | friction_torque_val = float(trick_data[friction_torque_var_name]) 85 | total_torque_pub = self.wrench_publishers[2 * i] 86 | friction_torque_pub = self.wrench_publishers[2 * i + 1] 87 | now = self.get_clock().now().to_msg() 88 | 89 | msg = WrenchStamped() 90 | msg.header = Header() 91 | msg.header.frame_id = self.link_frames[i] 92 | msg.header.stamp = now 93 | 94 | # send total torque 95 | msg.wrench.torque.x = self.rot_axes[i][0] * total_torque_val 96 | msg.wrench.torque.y = self.rot_axes[i][1] * total_torque_val 97 | msg.wrench.torque.z = self.rot_axes[i][2] * total_torque_val 98 | total_torque_pub.publish(msg) 99 | 100 | # send friction torque 101 | msg.wrench.torque.x = self.rot_axes[i][0] * friction_torque_val 102 | msg.wrench.torque.y = self.rot_axes[i][1] * friction_torque_val 103 | msg.wrench.torque.z = self.rot_axes[i][2] * friction_torque_val 104 | friction_torque_pub.publish(msg) 105 | 106 | 107 | def main(args=None): 108 | rclpy.init(args=args) 109 | node = WrenchPublisher() 110 | rclpy.spin(node) 111 | rclpy.shutdown() 112 | 113 | 114 | if __name__ == "__main__": 115 | main() 116 | -------------------------------------------------------------------------------- /ros_trick/ros_src/canadarm_wrench_publisher/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | canadarm_wrench_publisher 5 | 0.0.0 6 | TODO: Package description 7 | blazej 8 | TODO: License declaration 9 | 10 | ament_copyright 11 | ament_flake8 12 | ament_pep257 13 | python3-pytest 14 | 15 | 16 | ament_python 17 | 18 | 19 | -------------------------------------------------------------------------------- /ros_trick/ros_src/canadarm_wrench_publisher/resource/canadarm_wrench_publisher: -------------------------------------------------------------------------------- 1 | // Copyright 2024 Blazej Fiderek (xfiderek) 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // https://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | -------------------------------------------------------------------------------- /ros_trick/ros_src/canadarm_wrench_publisher/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/canadarm_wrench_publisher 3 | [install] 4 | install_scripts=$base/lib/canadarm_wrench_publisher 5 | -------------------------------------------------------------------------------- /ros_trick/ros_src/canadarm_wrench_publisher/setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import find_packages, setup 2 | 3 | package_name = "canadarm_wrench_publisher" 4 | 5 | setup( 6 | name=package_name, 7 | version="0.0.0", 8 | packages=find_packages(exclude=["test"]), 9 | data_files=[ 10 | ("share/ament_index/resource_index/packages", ["resource/" + package_name]), 11 | ("share/" + package_name, ["package.xml"]), 12 | ], 13 | install_requires=["setuptools"], 14 | zip_safe=True, 15 | maintainer="Blazej Fiderek (xfiderek)", 16 | maintainer_email="fiderekblazej@gmail.com", 17 | description="Get joint torques from trick and publish them as GeometryWrench", 18 | license="Apache-2.0", 19 | tests_require=["pytest"], 20 | entry_points={ 21 | "console_scripts": [ 22 | "canadarm_wrench_publisher_node = canadarm_wrench_publisher.canadarm_wrench_publisher_node:main" 23 | ], 24 | }, 25 | ) 26 | -------------------------------------------------------------------------------- /ros_trick/ros_src/ros_trick_bridge/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | ros_trick_bridge 5 | 0.0.0 6 | Bridge between ROS and NASA Trick simulator 7 | Blazej Fiderek (xfiderek) 8 | TODO: License declaration 9 | 10 | ament_copyright 11 | ament_flake8 12 | ament_pep257 13 | python3-pytest 14 | 15 | 16 | ament_python 17 | 18 | 19 | -------------------------------------------------------------------------------- /ros_trick/ros_src/ros_trick_bridge/params/ros_trick_bridge_example_params.yaml: -------------------------------------------------------------------------------- 1 | ros_trick_bridge: 2 | ros__parameters: 3 | sim_directory: /home/spaceros-user/trick_sims/SIM_trick_canadarm 4 | sim_inputfile: RUN_2DPlanar/input.py 5 | sim_executable: S_main_Linux_11.4_x86_64.exe 6 | sim_args: '' 7 | publish_clock: true 8 | clock_publish_period: 0.02 9 | -------------------------------------------------------------------------------- /ros_trick/ros_src/ros_trick_bridge/resource/ros_trick_bridge: -------------------------------------------------------------------------------- 1 | // Copyright 2024 Blazej Fiderek (xfiderek) 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // https://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | -------------------------------------------------------------------------------- /ros_trick/ros_src/ros_trick_bridge/ros_trick_bridge/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright 2024 Blazej Fiderek (xfiderek) 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # https://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | -------------------------------------------------------------------------------- /ros_trick/ros_src/ros_trick_bridge/ros_trick_bridge/trick_variable_server_client.py: -------------------------------------------------------------------------------- 1 | # Copyright 2024 Blazej Fiderek (xfiderek) 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # https://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | 16 | import socket 17 | import threading 18 | import time 19 | 20 | 21 | class TrickVariableServerClient: 22 | def __init__( 23 | self, 24 | host, 25 | port, 26 | client_tag, 27 | trick_variable_names, 28 | trick_var_cycle_time=0.1, 29 | on_receive_callback=None, 30 | ): 31 | self._client_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) 32 | self._client_socket.connect((host, port)) 33 | 34 | self._insock = self._client_socket.makefile("r") 35 | 36 | self._variable_names = trick_variable_names 37 | self._latest_data = {} 38 | self._on_receive_callback = on_receive_callback 39 | self._trick_var_cycle_time = trick_var_cycle_time 40 | 41 | # send configuration info to trick and ask for variables 42 | self._client_socket.send( 43 | f'trick.var_set_client_tag("{client_tag}") \n'.encode() 44 | ) 45 | self._client_socket.send( 46 | f"trick.var_cycle({str(trick_var_cycle_time)}) \n".encode() 47 | ) 48 | 49 | self._receive_thread = threading.Thread(target=self._rcv_trick_data_from_socket) 50 | self._receive_thread.start() 51 | 52 | def freeze_sim(self): 53 | self._client_socket.send("trick.exec_freeze()\n".encode()) 54 | self.stop_listening() 55 | 56 | def unfreeze_sim(self): 57 | self.start_listening() 58 | self._client_socket.send("trick.exec_run()\n".encode()) 59 | 60 | def start_listening(self): 61 | command = b"" 62 | for trick_var_name in self._variable_names: 63 | command += f'trick.var_add("{trick_var_name}") \n'.encode() 64 | self._client_socket.send("trick.var_ascii()\n".encode()) 65 | self._client_socket.send(command) 66 | 67 | def stop_listening(self): 68 | self._client_socket.send("trick.var_clear()\n".encode()) 69 | 70 | def send_data_to_sim(self, var_name: str, value: str) -> None: 71 | self._client_socket.send(f"{var_name} = {value} \n".encode()) 72 | 73 | def _rcv_trick_data_from_socket(self): 74 | while True: 75 | if self._insock: 76 | line = self._insock.readline() 77 | if line: 78 | variables = line.split("\t") 79 | if variables[0] == "0": 80 | for i, var_name in enumerate(self._variable_names): 81 | self._latest_data[var_name] = variables[i + 1].strip() 82 | if self._on_receive_callback: 83 | self._on_receive_callback(self._latest_data.copy()) 84 | else: 85 | time.sleep(self._trick_var_cycle_time) 86 | -------------------------------------------------------------------------------- /ros_trick/ros_src/ros_trick_bridge/setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/ros_trick_bridge 3 | [install] 4 | install_scripts=$base/lib/ros_trick_bridge 5 | -------------------------------------------------------------------------------- /ros_trick/ros_src/ros_trick_bridge/setup.py: -------------------------------------------------------------------------------- 1 | import os 2 | from glob import glob 3 | 4 | from setuptools import find_packages, setup 5 | 6 | package_name = "ros_trick_bridge" 7 | 8 | setup( 9 | name=package_name, 10 | version="0.0.0", 11 | packages=find_packages(exclude=["test"]), 12 | data_files=[ 13 | ("share/ament_index/resource_index/packages", ["resource/" + package_name]), 14 | ("share/" + package_name, ["package.xml"]), 15 | (os.path.join("share", package_name, "launch"), glob("launch/*.py")), 16 | (os.path.join("share", package_name, "params"), glob("params/*")), 17 | ], 18 | install_requires=["setuptools"], 19 | zip_safe=True, 20 | maintainer="Blazej Fiderek (xfiderek)", 21 | maintainer_email="fiderekblazej@gmail.com", 22 | description="", 23 | license="Apache-2.0", 24 | tests_require=["pytest"], 25 | entry_points={ 26 | "console_scripts": [ 27 | "ros_trick_bridge_node = ros_trick_bridge.ros_trick_bridge_node:main" 28 | ], 29 | }, 30 | ) 31 | -------------------------------------------------------------------------------- /ros_trick/ros_src/trick_canadarm_moveit_config/.setup_assistant: -------------------------------------------------------------------------------- 1 | moveit_setup_assistant_config: 2 | urdf: 3 | package: simulation 4 | relative_path: models/canadarm/urdf/SSRMS_Canadarm2.urdf 5 | srdf: 6 | relative_path: config/SSRMS_Canadarm2.srdf 7 | package_settings: 8 | author_name: Dharini Dutia 9 | author_email: dharini@openrobotics.org 10 | generated_timestamp: 1672757808 11 | control_xacro: 12 | command: 13 | - position 14 | - velocity 15 | state: 16 | - position 17 | - velocity 18 | modified_urdf: 19 | xacros: 20 | - control_xacro 21 | control_xacro: 22 | command: 23 | - position 24 | - velocity 25 | state: 26 | - position 27 | - velocity -------------------------------------------------------------------------------- /ros_trick/ros_src/trick_canadarm_moveit_config/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.22) 2 | project(trick_canadarm_moveit_config) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | 6 | ament_package() 7 | 8 | install(DIRECTORY launch DESTINATION share/${PROJECT_NAME} 9 | PATTERN "setup_assistant.launch" EXCLUDE) 10 | install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) 11 | install(FILES .setup_assistant DESTINATION share/${PROJECT_NAME}) 12 | -------------------------------------------------------------------------------- /ros_trick/ros_src/trick_canadarm_moveit_config/config/SSRMS_Canadarm2.srdf: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /ros_trick/ros_src/trick_canadarm_moveit_config/config/initial_positions.yaml: -------------------------------------------------------------------------------- 1 | initial_positions: 2 | Base_Joint: 0 3 | Elbow_Pitch: 0 4 | Shoulder_Roll: 0 5 | Shoulder_Yaw: 0 6 | Wrist_Pitch: 0 7 | Wrist_Roll: 0 8 | Wrist_Yaw: 0 9 | -------------------------------------------------------------------------------- /ros_trick/ros_src/trick_canadarm_moveit_config/config/joint_limits.yaml: -------------------------------------------------------------------------------- 1 | # joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed 2 | 3 | # For beginners, we downscale velocity and acceleration limits. 4 | # You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. 5 | default_velocity_scaling_factor: 0.1 6 | default_acceleration_scaling_factor: 0.1 7 | 8 | # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] 9 | # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] 10 | joint_limits: 11 | Base_Joint: 12 | has_velocity_limits: true 13 | max_velocity: 0.1 14 | has_acceleration_limits: true 15 | max_acceleration: 0.025 16 | Elbow_Pitch: 17 | has_velocity_limits: true 18 | max_velocity: 0.1 19 | has_acceleration_limits: true 20 | max_acceleration: 0.025 21 | Shoulder_Roll: 22 | has_velocity_limits: true 23 | max_velocity: 0.1 24 | has_acceleration_limits: true 25 | max_acceleration: 0.025 26 | Shoulder_Yaw: 27 | has_velocity_limits: true 28 | max_velocity: 0.1 29 | has_acceleration_limits: true 30 | max_acceleration: 0.025 31 | Wrist_Pitch: 32 | has_velocity_limits: true 33 | max_velocity: 0.1 34 | has_acceleration_limits: true 35 | max_acceleration: 0.025 36 | Wrist_Roll: 37 | has_velocity_limits: true 38 | max_velocity: 0.1 39 | has_acceleration_limits: true 40 | max_acceleration: 0.025 41 | Wrist_Yaw: 42 | has_velocity_limits: true 43 | max_velocity: 0.1 44 | has_acceleration_limits: true 45 | max_acceleration: 0.025 46 | -------------------------------------------------------------------------------- /ros_trick/ros_src/trick_canadarm_moveit_config/config/kinematics.yaml: -------------------------------------------------------------------------------- 1 | canadarm: 2 | kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin 3 | kinematics_solver_search_resolution: 0.005 4 | kinematics_solver_timeout: 0.05 5 | canadarm_planning_group: 6 | kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin 7 | kinematics_solver_search_resolution: 0.005 8 | kinematics_solver_timeout: 0.005 9 | -------------------------------------------------------------------------------- /ros_trick/ros_src/trick_canadarm_moveit_config/config/moveit_controllers.yaml: -------------------------------------------------------------------------------- 1 | moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager 2 | 3 | moveit_simple_controller_manager: 4 | controller_names: 5 | - canadarm_controller 6 | 7 | canadarm_controller: 8 | type: FollowJointTrajectory 9 | action_ns: follow_joint_trajectory 10 | default: true 11 | joints: 12 | - Base_Joint 13 | - Shoulder_Roll 14 | - Shoulder_Yaw 15 | - Elbow_Pitch 16 | - Wrist_Pitch 17 | - Wrist_Yaw 18 | - Wrist_Roll 19 | -------------------------------------------------------------------------------- /ros_trick/ros_src/trick_canadarm_moveit_config/config/pilz_cartesian_limits.yaml: -------------------------------------------------------------------------------- 1 | # Limits for the Pilz planner 2 | cartesian_limits: 3 | max_trans_vel: 1.0 4 | max_trans_acc: 2.25 5 | max_trans_dec: -5.0 6 | max_rot_vel: 1.57 7 | -------------------------------------------------------------------------------- /ros_trick/ros_src/trick_canadarm_moveit_config/config/ros2_controllers.yaml: -------------------------------------------------------------------------------- 1 | # This config file is used by ros2_control 2 | controller_manager: 3 | ros__parameters: 4 | update_rate: 100 # Hz 5 | 6 | canadarm_controller: 7 | type: joint_trajectory_controller/JointTrajectoryController 8 | 9 | 10 | joint_state_broadcaster: 11 | type: joint_state_broadcaster/JointStateBroadcaster 12 | 13 | canadarm_controller: 14 | ros__parameters: 15 | open_loop_control: false 16 | allow_nonzero_velocity_at_trajectory_end: false 17 | constraints: 18 | stopped_velocity_tolerance: 0.001 19 | goal_time: 0.0 20 | joints: 21 | - Base_Joint 22 | - Shoulder_Roll 23 | - Shoulder_Yaw 24 | - Elbow_Pitch 25 | - Wrist_Pitch 26 | - Wrist_Yaw 27 | - Wrist_Roll 28 | command_interfaces: 29 | - effort 30 | state_interfaces: 31 | - position 32 | - velocity 33 | state_publish_rate: 50.0 34 | action_monitor_rate: 20.0 35 | gains: 36 | Base_Joint: 37 | p: 92000.0 38 | i: 0.0 39 | d: 2050.0 40 | ff_velocity_scale: 0.0 41 | i_clamp: 0.0 42 | Shoulder_Roll: 43 | p: 92000.0 44 | i: 0.0 45 | d: 2050.0 46 | ff_velocity_scale: 0.0 47 | i_clamp: 0.0 48 | Shoulder_Yaw: 49 | p: 92000.0 50 | i: 0.0 51 | d: 2050.0 52 | ff_velocity_scale: 0.0 53 | i_clamp: 0.0 54 | Elbow_Pitch: 55 | p: 92000.0 56 | i: 0.0 57 | d: 2050.0 58 | ff_velocity_scale: 0.0 59 | i_clamp: 0.0 60 | Wrist_Pitch: 61 | p: 50000.0 62 | i: 0.0 63 | d: 1100.0 64 | ff_velocity_scale: 0.0 65 | i_clamp: 0.0 66 | Wrist_Yaw: 67 | p: 500.0 68 | i: 0.0 69 | d: 50.0 70 | ff_velocity_scale: 0.0 71 | i_clamp: 0.0 72 | Wrist_Roll: 73 | p: 500.0 74 | i: 0.0 75 | d: 50.0 76 | ff_velocity_scale: 0.0 77 | i_clamp: 0.0 78 | -------------------------------------------------------------------------------- /ros_trick/ros_src/trick_canadarm_moveit_config/launch/demo.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch.actions import IncludeLaunchDescription 3 | from launch.launch_description_sources import PythonLaunchDescriptionSource 4 | from launch_ros.actions import Node, SetParameter 5 | from moveit_configs_utils import MoveItConfigsBuilder 6 | 7 | 8 | def generate_launch_description(): 9 | moveit_config = MoveItConfigsBuilder( 10 | "SSRMS_Canadarm2", package_name="trick_canadarm_moveit_config" 11 | ).to_moveit_configs() 12 | moveit_config.move_group_capabilities["capabilities"] = "" 13 | ld = LaunchDescription() 14 | virtual_joints_launch = ( 15 | moveit_config.package_path / "launch/static_virtual_joint_tfs.launch.py" 16 | ) 17 | if virtual_joints_launch.exists(): 18 | ld.add_action( 19 | IncludeLaunchDescription( 20 | PythonLaunchDescriptionSource(str(virtual_joints_launch)), 21 | ) 22 | ) 23 | 24 | # Given the published joint states, publish tf for the robot links 25 | ld.add_action( 26 | IncludeLaunchDescription( 27 | PythonLaunchDescriptionSource( 28 | str(moveit_config.package_path / "launch/rsp.launch.py") 29 | ), 30 | ) 31 | ) 32 | 33 | ld.add_action( 34 | IncludeLaunchDescription( 35 | PythonLaunchDescriptionSource( 36 | str(moveit_config.package_path / "launch/move_group.launch.py") 37 | ), 38 | ) 39 | ) 40 | 41 | ld.add_action( 42 | Node( 43 | package="canadarm_wrench_publisher", 44 | executable="canadarm_wrench_publisher_node", 45 | ) 46 | ) 47 | 48 | ld.add_action( 49 | Node( 50 | package="controller_manager", 51 | executable="ros2_control_node", 52 | parameters=[ 53 | moveit_config.robot_description, 54 | str(moveit_config.package_path / "config/ros2_controllers.yaml"), 55 | ], 56 | ) 57 | ) 58 | 59 | ld.add_action( 60 | IncludeLaunchDescription( 61 | PythonLaunchDescriptionSource( 62 | str(moveit_config.package_path / "launch/spawn_controllers.launch.py") 63 | ), 64 | ) 65 | ) 66 | 67 | return ld 68 | -------------------------------------------------------------------------------- /ros_trick/ros_src/trick_canadarm_moveit_config/launch/move_group.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch.actions import DeclareLaunchArgument 3 | from launch.substitutions import LaunchConfiguration 4 | from launch_ros.parameter_descriptions import ParameterValue 5 | from moveit_configs_utils import MoveItConfigsBuilder 6 | from moveit_configs_utils.launch_utils import ( 7 | DeclareBooleanLaunchArg, 8 | add_debuggable_node, 9 | ) 10 | 11 | 12 | def generate_launch_description(): 13 | moveit_config = MoveItConfigsBuilder( 14 | "SSRMS_Canadarm2", package_name="trick_canadarm_moveit_config" 15 | ).to_moveit_configs() 16 | # print("prinint capab") 17 | # print(moveit_config.move_group_capabilities) 18 | moveit_config.move_group_capabilities["capabilities"] = "" 19 | ld = LaunchDescription() 20 | 21 | ld.add_action(DeclareBooleanLaunchArg("debug", default_value=False)) 22 | ld.add_action( 23 | DeclareBooleanLaunchArg("allow_trajectory_execution", default_value=True) 24 | ) 25 | ld.add_action( 26 | DeclareBooleanLaunchArg("publish_monitored_planning_scene", default_value=True) 27 | ) 28 | # load non-default MoveGroup capabilities (space separated) 29 | ld.add_action(DeclareLaunchArgument("capabilities", default_value="")) 30 | # inhibit these default MoveGroup capabilities (space separated) 31 | ld.add_action(DeclareLaunchArgument("disable_capabilities", default_value="")) 32 | 33 | # do not copy dynamics information from /joint_states to internal robot monitoring 34 | # default to false, because almost nothing in move_group relies on this information 35 | ld.add_action(DeclareBooleanLaunchArg("monitor_dynamics", default_value=False)) 36 | 37 | should_publish = LaunchConfiguration("publish_monitored_planning_scene") 38 | 39 | move_group_configuration = { 40 | "publish_robot_description_semantic": True, 41 | "allow_trajectory_execution": LaunchConfiguration("allow_trajectory_execution"), 42 | # Note: Wrapping the following values is necessary so that the parameter value can be the empty string 43 | "capabilities": ParameterValue( 44 | LaunchConfiguration("capabilities"), value_type=str 45 | ), 46 | "disable_capabilities": ParameterValue( 47 | LaunchConfiguration("disable_capabilities"), value_type=str 48 | ), 49 | # Publish the planning scene of the physical robot so that rviz plugin can know actual robot 50 | "publish_planning_scene": should_publish, 51 | "publish_geometry_updates": should_publish, 52 | "publish_state_updates": should_publish, 53 | "publish_transforms_updates": should_publish, 54 | "monitor_dynamics": False, 55 | } 56 | 57 | move_group_params = [ 58 | moveit_config.to_dict(), 59 | move_group_configuration, 60 | ] 61 | 62 | add_debuggable_node( 63 | ld, 64 | package="moveit_ros_move_group", 65 | executable="move_group", 66 | commands_file=str(moveit_config.package_path / "launch" / "gdb_settings.gdb"), 67 | output="screen", 68 | parameters=move_group_params, 69 | extra_debug_args=["--debug"], 70 | # Set the display variable, in case OpenGL code is used internally 71 | additional_env={"DISPLAY": ""}, 72 | ) 73 | return ld 74 | -------------------------------------------------------------------------------- /ros_trick/ros_src/trick_canadarm_moveit_config/launch/moveit_rviz.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from moveit_configs_utils import MoveItConfigsBuilder 3 | from moveit_configs_utils.launches import generate_moveit_rviz_launch 4 | 5 | 6 | def generate_launch_description(): 7 | ld = LaunchDescription() 8 | 9 | moveit_config = MoveItConfigsBuilder( 10 | "SSRMS_Canadarm2", package_name="trick_canadarm_moveit_config" 11 | ).to_moveit_configs() 12 | 13 | ld.add_action(generate_moveit_rviz_launch(moveit_config)) 14 | return ld 15 | -------------------------------------------------------------------------------- /ros_trick/ros_src/trick_canadarm_moveit_config/launch/rsp.launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch.actions import DeclareLaunchArgument 3 | from launch.substitutions import LaunchConfiguration 4 | from launch_ros.actions import Node 5 | from moveit_configs_utils import MoveItConfigsBuilder 6 | 7 | 8 | def generate_launch_description(): 9 | moveit_config = MoveItConfigsBuilder( 10 | "SSRMS_Canadarm2", package_name="trick_canadarm_moveit_config" 11 | ).to_moveit_configs() 12 | moveit_config.move_group_capabilities["capabilities"] = "" 13 | 14 | ld = LaunchDescription() 15 | ld.add_action(DeclareLaunchArgument("publish_frequency", default_value="15.0")) 16 | 17 | # Given the published joint states, publish tf for the robot links and the robot description 18 | rsp_node = Node( 19 | package="robot_state_publisher", 20 | executable="robot_state_publisher", 21 | respawn=True, 22 | output="screen", 23 | parameters=[ 24 | moveit_config.robot_description, 25 | { 26 | "publish_frequency": LaunchConfiguration("publish_frequency"), 27 | }, 28 | ], 29 | ) 30 | ld.add_action(rsp_node) 31 | 32 | return ld 33 | -------------------------------------------------------------------------------- /ros_trick/ros_src/trick_canadarm_moveit_config/launch/setup_assistant.launch.py: -------------------------------------------------------------------------------- 1 | from moveit_configs_utils import MoveItConfigsBuilder 2 | from moveit_configs_utils.launches import generate_setup_assistant_launch 3 | 4 | 5 | def generate_launch_description(): 6 | moveit_config = MoveItConfigsBuilder( 7 | "SSRMS_Canadarm2", package_name="trick_canadarm_moveit_config" 8 | ).to_moveit_configs() 9 | return generate_setup_assistant_launch(moveit_config) 10 | -------------------------------------------------------------------------------- /ros_trick/ros_src/trick_canadarm_moveit_config/launch/spawn_controllers.launch.py: -------------------------------------------------------------------------------- 1 | from moveit_configs_utils import MoveItConfigsBuilder 2 | from moveit_configs_utils.launches import generate_spawn_controllers_launch 3 | 4 | 5 | def generate_launch_description(): 6 | moveit_config = MoveItConfigsBuilder( 7 | "SSRMS_Canadarm2", package_name="trick_canadarm_moveit_config" 8 | ).to_moveit_configs() 9 | return generate_spawn_controllers_launch(moveit_config) 10 | -------------------------------------------------------------------------------- /ros_trick/ros_src/trick_canadarm_moveit_config/launch/static_virtual_joint_tfs.launch.py: -------------------------------------------------------------------------------- 1 | from moveit_configs_utils import MoveItConfigsBuilder 2 | from moveit_configs_utils.launches import generate_static_virtual_joint_tfs_launch 3 | 4 | 5 | def generate_launch_description(): 6 | moveit_config = MoveItConfigsBuilder( 7 | "SSRMS_Canadarm2", package_name="trick_canadarm_moveit_config" 8 | ).to_moveit_configs() 9 | return generate_static_virtual_joint_tfs_launch(moveit_config) 10 | -------------------------------------------------------------------------------- /ros_trick/ros_src/trick_canadarm_moveit_config/launch/warehouse_db.launch.py: -------------------------------------------------------------------------------- 1 | from moveit_configs_utils import MoveItConfigsBuilder 2 | from moveit_configs_utils.launches import generate_warehouse_db_launch 3 | 4 | 5 | def generate_launch_description(): 6 | moveit_config = MoveItConfigsBuilder( 7 | "SSRMS_Canadarm2", package_name="trick_canadarm_moveit_config" 8 | ).to_moveit_configs() 9 | return generate_warehouse_db_launch(moveit_config) 10 | -------------------------------------------------------------------------------- /ros_trick/ros_src/trick_canadarm_moveit_config/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | trick_canadarm_moveit_config 5 | 0.3.0 6 | 7 | An automatically generated package with all the configuration and launch files for using the SSRMS_Canadarm2 with the MoveIt Motion Planning Framework 8 | Derived from spaceros/demos/canadarm_moveit_config demo 9 | 10 | Dharini Dutia 11 | 12 | BSD 13 | 14 | Blazej Fiderek(xfiderek) 15 | Dharini Dutia 16 | 17 | ament_cmake 18 | 19 | moveit_ros_move_group 20 | moveit_kinematics 21 | moveit_planners 22 | moveit_simple_controller_manager 23 | joint_state_publisher 24 | joint_state_publisher_gui 25 | tf2_ros 26 | xacro 27 | controller_manager 28 | moveit_configs_utils 29 | moveit_ros_move_group 30 | moveit_ros_visualization 31 | moveit_ros_warehouse 32 | moveit_setup_assistant 33 | robot_state_publisher 34 | rviz2 35 | rviz_common 36 | rviz_default_plugins 37 | simulation 38 | tf2_ros 39 | warehouse_ros_mongo 40 | xacro 41 | 42 | 43 | 44 | ament_cmake 45 | 46 | 47 | -------------------------------------------------------------------------------- /ros_trick/ros_src/trick_ros2_control/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(trick_ros2_control) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | # find dependencies 9 | find_package(ament_cmake REQUIRED) 10 | find_package(hardware_interface REQUIRED) 11 | find_package(pluginlib REQUIRED) 12 | find_package(rclcpp REQUIRED) 13 | find_package(rclcpp_lifecycle REQUIRED) 14 | find_package(realtime_tools REQUIRED) 15 | 16 | 17 | add_library( 18 | trick_ros2_control 19 | SHARED 20 | src/trick_system.cpp 21 | src/socket.cpp 22 | ) 23 | target_include_directories( 24 | trick_ros2_control 25 | PUBLIC 26 | include 27 | ) 28 | ament_target_dependencies( 29 | trick_ros2_control 30 | hardware_interface 31 | rclcpp 32 | rclcpp_lifecycle 33 | realtime_tools 34 | ) 35 | # prevent pluginlib from using boost 36 | target_compile_definitions(trick_ros2_control PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") 37 | 38 | pluginlib_export_plugin_description_file( 39 | hardware_interface trick_ros2_control.xml) 40 | 41 | install( 42 | TARGETS 43 | trick_ros2_control 44 | RUNTIME DESTINATION bin 45 | ARCHIVE DESTINATION lib 46 | LIBRARY DESTINATION lib 47 | ) 48 | 49 | install( 50 | DIRECTORY include/ 51 | DESTINATION include 52 | ) 53 | 54 | if(BUILD_TESTING) 55 | find_package(ament_lint_auto REQUIRED) 56 | # the following line skips the linter which checks for copyrights 57 | # comment the line when a copyright and license is added to all source files 58 | set(ament_cmake_copyright_FOUND TRUE) 59 | # the following line skips cpplint (only works in a git repo) 60 | # comment the line when this package is in a git repo and when 61 | # a copyright and license is added to all source files 62 | set(ament_cmake_cpplint_FOUND TRUE) 63 | ament_lint_auto_find_test_dependencies() 64 | find_package(ament_cmake_gmock REQUIRED) 65 | find_package(ros2_control_test_assets REQUIRED) 66 | endif() 67 | 68 | ament_export_include_directories( 69 | include 70 | ) 71 | ament_export_libraries( 72 | trick_ros2_control 73 | ) 74 | ament_export_dependencies( 75 | hardware_interface 76 | pluginlib 77 | rclcpp 78 | rclcpp_lifecycle 79 | ) 80 | 81 | ament_package() 82 | -------------------------------------------------------------------------------- /ros_trick/ros_src/trick_ros2_control/include/trick_ros2_control/socket.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2024 Blazej Fiderek (xfiderek) 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // https://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | // Largely inspired by 16 | // https://github.com/nasa/trick/blob/master/trick_sims/SIM_billiards/models/graphics/cpp/Socket.cpp 17 | #ifndef TRICK_ROS2_CONTROL__SOCKET_HPP_ 18 | #define TRICK_ROS2_CONTROL__SOCKET_HPP_ 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #define SOCKET_BUF_SIZE 20480 29 | 30 | class Socket 31 | { 32 | public: 33 | Socket(); 34 | int init(std::string hostname, int port); 35 | 36 | int send(std::string message); 37 | int operator<<(std::string message); 38 | 39 | std::string receive(); 40 | void operator>>(std::string& ret); 41 | 42 | private: 43 | int _port; 44 | std::string _hostname; 45 | int _socket_fd; 46 | bool _initialized; 47 | char carry_on_buffer_[SOCKET_BUF_SIZE] = { '\0' }; 48 | char trick_delimiter_ = '\n'; 49 | }; 50 | 51 | #endif 52 | -------------------------------------------------------------------------------- /ros_trick/ros_src/trick_ros2_control/include/trick_ros2_control/utils.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2024 Blazej Fiderek (xfiderek) 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // https://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | double trick_string_convert(const std::string& str) 20 | { 21 | std::istringstream ss(str); 22 | double num; 23 | ss >> num; 24 | return num; 25 | } 26 | 27 | std::vector trick_split_response(std::string& str, const char delim) 28 | { 29 | std::stringstream ss(str); 30 | std::string s; 31 | std::vector ret; 32 | while (ss >> s) 33 | { 34 | ret.push_back(s); 35 | } 36 | return ret; 37 | } 38 | 39 | std::vector trick_response_convert(std::string& response) 40 | { 41 | auto responseSplit = trick_split_response(response, '\t'); 42 | std::vector result; 43 | if (responseSplit[0] != "0") 44 | { 45 | return result; 46 | } 47 | for (int i = 1; i < responseSplit.size(); i++) 48 | { 49 | result.push_back(trick_string_convert(responseSplit[i])); 50 | } 51 | return result; 52 | } 53 | -------------------------------------------------------------------------------- /ros_trick/ros_src/trick_ros2_control/include/trick_ros2_control/visibility_control.h: -------------------------------------------------------------------------------- 1 | // Copyright 2024 Blazej Fiderek (xfiderek) 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // https://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | 16 | #ifndef TRICK_ROS2_CONTROL__VISIBILITY_CONTROL_H_ 17 | #define TRICK_ROS2_CONTROL__VISIBILITY_CONTROL_H_ 18 | 19 | // This logic was borrowed (then namespaced) from the examples on the gcc wiki: 20 | // https://gcc.gnu.org/wiki/Visibility 21 | 22 | #if defined _WIN32 || defined __CYGWIN__ 23 | #ifdef __GNUC__ 24 | #define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __attribute__((dllexport)) 25 | #define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT __attribute__((dllimport)) 26 | #else 27 | #define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT __declspec(dllexport) 28 | #define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT __declspec(dllimport) 29 | #endif 30 | #ifdef TEMPLATES__ROS2_CONTROL__VISIBILITY_BUILDING_DLL 31 | #define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC \ 32 | TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT 33 | #else 34 | #define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC \ 35 | TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT 36 | #endif 37 | #define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE \ 38 | TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC 39 | #define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL 40 | #else 41 | #define TEMPLATES__ROS2_CONTROL__VISIBILITY_EXPORT \ 42 | __attribute__((visibility("default"))) 43 | #define TEMPLATES__ROS2_CONTROL__VISIBILITY_IMPORT 44 | #if __GNUC__ >= 4 45 | #define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC \ 46 | __attribute__((visibility("default"))) 47 | #define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL \ 48 | __attribute__((visibility("hidden"))) 49 | #else 50 | #define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC 51 | #define TEMPLATES__ROS2_CONTROL__VISIBILITY_LOCAL 52 | #endif 53 | #define TEMPLATES__ROS2_CONTROL__VISIBILITY_PUBLIC_TYPE 54 | #endif 55 | 56 | #endif // TRICK_ROS2_CONTROL__VISIBILITY_CONTROL_H_ 57 | -------------------------------------------------------------------------------- /ros_trick/ros_src/trick_ros2_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | trick_ros2_control 5 | 0.0.0 6 | TODO: Package description 7 | blazej 8 | TODO: License declaration 9 | 10 | ament_cmake 11 | 12 | hardware_interface 13 | 14 | pluginlib 15 | 16 | rclcpp 17 | 18 | rclcpp_lifecycle 19 | realtime_tools 20 | 21 | ament_lint_auto 22 | ament_lint_common 23 | ament_cmake_gmock 24 | ros2_control_test_assets 25 | 26 | 27 | ament_cmake 28 | 29 | 30 | -------------------------------------------------------------------------------- /ros_trick/ros_src/trick_ros2_control/src/socket.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2024 Blazej Fiderek (xfiderek) 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // https://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | // Largely inspired by 16 | // https://github.com/nasa/trick/blob/master/trick_sims/SIM_billiards/models/graphics/cpp/Socket.cpp 17 | 18 | #include "trick_ros2_control/socket.hpp" 19 | 20 | Socket::Socket() : _initialized(false) 21 | { 22 | } 23 | 24 | int Socket::init(std::string hostname, int port) 25 | { 26 | _hostname = hostname; 27 | _port = port; 28 | _socket_fd = socket(AF_INET, SOCK_STREAM, 0); 29 | if (_socket_fd < 0) 30 | { 31 | std::cout << "Socket connection failed" << std::endl; 32 | return -1; 33 | } 34 | 35 | struct sockaddr_in serv_addr; 36 | serv_addr.sin_family = AF_INET; 37 | serv_addr.sin_port = htons(port); 38 | 39 | if (inet_pton(AF_INET, "127.0.0.1", &serv_addr.sin_addr) <= 0) 40 | { 41 | std::cout << "Invalid address/ Address not supported" << std::endl; 42 | return -1; 43 | } 44 | 45 | if (connect(_socket_fd, (struct sockaddr*)&serv_addr, sizeof(serv_addr)) < 0) 46 | { 47 | std::cout << "Connection failed" << std::endl; 48 | return -1; 49 | } 50 | 51 | _initialized = true; 52 | return 0; 53 | } 54 | 55 | int Socket::send(std::string message) 56 | { 57 | int success = ::send(_socket_fd, message.c_str(), message.size(), 0); 58 | if (success < static_cast(message.size())) 59 | { 60 | std::cout << "Failed to send message" << std::endl; 61 | } 62 | return success; 63 | } 64 | 65 | int Socket::operator<<(std::string message) 66 | { 67 | return send(message); 68 | } 69 | 70 | std::string Socket::receive() 71 | { 72 | char buffer[SOCKET_BUF_SIZE]; 73 | int numBytes = read(_socket_fd, buffer, SOCKET_BUF_SIZE); 74 | if (numBytes < 0) 75 | { 76 | throw std::runtime_error("Failed to read from socket\n"); 77 | } 78 | else if (numBytes < SOCKET_BUF_SIZE) 79 | { 80 | buffer[numBytes] = '\0'; 81 | } 82 | 83 | std::string string_buffer = std::string(carry_on_buffer_) + std::string(buffer); 84 | int last_newline_idx = string_buffer.rfind(trick_delimiter_, string_buffer.size() - 1); 85 | int second_last_newline_idx = string_buffer.rfind(trick_delimiter_, string_buffer.size() - 2); 86 | 87 | // it means that we can clear carry on buffer 88 | if (last_newline_idx == string_buffer.size() - 1) 89 | { 90 | carry_on_buffer_[0] = '\0'; 91 | } 92 | // in this case we need to store the remaining characters in the carry on 93 | // buffer 94 | else 95 | { 96 | std::strcpy(carry_on_buffer_, string_buffer.substr(last_newline_idx + 1).c_str()); 97 | } 98 | 99 | // Always return the latest message 100 | return string_buffer.substr(second_last_newline_idx + 1, last_newline_idx - second_last_newline_idx - 1); 101 | } 102 | 103 | void Socket::operator>>(std::string& ret) 104 | { 105 | ret = receive(); 106 | } 107 | -------------------------------------------------------------------------------- /ros_trick/ros_src/trick_ros2_control/trick_ros2_control.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | ros2_control hardware interface. 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /ros_trick/ros_trick_bridge.Dockerfile: -------------------------------------------------------------------------------- 1 | # Copyright 2024 Blazej Fiderek (xfiderek) 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # https://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | FROM osrf/space-ros:humble-2024.10.0 16 | 17 | # Install trick dependencies. 18 | RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ 19 | --mount=type=cache,target=/var/lib/apt,sharing=locked \ 20 | sudo apt-get update && sudo apt-get install -y \ 21 | bison \ 22 | clang \ 23 | cmake \ 24 | curl \ 25 | flex \ 26 | g++ \ 27 | git \ 28 | libclang-dev \ 29 | libgtest-dev \ 30 | libmotif-common \ 31 | libmotif-dev \ 32 | libudunits2-dev \ 33 | libx11-dev \ 34 | libxml2-dev \ 35 | libxt-dev \ 36 | llvm \ 37 | llvm-dev \ 38 | make \ 39 | maven \ 40 | openjdk-11-jdk \ 41 | python3-dev \ 42 | swig \ 43 | zip \ 44 | zlib1g-dev 45 | 46 | ENV PYTHON_VERSION=3 47 | 48 | # Get Trick version 19.7.2 from GitHub, configure and build it. 49 | RUN mkdir ${HOME_DIR}/trick 50 | WORKDIR ${HOME_DIR}/trick 51 | RUN git clone --branch 19.7.2 --depth 1 https://github.com/nasa/trick.git . 52 | RUN ./configure && make 53 | 54 | # Add ${TRICK_HOME}/bin to the PATH variable. 55 | ENV TRICK_HOME="${HOME_DIR}/trick" 56 | RUN echo "export PATH=${PATH}:${TRICK_HOME}/bin" >> ~/.bashrc 57 | 58 | # Build SPACEROS workspace with ros trick bridge. 59 | RUN mkdir ${HOME_DIR}/ros_trick_bridge_ws 60 | WORKDIR ${HOME_DIR}/ros_trick_bridge_ws 61 | ENV ROS_TRICK_BRIDGE_WS="${HOME_DIR}/ros_trick_bridge_ws/" 62 | COPY --chown=spaceros-user:spaceros-user ros_src/ros_trick_bridge src/ 63 | RUN /bin/bash -c 'source ${SPACEROS_DIR}/install/setup.bash \ 64 | && colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release -DCMAKE_EXPORT_COMPILE_COMMANDS=ON --event-handlers desktop_notification- status-' 65 | RUN rm -rf build log src 66 | 67 | # The parts below are only required for the canadarm demo. Replace them as you 68 | # see fit. 69 | 70 | # Install RBDL, which is used for calculating forward dynamics in trick. 71 | RUN mkdir ${HOME_DIR}/rbdl 72 | WORKDIR ${HOME_DIR}/rbdl 73 | RUN git clone --branch v3.3.0 --depth 1 https://github.com/rbdl/rbdl.git . \ 74 | && git submodule update --init --remote --depth 1 addons/urdfreader/ 75 | RUN mkdir ./rbdl-build \ 76 | && cd rbdl-build/ \ 77 | && cmake \ 78 | -DCMAKE_BUILD_TYPE=Release \ 79 | -DRBDL_BUILD_ADDON_URDFREADER="ON" \ 80 | .. \ 81 | && make \ 82 | && sudo make install \ 83 | && cd .. \ 84 | && rm -r ./rbdl-build 85 | # Make it easy to link the rbdl library. 86 | # LD_LIBRARY_PATH is empty at this stage of dockerbuild 87 | ENV LD_LIBRARY_PATH="/usr/local/lib" 88 | 89 | # Copy the created canadarm trick simulation and compile it. 90 | RUN mkdir -p ${HOME_DIR}/trick_sims/SIM_trick_canadarm 91 | WORKDIR ${HOME_DIR}/trick_sims/SIM_trick_canadarm 92 | COPY --chown=spaceros-user:spaceros-user trick_src/SIM_trick_canadarm/ . 93 | RUN ${TRICK_HOME}/bin/trick-CP 94 | # Include canadarm URDF for RBDL 95 | COPY --chown=spaceros-user:spaceros-user ros_src/trick_canadarm_moveit_config/config/SSRMS_Canadarm2.urdf.xacro . 96 | 97 | WORKDIR ${ROS_TRICK_BRIDGE_WS} 98 | -------------------------------------------------------------------------------- /ros_trick/run.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # Copyright 2024 Blazej Fiderek (xfiderek) 3 | # 4 | # Licensed under the Apache License, Version 2.0 (the "License"); 5 | # you may not use this file except in compliance with the License. 6 | # You may obtain a copy of the License at 7 | # 8 | # https://www.apache.org/licenses/LICENSE-2.0 9 | # 10 | # Unless required by applicable law or agreed to in writing, software 11 | # distributed under the License is distributed on an "AS IS" BASIS, 12 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | # See the License for the specific language governing permissions and 14 | # limitations under the License. 15 | 16 | set -e 17 | docker compose up --build 18 | -------------------------------------------------------------------------------- /ros_trick/spaceros_docker_repo.patch: -------------------------------------------------------------------------------- 1 | diff --git a/moveit2/Dockerfile b/moveit2/Dockerfile 2 | index d5e095f..80e4514 100644 3 | --- a/moveit2/Dockerfile 4 | +++ b/moveit2/Dockerfile 5 | @@ -94,13 +94,15 @@ RUN python3 -m pip install -U \ 6 | 7 | # Get the MoveIt2 source code 8 | WORKDIR ${HOME_DIR} 9 | -RUN sudo git clone https://github.com/ros-planning/moveit2.git -b ${ROS_DISTRO} moveit2/src 10 | +RUN sudo git clone https://github.com/ros-planning/moveit2.git ${MOVEIT2_DIR}/src \ 11 | + && sudo chown -R ${USERNAME}:${USERNAME} ${MOVEIT2_DIR}/src \ 12 | + && cd moveit2/src \ 13 | + && git checkout 69c62f8114968c41857543d615a97eb36fdbaa97 14 | RUN cd ${MOVEIT2_DIR}/src \ 15 | - && sudo git clone https://github.com/ros-planning/moveit2_tutorials.git -b ${ROS_DISTRO} 16 | - 17 | -# Update the ownership of the source files (had to use sudo above to work around 18 | -# a possible inherited 'insteadof' from the host that forces use of ssh 19 | -RUN sudo chown -R ${USERNAME}:${USERNAME} ${MOVEIT2_DIR} 20 | +&& sudo git clone https://github.com/ros-planning/moveit2_tutorials.git \ 21 | +&& sudo chown -R ${USERNAME}:${USERNAME} moveit2_tutorials \ 22 | +&& cd moveit2_tutorials \ 23 | +&& git checkout 3e6576410e3322c86023ce9b35359fc73afc0de5 24 | 25 | # Get rosinstall_generator 26 | RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ 27 | @@ -134,10 +136,6 @@ RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ 28 | /bin/bash -c 'source ${SPACEROS_DIR}/install/setup.bash' \ 29 | && rosdep install --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} -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" 30 | 31 | -# Apply a patch to octomap_msgs to work around a build issue 32 | -COPY --chown=${USERNAME}:${USERNAME} octomap_fix.diff ./src/octomap_msgs 33 | -RUN cd src/octomap_msgs && git apply octomap_fix.diff 34 | - 35 | # Build MoveIt2 36 | RUN /bin/bash -c 'source ${SPACEROS_DIR}/install/setup.bash \ 37 | && colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release -DCMAKE_EXPORT_COMPILE_COMMANDS=ON --event-handlers desktop_notification- status-' 38 | -------------------------------------------------------------------------------- /ros_trick/trick_src/SIM_trick_canadarm/RUN_2DPlanar/input.py: -------------------------------------------------------------------------------- 1 | # Copyright 2024 Blazej Fiderek (xfiderek) 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # https://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | 16 | trick.real_time_enable() 17 | trick.exec_set_software_frame(0.02) 18 | 19 | trick.exec_set_enable_freeze(True) 20 | trick.exec_set_freeze_command(True) 21 | 22 | 23 | trick.var_set_copy_mode(2) 24 | trick.var_server_set_port(49765) 25 | trick_message.mtcout.init() 26 | trick.message_subscribe(trick_message.mtcout) 27 | trick_message.separate_thread_set_enabled(True) 28 | simControlPanel = trick.SimControlPanel() 29 | trick.add_external_application(simControlPanel) 30 | trick_view = trick.TrickView() 31 | trick.add_external_application(trick_view) 32 | -------------------------------------------------------------------------------- /ros_trick/trick_src/SIM_trick_canadarm/S_define: -------------------------------------------------------------------------------- 1 | // Copyright 2024 Blazej Fiderek (xfiderek) 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // https://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | /************************TRICK HEADER************************* 16 | PURPOSE: 17 | ( Simulate dynamics of SSRMS ) 18 | LIBRARY DEPENDENCIES: 19 | ( 20 | (manipulator/manipulator.cc) 21 | ) 22 | *************************************************************/ 23 | #include "sim_objects/default_trick_sys.sm" 24 | 25 | ##include "include/trick/exec_proto.h" 26 | ##include "manipulator/manipulator.hh" 27 | 28 | class ManipulatorSimObject : public Trick::SimObject 29 | { 30 | 31 | public: 32 | Manip manip; 33 | 34 | ManipulatorSimObject(): manip() 35 | { 36 | ("default_data") manip.defaultData(); 37 | ("derivative") manip.stateDeriv(); 38 | ("integration") trick_ret = manip.stateInteg(); 39 | } 40 | 41 | }; 42 | 43 | ManipulatorSimObject CanadarmManip; 44 | 45 | IntegLoop armIntegLoop(0.01) CanadarmManip; 46 | 47 | void create_connections() { 48 | armIntegLoop.getIntegrator(Runge_Kutta_4, 2*CanadarmManip.manip.NDOF); 49 | } 50 | -------------------------------------------------------------------------------- /ros_trick/trick_src/SIM_trick_canadarm/S_overrides.mk: -------------------------------------------------------------------------------- 1 | # Copyright 2024 Blazej Fiderek (xfiderek) 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # https://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | TRICK_CFLAGS += -I./models -I${TRICK_HOME} -I/usr/include/eigen3 -g -O0 -Wall -Wextra -Wshadow 16 | TRICK_CXXFLAGS += -I./models -I${TRICK_HOME} -I/usr/include/eigen3 -g -O0 -Wall -Wextra -Wshadow 17 | TRICK_USER_LINK_LIBS += -lrbdl -lrbdl_urdfreader 18 | 19 | -------------------------------------------------------------------------------- /ros_trick/trick_src/SIM_trick_canadarm/models/manipulator/manipulator.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2024 Blazej Fiderek (xfiderek) 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // https://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "manipulator/manipulator.hh" 16 | #include "trick/integrator_c_intf.h" 17 | 18 | Manip::Manip() : rbdl_model() 19 | { 20 | RigidBodyDynamics::Addons::URDFReadFromFile("./SSRMS_Canadarm2.urdf.xacro", &rbdl_model, false); 21 | std::cout << "Loaded URDF model into trick"; 22 | std::cout << RigidBodyDynamics::Utils::GetModelDOFOverview(rbdl_model); 23 | 24 | rbdl_model.gravity = RigidBodyDynamics::Math::Vector3d::Zero(); 25 | 26 | q_vec_ = RigidBodyDynamics::Math::VectorNd::Zero(rbdl_model.q_size); 27 | q_dotvec_ = RigidBodyDynamics::Math::VectorNd::Zero(rbdl_model.q_size); 28 | q_ddotvec_ = RigidBodyDynamics::Math::VectorNd::Zero(rbdl_model.q_size); 29 | torque_vec_ = RigidBodyDynamics::Math::VectorNd::Zero(rbdl_model.q_size); 30 | 31 | std::cout << "starting Trick Canadarm simulation" << std::endl; 32 | } 33 | 34 | int Manip::defaultData() 35 | { 36 | for (size_t i = 0; i < NDOF; ++i) 37 | { 38 | q[i] = 0.0; 39 | q_dot[i] = 0.0; 40 | q_dotdot[i] = 0.0; 41 | input_torque[i] = 0.0; 42 | applied_torque[i] = 0.0; 43 | friction_torque[i] = 0.0; 44 | q_vec_[i] = 0.0; 45 | q_dotvec_[i] = 0.0; 46 | q_ddotvec_[i] = 0.0; 47 | torque_vec_[i] = 0.0; 48 | breakaway_friction_torque[i] = 2.5; 49 | coloumb_friction_torque[i] = 2.0; 50 | breakaway_friction_velocity[i] = 0.005; 51 | coulomb_velocity_threshold[i] = breakaway_friction_velocity[i] / 10; 52 | stribeck_velocity_threshold[i] = M_SQRT2 * breakaway_friction_torque[i]; 53 | viscous_friction_coefficient[i] = 0.001; 54 | } 55 | 56 | return (0); 57 | } 58 | 59 | int Manip::forwardDynamics() 60 | { 61 | // calculate torques and store as RBDL vector as well 62 | for (size_t i = 0; i < NDOF; ++i) 63 | { 64 | // https://www.mathworks.com/help/simscape/ref/rotationalfriction.html 65 | double curr_to_stribeck_vel = q_dot[i] / stribeck_velocity_threshold[i]; 66 | double curr_to_coulomb_vel = q_dot[i] / coulomb_velocity_threshold[i]; 67 | friction_torque[i] = M_SQRT2 * M_E * (breakaway_friction_torque[i] - coloumb_friction_torque[i]) * 68 | std::exp(-std::pow(curr_to_stribeck_vel, 2)) * curr_to_stribeck_vel; 69 | friction_torque[i] += coloumb_friction_torque[i] * std::tanh(curr_to_coulomb_vel); 70 | friction_torque[i] += viscous_friction_coefficient[i] * q_dot[i]; 71 | 72 | applied_torque[i] = input_torque[i] - friction_torque[i]; 73 | 74 | // copy positions and velocities from C-arrays to RBDL vectors 75 | torque_vec_[i] = applied_torque[i]; 76 | q_vec_[i] = q[i]; 77 | q_dotvec_[i] = q_dot[i]; 78 | q_ddotvec_[i] = 0.0; 79 | } 80 | // calculate dynamics (q_dotdot) with RBDL 81 | RigidBodyDynamics::ForwardDynamics(rbdl_model, q_vec_, q_dotvec_, torque_vec_, q_ddotvec_); 82 | 83 | // copy Q_dotdot from RBDL vector type to simple C-arrays 84 | for (size_t i = 0; i < NDOF; ++i) 85 | { 86 | q_dotdot[i] = q_ddotvec_[i]; 87 | } 88 | 89 | return (0); 90 | } 91 | 92 | int Manip::stateDeriv() 93 | { 94 | int status_code = forwardDynamics(); 95 | return (status_code); 96 | } 97 | 98 | int Manip::stateInteg() 99 | { 100 | int integration_step; 101 | load_state(&q[0], &q[1], &q[2], &q[3], &q[4], &q[5], &q[6], &q_dot[0], &q_dot[1], &q_dot[2], &q_dot[3], &q_dot[4], 102 | &q_dot[5], &q_dot[6], NULL); 103 | load_deriv(&q_dot[0], &q_dot[1], &q_dot[2], &q_dot[3], &q_dot[4], &q_dot[5], &q_dot[6], &q_dotdot[0], &q_dotdot[1], 104 | &q_dotdot[2], &q_dotdot[3], &q_dotdot[4], &q_dotdot[5], &q_dotdot[6], NULL); 105 | // integrate 106 | integration_step = integrate(); 107 | 108 | unload_state(&q[0], &q[1], &q[2], &q[3], &q[4], &q[5], &q[6], &q_dot[0], &q_dot[1], &q_dot[2], &q_dot[3], &q_dot[4], 109 | &q_dot[5], &q_dot[6], NULL); 110 | 111 | return (integration_step); 112 | } 113 | -------------------------------------------------------------------------------- /ros_trick/trick_src/SIM_trick_canadarm/models/manipulator/manipulator.hh: -------------------------------------------------------------------------------- 1 | // Copyright 2024 Blazej Fiderek (xfiderek) 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // https://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | 16 | #ifndef __MANIPULATOR_HH_ 17 | #define __MANIPULATOR_HH_ 18 | /************************************************************************** 19 | PURPOSE: (2D Manipulator class definitions including kinematics and control) 20 | ***************************************************************************/ 21 | #define TRICK_NO_MONTE_CARLO 22 | #define TRICK_NO_MASTERSLAVE 23 | #define TRICK_NO_INSTRUMENTATION 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | class Manip 33 | { 34 | public: 35 | Manip(); 36 | 37 | /** 38 | * @brief Calculate Q_dotdot (joint accelerations) given current state and input torques 39 | * The dynamics is calculated using Articulated Body Algorithm (ABA) with RBDL library 40 | * Friction is modelled with Stribeck model taken from Mathworks docs 41 | * https://www.mathworks.com/help/simscape/ref/rotationalfriction.html 42 | * @return int status code 43 | */ 44 | int forwardDynamics(); 45 | 46 | /** 47 | * @brief Calculate state derivative. Calls forwardDynamics(). 48 | * 49 | * @return int status code 50 | */ 51 | int stateDeriv(); 52 | /** 53 | * @brief Propagate state for the current timestamp by integration. Called after stateDeriv() 54 | * 55 | * @return int status code 56 | */ 57 | int stateInteg(); 58 | 59 | /** 60 | * @brief Initializes all data structures. Called on startup. 61 | * 62 | * @return int status code 63 | */ 64 | int defaultData(); 65 | 66 | /** 67 | * @brief Model encapsulating kinematics and dynamics of Canadarm with RBDL library 68 | * 69 | */ 70 | RigidBodyDynamics::Model rbdl_model; /* ** -- class from RBDL that calculates forward dynamics */ 71 | static const size_t NDOF = 7; /* ** -- ndof */ 72 | 73 | double input_torque[NDOF] = { 0.0 }; /* *i (N.m) input (commanded) torque for each joint */ 74 | 75 | double q[NDOF] = { 0.0 }; /* *o rad angle of joints */ 76 | double q_dot[NDOF] = { 0.0 }; /* *o (rad/s) velocity of joints */ 77 | double q_dotdot[NDOF] = { 0.0 }; /* *o (rad/s^2) accelerations of joints */ 78 | double friction_torque[NDOF] = { 0.0 }; /* *o (N.m) Torque comming from friction */ 79 | double applied_torque[NDOF] = { 0.0 }; /* *o (N.m) final torque applied for each joint */ 80 | 81 | // Friction-related parameters. For now we assume same params for every joint 82 | // The friction is modelled using Stribeck function 83 | // https://www.mathworks.com/help/simscape/ref/rotationalfriction.html 84 | double breakaway_friction_torque[NDOF]; /* *i (N.m) */ 85 | double coloumb_friction_torque[NDOF]; /* *i (N.m) */ 86 | double breakaway_friction_velocity[NDOF]; /* *i (rad/s) */ 87 | double coulomb_velocity_threshold[NDOF]; /* *i (rad.s) */ 88 | double stribeck_velocity_threshold[NDOF]; /* *i (N.m/rad.s) */ 89 | double viscous_friction_coefficient[NDOF]; /* *i (N.m/rad.s) */ 90 | 91 | private: 92 | RigidBodyDynamics::Math::VectorNd q_vec_; 93 | RigidBodyDynamics::Math::VectorNd q_dotvec_; 94 | RigidBodyDynamics::Math::VectorNd q_ddotvec_; 95 | RigidBodyDynamics::Math::VectorNd torque_vec_; 96 | }; 97 | #endif 98 | -------------------------------------------------------------------------------- /space_ros_memory_allocation_demo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(space_ros_memory_allocation_demo) 3 | 4 | if(NOT CMAKE_CXX_STANDARD) 5 | set(CMAKE_CXX_STANDARD 11) 6 | endif() 7 | 8 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 9 | add_compile_options(-Wall -Wextra -Wpedantic) 10 | endif() 11 | 12 | # find dependencies 13 | find_package(ament_cmake REQUIRED) 14 | find_package(rcutils REQUIRED) 15 | find_package(rclcpp REQUIRED) 16 | find_package(rclcpp_components REQUIRED) 17 | find_package(std_msgs REQUIRED) 18 | find_package(osrf_testing_tools_cpp REQUIRED) 19 | 20 | add_executable(talker_main src/talker_main.cpp) 21 | target_include_directories(talker_main PUBLIC 22 | "$" 23 | "$") 24 | target_link_libraries(talker_main osrf_testing_tools_cpp::memory_tools) 25 | ament_target_dependencies(talker_main 26 | "rclcpp" 27 | "rclcpp_components" 28 | "rcutils" 29 | "std_msgs") 30 | 31 | add_executable(listener_main src/listener_main.cpp) 32 | target_include_directories(listener_main PUBLIC 33 | "$" 34 | "$") 35 | target_link_libraries(listener_main osrf_testing_tools_cpp::memory_tools) 36 | ament_target_dependencies(listener_main 37 | "rclcpp" 38 | "rclcpp_components" 39 | "rcutils" 40 | "std_msgs") 41 | 42 | add_library(${PROJECT_NAME}_library SHARED src/talker_library.cpp src/listener_library.cpp) 43 | target_include_directories(${PROJECT_NAME}_library PUBLIC 44 | "$" 45 | "$") 46 | target_compile_definitions(${PROJECT_NAME}_library 47 | PRIVATE "SPACE_ROS_MEMORY_ALLOCATION_DEMO_BUILDING_DLL") 48 | target_link_libraries(${PROJECT_NAME}_library osrf_testing_tools_cpp::memory_tools) 49 | ament_target_dependencies(${PROJECT_NAME}_library 50 | "rclcpp" 51 | "rclcpp_components" 52 | "rcutils" 53 | "std_msgs") 54 | 55 | rclcpp_components_register_node(${PROJECT_NAME}_library 56 | PLUGIN "space_ros_memory_allocation_demo::Talker" 57 | EXECUTABLE talker) 58 | rclcpp_components_register_node(${PROJECT_NAME}_library 59 | PLUGIN "space_ros_memory_allocation_demo::Listener" 60 | EXECUTABLE listener) 61 | 62 | if(BUILD_TESTING) 63 | find_package(ament_lint_auto REQUIRED) 64 | # the following line skips the linter which checks for copyrights 65 | # comment the line when a copyright and license is added to all source files 66 | set(ament_cmake_copyright_FOUND TRUE) 67 | # the following line skips cpplint (only works in a git repo) 68 | # comment the line when this package is in a git repo and when 69 | # a copyright and license is added to all source files 70 | set(ament_cmake_cpplint_FOUND TRUE) 71 | ament_lint_auto_find_test_dependencies() 72 | endif() 73 | 74 | ament_package() 75 | 76 | install(TARGETS 77 | ${PROJECT_NAME}_library 78 | ARCHIVE DESTINATION lib 79 | LIBRARY DESTINATION lib 80 | RUNTIME DESTINATION bin) 81 | 82 | install(TARGETS talker_main listener_main 83 | DESTINATION lib/${PROJECT_NAME}) 84 | 85 | install( 86 | DIRECTORY include/ 87 | DESTINATION include/${PROJECT_NAME}) 88 | 89 | # Install launch files. 90 | install(DIRECTORY 91 | launch 92 | DESTINATION share/${PROJECT_NAME}/ 93 | ) 94 | -------------------------------------------------------------------------------- /space_ros_memory_allocation_demo/COLCON_IGNORE: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/space-ros/demos/d82db232718d4a8e07f593c54f1f2137a84c0886/space_ros_memory_allocation_demo/COLCON_IGNORE -------------------------------------------------------------------------------- /space_ros_memory_allocation_demo/include/space_ros_memory_allocation_demo/memory_allocator.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef SPACE_ROS_MEMORY_ALLOCATION_DEMO__MEMORY_ALLOCATOR_HPP_ 16 | #define SPACE_ROS_MEMORY_ALLOCATION_DEMO__MEMORY_ALLOCATOR_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | // Provides std::pmr, accounting for differences in different platforms. 23 | #include "space_ros_memory_allocation_demo/memory_resource_helper.hpp" 24 | 25 | namespace space_ros_memory_allocation_demo 26 | { 27 | 28 | const unsigned int SMALL_MEM = 128; 29 | const unsigned int MEDIUM_MEM = 4096; 30 | const unsigned int LARGE_MEM = 32768; 31 | 32 | template 33 | using MEMORY_BUFFER = std::array; 34 | 35 | template 36 | using PREALLOCATE = std::array; 37 | 38 | 39 | class MemoryAllocator : public std::pmr::memory_resource 40 | { 41 | public: 42 | MemoryAllocator( 43 | // std::string name, 44 | unsigned char * buffer, 45 | size_t buffer_size) 46 | : // m_name(std::move(name)), 47 | monotonic(buffer, buffer_size, std::pmr::null_memory_resource()), 48 | memory_pool(&monotonic) 49 | {} 50 | 51 | private: 52 | // std::string m_name; 53 | // std::pmr::memory_resource * m_upstream; 54 | 55 | // Declare memory in the class stack 56 | std::pmr::monotonic_buffer_resource monotonic; 57 | std::pmr::unsynchronized_pool_resource memory_pool; 58 | 59 | void * 60 | do_allocate(std::size_t bytes, std::size_t alignment) override 61 | { 62 | auto result = memory_pool.allocate(bytes, alignment); 63 | return result; 64 | } 65 | 66 | void 67 | do_deallocate(void * p, std::size_t bytes, std::size_t alignment) override 68 | { 69 | memory_pool.deallocate(p, bytes, alignment); 70 | } 71 | 72 | bool 73 | do_is_equal(const memory_resource & other) const noexcept override 74 | { 75 | return this == &other; 76 | } 77 | }; 78 | 79 | } // namespace space_ros_memory_allocation_demo 80 | 81 | #endif // SPACE_ROS_MEMORY_ALLOCATION_DEMO__MEMORY_ALLOCATOR_HPP_ 82 | -------------------------------------------------------------------------------- /space_ros_memory_allocation_demo/include/space_ros_memory_allocation_demo/visibility_control.h: -------------------------------------------------------------------------------- 1 | // Copyright 2016 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef SPACE_ROS_MEMORY_ALLOCATION_DEMO__VISIBILITY_CONTROL_H_ 16 | #define SPACE_ROS_MEMORY_ALLOCATION_DEMO__VISIBILITY_CONTROL_H_ 17 | 18 | #ifdef __cplusplus 19 | extern "C" 20 | { 21 | #endif 22 | 23 | // This logic was borrowed (then namespaced) from the examples on the gcc wiki: 24 | // https://gcc.gnu.org/wiki/Visibility 25 | 26 | #if defined _WIN32 || defined __CYGWIN__ 27 | #ifdef __GNUC__ 28 | #define SPACE_ROS_MEMORY_ALLOCATION_DEMO_EXPORT __attribute__ ((dllexport)) 29 | #define SPACE_ROS_MEMORY_ALLOCATION_DEMO_IMPORT __attribute__ ((dllimport)) 30 | #else 31 | #define SPACE_ROS_MEMORY_ALLOCATION_DEMO_EXPORT __declspec(dllexport) 32 | #define SPACE_ROS_MEMORY_ALLOCATION_DEMO_IMPORT __declspec(dllimport) 33 | #endif 34 | #ifdef SPACE_ROS_MEMORY_ALLOCATION_DEMO_BUILDING_DLL 35 | #define SPACE_ROS_MEMORY_ALLOCATION_DEMO_PUBLIC SPACE_ROS_MEMORY_ALLOCATION_DEMO_EXPORT 36 | #else 37 | #define SPACE_ROS_MEMORY_ALLOCATION_DEMO_PUBLIC SPACE_ROS_MEMORY_ALLOCATION_DEMO_IMPORT 38 | #endif 39 | #define SPACE_ROS_MEMORY_ALLOCATION_DEMO_PUBLIC_TYPE SPACE_ROS_MEMORY_ALLOCATION_DEMO_PUBLIC 40 | #define SPACE_ROS_MEMORY_ALLOCATION_DEMO_LOCAL 41 | #else 42 | #define SPACE_ROS_MEMORY_ALLOCATION_DEMO_EXPORT __attribute__ ((visibility("default"))) 43 | #define SPACE_ROS_MEMORY_ALLOCATION_DEMO_IMPORT 44 | #if __GNUC__ >= 4 45 | #define SPACE_ROS_MEMORY_ALLOCATION_DEMO_PUBLIC __attribute__ ((visibility("default"))) 46 | #define SPACE_ROS_MEMORY_ALLOCATION_DEMO_LOCAL __attribute__ ((visibility("hidden"))) 47 | #else 48 | #define SPACE_ROS_MEMORY_ALLOCATION_DEMO_PUBLIC 49 | #define SPACE_ROS_MEMORY_ALLOCATION_DEMO_LOCAL 50 | #endif 51 | #define SPACE_ROS_MEMORY_ALLOCATION_DEMO_PUBLIC_TYPE 52 | #endif 53 | 54 | #ifdef __cplusplus 55 | } 56 | #endif 57 | 58 | #endif // SPACE_ROS_MEMORY_ALLOCATION_DEMO__VISIBILITY_CONTROL_H_ 59 | -------------------------------------------------------------------------------- /space_ros_memory_allocation_demo/launch/talker_listener.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /space_ros_memory_allocation_demo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | space_ros_memory_allocation_demo 7 | 0.1.0 8 | A demo of how the C++ pmr allocator pattern can be used with space-ros and ROS 2. 9 | William Woodall 10 | Apache-2.0 11 | 12 | ament_cmake 13 | 14 | rclcpp 15 | example_interfaces 16 | std_msgs 17 | rclcpp_components 18 | osrf_testing_tools_cpp 19 | 20 | ament_lint_auto 21 | ament_lint_common 22 | 23 | 24 | ament_cmake 25 | 26 | 27 | -------------------------------------------------------------------------------- /space_ros_memory_allocation_demo/src/common_main.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef SPACE_ROS_MEMORY_ALLOCATION_DEMO__COMMON_MAIN_HPP_ 16 | #define SPACE_ROS_MEMORY_ALLOCATION_DEMO__COMMON_MAIN_HPP_ 17 | 18 | #include "rclcpp/rclcpp.hpp" 19 | #include "osrf_testing_tools_cpp/memory_tools/memory_tools.hpp" 20 | 21 | #include "space_ros_memory_allocation_demo/memory_allocator.hpp" 22 | 23 | using space_ros_memory_allocation_demo::MemoryAllocator; 24 | 25 | int 26 | common_main( 27 | int argc, 28 | char const * argv[], 29 | std::function actual_main) 30 | { 31 | setvbuf(stdout, NULL, _IONBF, BUFSIZ); 32 | 33 | std::array buffer; 34 | MemoryAllocator ma(buffer.data(), buffer.size()); 35 | 36 | using osrf_testing_tools_cpp::memory_tools::MemoryToolsService; 37 | auto callback_factory = [](std::string msg, bool should_trace) { 38 | return [msg, should_trace](MemoryToolsService & service) { 39 | // filter out things that come from below the rmw layer 40 | auto st = service.get_stack_trace(); 41 | // const std::regex pattern("/?librmw_\\."); 42 | const std::regex is_fastrtps("fastrtps"); 43 | if (st && st->matches_any_object_filename(is_fastrtps)) { 44 | service.ignore(); 45 | return; 46 | } 47 | // printf("%s\n", msg.c_str()); 48 | if (should_trace) { 49 | service.print_backtrace(); 50 | } 51 | }; 52 | }; 53 | 54 | osrf_testing_tools_cpp::memory_tools::initialize(); 55 | 56 | // osrf_testing_tools_cpp::memory_tools::enable_monitoring_in_all_threads(); 57 | osrf_testing_tools_cpp::memory_tools::enable_monitoring(); 58 | if (!osrf_testing_tools_cpp::memory_tools::is_working()) { 59 | printf("memory_tools not working\n"); 60 | } 61 | osrf_testing_tools_cpp::memory_tools::disable_monitoring_in_all_threads(); 62 | 63 | osrf_testing_tools_cpp::memory_tools::on_unexpected_malloc(callback_factory("malloc", true)); 64 | osrf_testing_tools_cpp::memory_tools::on_unexpected_calloc(callback_factory("calloc", true)); 65 | osrf_testing_tools_cpp::memory_tools::on_unexpected_realloc(callback_factory("realloc", true)); 66 | // osrf_testing_tools_cpp::memory_tools::on_unexpected_free(callback_factory("free", true)); 67 | 68 | osrf_testing_tools_cpp::memory_tools::enable_monitoring(); 69 | 70 | return actual_main(argc, argv, ma); 71 | } 72 | 73 | #endif // SPACE_ROS_MEMORY_ALLOCATION_DEMO__COMMON_MAIN_HPP_ 74 | -------------------------------------------------------------------------------- /space_ros_memory_allocation_demo/src/expected_push_pop.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef SPACE_ROS_MEMORY_ALLOCATION_DEMO__EXPECTED_PUSH_POP_HPP_ 16 | #define SPACE_ROS_MEMORY_ALLOCATION_DEMO__EXPECTED_PUSH_POP_HPP_ 17 | 18 | #include "osrf_testing_tools_cpp/memory_tools/memory_tools.hpp" 19 | 20 | struct states 21 | { 22 | bool malloc_expected; 23 | bool calloc_expected; 24 | bool realloc_expected; 25 | bool free_expected; 26 | }; 27 | 28 | inline 29 | void 30 | set_expectations_from_states(states expectations) 31 | { 32 | if (expectations.malloc_expected) { 33 | osrf_testing_tools_cpp::memory_tools::expect_no_malloc_end(); 34 | } else { 35 | osrf_testing_tools_cpp::memory_tools::expect_no_malloc_begin(); 36 | } 37 | if (expectations.calloc_expected) { 38 | osrf_testing_tools_cpp::memory_tools::expect_no_calloc_end(); 39 | } else { 40 | osrf_testing_tools_cpp::memory_tools::expect_no_calloc_begin(); 41 | } 42 | if (expectations.realloc_expected) { 43 | osrf_testing_tools_cpp::memory_tools::expect_no_realloc_end(); 44 | } else { 45 | osrf_testing_tools_cpp::memory_tools::expect_no_realloc_begin(); 46 | } 47 | if (expectations.free_expected) { 48 | osrf_testing_tools_cpp::memory_tools::expect_no_free_end(); 49 | } else { 50 | osrf_testing_tools_cpp::memory_tools::expect_no_free_begin(); 51 | } 52 | } 53 | 54 | inline 55 | states 56 | push_expectations(states new_states) 57 | { 58 | states s; 59 | s.malloc_expected = osrf_testing_tools_cpp::memory_tools::malloc_expected(); 60 | s.calloc_expected = osrf_testing_tools_cpp::memory_tools::calloc_expected(); 61 | s.realloc_expected = osrf_testing_tools_cpp::memory_tools::realloc_expected(); 62 | s.free_expected = osrf_testing_tools_cpp::memory_tools::free_expected(); 63 | set_expectations_from_states(new_states); 64 | return s; 65 | } 66 | 67 | inline 68 | void 69 | pop_expectations(states previous_states) 70 | { 71 | set_expectations_from_states(previous_states); 72 | } 73 | 74 | #endif // SPACE_ROS_MEMORY_ALLOCATION_DEMO__EXPECTED_PUSH_POP_HPP_ 75 | -------------------------------------------------------------------------------- /space_ros_memory_allocation_demo/src/listener.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef SPACE_ROS_MEMORY_ALLOCATION_DEMO__LISTENER_HPP_ 16 | #define SPACE_ROS_MEMORY_ALLOCATION_DEMO__LISTENER_HPP_ 17 | 18 | #include "rclcpp/rclcpp.hpp" 19 | 20 | #include "std_msgs/msg/string.hpp" 21 | 22 | #include "space_ros_memory_allocation_demo/visibility_control.h" 23 | #include "space_ros_memory_allocation_demo/memory_allocator.hpp" 24 | 25 | #include "expected_push_pop.hpp" 26 | 27 | namespace space_ros_memory_allocation_demo 28 | { 29 | 30 | // TODO(wjwwood): Cannot use this until we fix https://github.com/ros2/rosidl/issues/566 31 | // using String = std_msgs::msg::String_; 32 | using String = std_msgs::msg::String; 33 | 34 | // Create a Listener class that subclasses the generic rclcpp::Node base class. 35 | // The main function below will instantiate the class as a ROS node. 36 | class Listener : public rclcpp::Node 37 | { 38 | public: 39 | SPACE_ROS_MEMORY_ALLOCATION_DEMO_PUBLIC 40 | explicit Listener( 41 | const rclcpp::NodeOptions & options, 42 | std::pmr::memory_resource * memory_resource = std::pmr::get_default_resource()) 43 | : Node("listener", options) 44 | { 45 | RCUTILS_UNUSED(memory_resource); 46 | // Create a callback function for when messages are received. 47 | // Variations of this function also exist using, for example UniquePtr for zero-copy transport. 48 | auto callback = 49 | [this](std_msgs::msg::String::ConstSharedPtr msg) -> void 50 | { 51 | RCLCPP_INFO(this->get_logger(), "I heard: [%s]", msg->data.c_str()); 52 | this->get_node_base_interface()->get_context()->shutdown("done"); 53 | }; 54 | // Create a subscription to the topic which can be matched with one or more compatible ROS 55 | // publishers. 56 | // Note that not all publishers on the same topic with the same type will be compatible: 57 | // they must have compatible Quality of Service policies. 58 | sub_ = create_subscription("chatter", 10, callback); 59 | } 60 | 61 | private: 62 | rclcpp::Subscription::SharedPtr sub_; 63 | }; 64 | 65 | } // namespace space_ros_memory_allocation_demo 66 | 67 | #endif // SPACE_ROS_MEMORY_ALLOCATION_DEMO__LISTENER_HPP_ 68 | -------------------------------------------------------------------------------- /space_ros_memory_allocation_demo/src/listener_library.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "rclcpp/rclcpp.hpp" 16 | #include "rclcpp_components/register_node_macro.hpp" 17 | 18 | #include "listener.hpp" 19 | 20 | RCLCPP_COMPONENTS_REGISTER_NODE(space_ros_memory_allocation_demo::Listener) 21 | -------------------------------------------------------------------------------- /space_ros_memory_allocation_demo/src/listener_main.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | #include "osrf_testing_tools_cpp/memory_tools/memory_tools.hpp" 18 | #include "rclcpp/rclcpp.hpp" 19 | 20 | #include "common_main.hpp" 21 | #include "expected_push_pop.hpp" 22 | #include "listener.hpp" 23 | 24 | int 25 | actual_main(int argc, char const * argv[], space_ros_memory_allocation_demo::MemoryAllocator & ma) 26 | { 27 | using std::pmr::polymorphic_allocator; 28 | // Create a C-style allocator that wraps the pmr C++ allocator. 29 | polymorphic_allocator pa(&ma); 30 | rcl_allocator_t rcl_allocator = 31 | rclcpp::allocator::get_rcl_allocator>(pa); 32 | 33 | using rclcpp::Context; 34 | rclcpp::InitOptions io(rcl_allocator); 35 | auto context = std::allocate_shared>(&ma); 36 | context->init(argc, argv, io); 37 | 38 | rclcpp::NodeOptions no(rcl_allocator); 39 | no.context(context); 40 | using space_ros_memory_allocation_demo::Listener; 41 | auto listener = 42 | std::allocate_shared>(&ma, no, &ma); 43 | 44 | // TODO(wjwwood): use ma in executor memory_strategy 45 | rclcpp::ExecutorOptions eo; 46 | eo.context = context; 47 | rclcpp::executors::SingleThreadedExecutor executor(eo); 48 | 49 | executor.add_node(listener); 50 | EXPECT_NO_MEMORY_OPERATIONS( 51 | { 52 | executor.spin(); 53 | }); 54 | 55 | return 0; 56 | } 57 | 58 | int 59 | main(int argc, char const * argv[]) 60 | { 61 | return common_main(argc, argv, actual_main); 62 | } 63 | -------------------------------------------------------------------------------- /space_ros_memory_allocation_demo/src/talker.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef SPACE_ROS_MEMORY_ALLOCATION_DEMO__TALKER_HPP_ 16 | #define SPACE_ROS_MEMORY_ALLOCATION_DEMO__TALKER_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include "rclcpp/rclcpp.hpp" 24 | 25 | #include "std_msgs/msg/string.hpp" 26 | 27 | #include "space_ros_memory_allocation_demo/visibility_control.h" 28 | #include "space_ros_memory_allocation_demo/memory_allocator.hpp" 29 | 30 | #include "expected_push_pop.hpp" 31 | 32 | using namespace std::chrono_literals; 33 | 34 | namespace space_ros_memory_allocation_demo 35 | { 36 | 37 | // TODO(wjwwood): Cannot use this until we fix https://github.com/ros2/rosidl/issues/566 38 | // using String = std_msgs::msg::String_; 39 | using String = std_msgs::msg::String; 40 | 41 | // Create a Talker class that subclasses the generic rclcpp::Node base class. 42 | // The main function below will instantiate the class as a ROS node. 43 | class Talker : public rclcpp::Node 44 | { 45 | public: 46 | SPACE_ROS_MEMORY_ALLOCATION_DEMO_PUBLIC 47 | explicit Talker( 48 | const rclcpp::NodeOptions & options, 49 | std::pmr::memory_resource * memory_resource = std::pmr::get_default_resource()) 50 | : Node("talker", options) 51 | { 52 | RCUTILS_UNUSED(memory_resource); 53 | // Create a function for when messages are to be sent. 54 | auto publish_message = 55 | [this]() -> void 56 | { 57 | msg_ = std::make_unique(); 58 | msg_->data = "Hello World: " + std::to_string(count_++); 59 | RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", msg_->data.c_str()); 60 | // Put the message into a queue to be processed by the middleware. 61 | // This call is non-blocking. 62 | auto states = push_expectations({false, false, false, false}); 63 | osrf_testing_tools_cpp::memory_tools::guaranteed_malloc("it's working"); 64 | pub_->publish(std::move(msg_)); 65 | pop_expectations(states); 66 | this->get_node_base_interface()->get_context()->shutdown("done"); 67 | }; 68 | // Create a publisher with a custom Quality of Service profile. 69 | // Uniform initialization is suggested so it can be trivially changed to 70 | // rclcpp::KeepAll{} if the user wishes. 71 | // (rclcpp::KeepLast(7) -> rclcpp::KeepAll() fails to compile) 72 | rclcpp::QoS qos(rclcpp::KeepLast{7}); 73 | pub_ = this->create_publisher("chatter", qos); 74 | 75 | // Use a timer to schedule periodic message publishing. 76 | timer_ = this->create_wall_timer(1s, publish_message); 77 | } 78 | 79 | private: 80 | size_t count_ = 1; 81 | std::unique_ptr msg_; 82 | rclcpp::Publisher::SharedPtr pub_; 83 | rclcpp::TimerBase::SharedPtr timer_; 84 | }; 85 | 86 | } // namespace demo_nodes_cpp 87 | 88 | #endif // SPACE_ROS_MEMORY_ALLOCATION_DEMO__TALKER_HPP_ 89 | -------------------------------------------------------------------------------- /space_ros_memory_allocation_demo/src/talker_library.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "rclcpp/rclcpp.hpp" 16 | #include "rclcpp_components/register_node_macro.hpp" 17 | 18 | #include "talker.hpp" 19 | 20 | RCLCPP_COMPONENTS_REGISTER_NODE(space_ros_memory_allocation_demo::Talker) 21 | -------------------------------------------------------------------------------- /space_ros_memory_allocation_demo/src/talker_main.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Open Source Robotics Foundation, Inc. 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | 17 | #include "rclcpp/rclcpp.hpp" 18 | #include "osrf_testing_tools_cpp/memory_tools/memory_tools.hpp" 19 | 20 | #include "talker.hpp" 21 | #include "common_main.hpp" 22 | 23 | int 24 | actual_main(int argc, char const * argv[], space_ros_memory_allocation_demo::MemoryAllocator & ma) 25 | { 26 | using std::pmr::polymorphic_allocator; 27 | // Create a C-style allocator that wraps the pmr C++ allocator. 28 | polymorphic_allocator pa(&ma); 29 | rcl_allocator_t rcl_allocator = 30 | rclcpp::allocator::get_rcl_allocator>(pa); 31 | 32 | using rclcpp::Context; 33 | rclcpp::InitOptions io(rcl_allocator); 34 | auto context = std::allocate_shared>(&ma); 35 | context->init(argc, argv, io); 36 | 37 | rclcpp::NodeOptions no(rcl_allocator); 38 | no.context(context); 39 | using space_ros_memory_allocation_demo::Talker; 40 | auto talker = 41 | std::allocate_shared>(&ma, no, &ma); 42 | 43 | // TODO(wjwwood): use ma in executor memory_strategy 44 | rclcpp::ExecutorOptions eo; 45 | eo.context = context; 46 | rclcpp::executors::SingleThreadedExecutor executor(eo); 47 | 48 | executor.add_node(talker); 49 | executor.spin(); 50 | 51 | return 0; 52 | } 53 | 54 | int 55 | main(int argc, char const * argv[]) 56 | { 57 | return common_main(argc, argv, actual_main); 58 | } 59 | --------------------------------------------------------------------------------