├── .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 |
--------------------------------------------------------------------------------