diff --git a/racs2_demos_on_spaceros/CMakeLists.txt b/racs2_demos_on_spaceros/CMakeLists.txt new file mode 100644 index 0000000..9f550fe --- /dev/null +++ b/racs2_demos_on_spaceros/CMakeLists.txt @@ -0,0 +1,53 @@ +cmake_minimum_required(VERSION 3.8) +project(mars_rover) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) +find_package(control_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclpy REQUIRED) +find_package(simulation REQUIRED) +find_package(std_msgs REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +install(DIRECTORY + config + launch + worlds + DESTINATION share/${PROJECT_NAME} +) + +ament_python_install_package(${PROJECT_NAME}) + +install(PROGRAMS + nodes/move_arm + nodes/move_mast + nodes/move_wheel + nodes/run_demo + nodes/odom_tf_publisher + nodes/RACS2Bridge_geometry_msgs_pb2.py + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() diff --git a/racs2_demos_on_spaceros/Dockerfile b/racs2_demos_on_spaceros/Dockerfile new file mode 100644 index 0000000..eb79042 --- /dev/null +++ b/racs2_demos_on_spaceros/Dockerfile @@ -0,0 +1,142 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# A Docker configuration script to build the Space ROS image. +# +# The script provides the following build arguments: +# +# VCS_REF - The git revision of the Space ROS source code (no default value). +# VERSION - The version of Space ROS (default: "preview") + +FROM openrobotics/space_robots_demo:latest + +# Define arguments used in the metadata definition +ARG VCS_REF +ARG VERSION="preview" + +# Specify the docker image metadata +LABEL org.label-schema.schema-version="1.0" +LABEL org.label-schema.name="RACS2 demo on Curiosity Rover" +LABEL org.label-schema.description="RACS2 demo on the Space ROS platform" +LABEL org.label-schema.vendor="JAXA" +LABEL org.label-schema.version=${VERSION} +LABEL org.label-schema.url="https://github.com/space-ros" +LABEL org.label-schema.vcs-url="https://github.com/space-ros/docker" +LABEL org.label-schema.vcs-ref=${VCS_REF} + +# Define a few key variables +ENV RACS2_DEMO_DIR=${HOME_DIR}/racs2_ws + +# Disable prompting during package installation +ARG DEBIAN_FRONTEND=noninteractive + +# Get rosinstall_generator +# Using Docker BuildKit cache mounts for /var/cache/apt and /var/lib/apt ensures that +# the cache won't make it into the built image but will be maintained between steps. +RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ + --mount=type=cache,target=/var/lib/apt,sharing=locked \ + sudo apt-get update -y && sudo apt-get install -y python3-rosinstall-generator + +RUN mkdir -p ${RACS2_DEMO_DIR} +WORKDIR ${RACS2_DEMO_DIR} + +# Install dependencies +RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ + --mount=type=cache,target=/var/lib/apt,sharing=locked \ + sudo apt-get install libwebsockets-dev libwebsockets-dev protobuf-c-compiler libprotobuf-c-dev python3-pip tmux gdb -y +RUN python3 -m pip install protobuf==3.20.0 websockets==12.0 + +# Get the cFS source code +RUN git clone --recursive -b v6.7.0a https://github.com/nasa/cFS/ cfs +WORKDIR ${RACS2_DEMO_DIR}/cfs +RUN git submodule init +RUN git submodule update + +# Get the RACS2 source code +WORKDIR ${RACS2_DEMO_DIR} +RUN git clone https://github.com/jaxa/racs2_bridge -b v1.1 + +# Get the demo source code +# rename old demo directory and exclude from build +WORKDIR ${DEMO_DIR}/src +# RUN mv demos demos_old +RUN cp -r demos demos_old +RUN touch demos_old/COLCON_IGNORE +# git clone new demo +# RUN git clone https://github.com/tt-saito/demos.git -b racs2_demo2 +# RUN git clone https://github.com/space-ros/demos + +# Customize cFS to run the bridge +WORKDIR ${RACS2_DEMO_DIR}/cfs +RUN cp cfe/cmake/Makefile.sample Makefile +RUN cp -r cfe/cmake/sample_defs sample_defs +RUN cp -pr ${RACS2_DEMO_DIR}/racs2_bridge/cFS/Bridge/Client_C/apps/racs2_bridge_client apps/ + +# Deploy the run_app application and adjust the startup scripts +# RUN cp -pr ${DEMO_DIR}/src/demos/racs2_demos_on_spaceros/cFS/sample_defs/* ${RACS2_DEMO_DIR}/cfs/sample_defs/ +# RUN cp -pr ${DEMO_DIR}/src/demos/racs2_demos_on_spaceros/cFS/apps/run_app ${RACS2_DEMO_DIR}/cfs/apps/ +COPY cFS/sample_defs/cpu1_cfe_es_startup.scr ${RACS2_DEMO_DIR}/cfs/sample_defs/cpu1_cfe_es_startup.scr +COPY cFS/sample_defs/racs2_bridge_config.txt ${RACS2_DEMO_DIR}/cfs/sample_defs/racs2_bridge_config.txt +COPY cFS/sample_defs/targets.cmake ${RACS2_DEMO_DIR}/cfs/sample_defs/targets.cmake +COPY cFS/apps/run_app ${RACS2_DEMO_DIR}/cfs/apps/run_app + +# This is necessary to run cFS inside docker +RUN sed -i -e 's/^#undef OSAL_DEBUG_PERMISSIVE_MODE/#define OSAL_DEBUG_PERMISSIVE_MODE 1/g' sample_defs/default_osconfig.h +RUN sed -i -e 's/^#undef OSAL_DEBUG_DISABLE_TASK_PRIORITIES/#define OSAL_DEBUG_DISABLE_TASK_PRIORITIES 1/g' sample_defs/default_osconfig.h + +# This is only needed because docker by default starts in IPv4. This setting +# is specific to the JAXA bridge. +RUN sed -i -e 's/^wss_uri=.*/wss_uri=127.0.0.1/g' sample_defs/racs2_bridge_config.txt + +# Compile cFS +RUN make SIMULATION=native prep +RUN make +RUN make install + +# Prepare ROS packages +WORKDIR ${DEMO_DIR} +# Copy bridge & racs2_msg node +RUN cp -pr ${RACS2_DEMO_DIR}/racs2_bridge/ROS2/Bridge/Server_Python/bridge_py_s src/ +RUN cp -pr ${RACS2_DEMO_DIR}/racs2_bridge/Example/Case.1/ROS2/racs2_msg src/ + + +COPY CMakeLists.txt ${DEMO_DIR}/src/demos/mars_rover/CMakeLists.txt +COPY package.xml ${DEMO_DIR}/src/demos/mars_rover/package.xml +# COPY racs2_demos_on_spaceros ${DEMO_DIR}/src/demos/mars_rover/racs2_demos_on_spaceros +COPY launch/mars_rover.launch.py ${DEMO_DIR}/src/demos/mars_rover/launch/mars_rover.launch.py +COPY nodes/RACS2Bridge_geometry_msgs_pb2.py ${DEMO_DIR}/src/demos/mars_rover/nodes/RACS2Bridge_geometry_msgs_pb2.py +COPY nodes/move_wheel ${DEMO_DIR}/src/demos/mars_rover/nodes/move_wheel + + + + +# Install dependencies +RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ + --mount=type=cache,target=/var/lib/apt,sharing=locked \ + sudo apt-get update -y \ +&& /bin/bash -c 'source "${SPACEROS_DIR}/install/setup.bash"' \ +&& /bin/bash -c 'source "${MOVEIT2_DIR}/install/setup.bash"' \ +&& rosdep install --from-paths src --ignore-src -r -y --rosdistro ${ROS_DISTRO} + +# Build the demo +RUN /bin/bash -c 'source ${SPACEROS_DIR}/install/setup.bash && source ${MOVEIT2_DIR}/install/setup.bash \ + && colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release' + +# Add the user to the render group so that the user can access /dev/dri/renderD128 +RUN sudo usermod -aG render $USERNAME + +# Setup the entrypoint +COPY ./entrypoint.sh / +ENTRYPOINT ["/entrypoint.sh"] +CMD ["bash"] diff --git a/racs2_demos_on_spaceros/README.md b/racs2_demos_on_spaceros/README.md new file mode 100644 index 0000000..3d08cc3 --- /dev/null +++ b/racs2_demos_on_spaceros/README.md @@ -0,0 +1,93 @@ +# RACS2 on Space ROS and Space Robots Demo Docker Image + +The RACS2 on Space ROS and Space Robots Demo docker image uses the space_robots demo docker image (*openrobotics/space_robots_demo:latest*) as its base image. +Build instructions for that image can be found in [this README](../space_robots/README.md). +The Dockerfile installs all of the prerequisite system dependencies along with the demos source code, then builds the Space ROS Space Robots and RACS2 demo. + +This is RACS2 Bridge demo for Curiosity Mars rover. + +## Building the Demo Docker + +The demo image builds on top of the `spaceros`, `moveit2`, `space_robots` images. +To build the docker image, first ensure the `spaceros` base image is available either by [building it locally](https://github.com/space-ros/space-ros) or pulling it. + +Then build the `moveit2`, `space_robots` and `racs2_demos_on_spaceros` demo images: + +```bash +git clone https://github.com/space-ros/docker.git +cd docker/moveit2 +./build.sh +cd ../space_robots +./build.sh +cd ../../ +./build.sh +``` + +## Running the Demo Docker + +(at /path/to/demos/racs2_demos_on_spaceros/docker/racs2_demos_on_spaceros) +run the following to allow GUI passthrough: +```bash +xhost +local:docker +``` + +Then run: +```bash +./run.sh +``` + +Depending on the host computer, you might need to remove the ```--gpus all``` flag in ```run.sh```, which uses your GPUs. + +## Running the Demos + +### Curiosity Mars rover demo +Launch the rover demo (calling Terminal 1): +```bash +source install/setup.bash +ros2 launch mars_rover mars_rover.launch.py +``` + +#### RACS2 Bridge demo + +##### Running racs2 bridge node +Open a new terminal (calling Terminal 2) and attach to the currently running container: + +```bash +docker exec -it bash +source install/setup.bash +ros2 run bridge_py_s bridge_py_s_node --ros-args --params-file ./src/bridge_py_s/config/params.yaml +``` + +##### Running cFS bridge app & run_app app +Open a new terminal (calling Terminal 3) and attach to the currently running container: + +```bash +docker exec -it bash +cd ~/racs2_ws +cd cfs/build/exe/cpu1/ +./core-cpu1 +``` + +**Executing commands to the rover must be done with this terminal active.** + + +##### Available Commands + +Drive commands to the rover are input via keyboard in Terimnal 3. The keymap is as follows. + +* "w": Drive the rover forward +* "s": Drive the rover backward +* "a": Turn left +* "d": Turn right +* "x": Stop the rover + +##### Nodes +![RACS2 demo on Space ROS Mars rover demo](racs2_demo_on_spaceros_nodes.png) + +## Reference + +* [RACS2 bridge project by Japan Aerospace Exploration Agency (JAXA)](https://github.com/jaxa/racs2_bridge) + +* [Hiroki Kato and Tatsuhiko Saito, "RACS2: the ROS2 and cFS System - launched" Flight Software Workshop 2023.](https://drive.google.com/drive/folders/1C9fokWGDl2e4NfgX_ZU3f98FfPe9udwQ) + +* [Hiroki Kato and Tatsuhiko Saito, "ROS and cFS System (RACS): Easing Space Robotic Development post-opensource activities and ROS2 integration" Flight Software Workshop 2021.](https://drive.google.com/file/d/11L48doT_pRNs7R0hdChPALqJO849TvV2/view?usp=drive_web) diff --git a/racs2_demos_on_spaceros/build.sh b/racs2_demos_on_spaceros/build.sh new file mode 100755 index 0000000..ded50cd --- /dev/null +++ b/racs2_demos_on_spaceros/build.sh @@ -0,0 +1,23 @@ +#!/usr/bin/env bash + +ORG=jaxa +IMAGE=racs2_demos_on_spaceros +TAG=latest + +VCS_REF="" +VERSION=preview + +# Exit script with failure if build fails +set -eo pipefail + +echo "" +echo "##### Building RACS2 Demo on Space ROS Docker Image #####" +echo "" + +docker build -t $ORG/$IMAGE:$TAG \ + --build-arg VCS_REF="$VCS_REF" \ + --build-arg VERSION="$VERSION" . + +echo "" +echo "##### Done! #####" + diff --git a/racs2_demos_on_spaceros/cFS/apps/run_app/CMakeLists.txt b/racs2_demos_on_spaceros/cFS/apps/run_app/CMakeLists.txt new file mode 100644 index 0000000..fe32de4 --- /dev/null +++ b/racs2_demos_on_spaceros/cFS/apps/run_app/CMakeLists.txt @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 2.6.4) +project(CFE_RUN_APP C) + +include_directories(fsw/mission_inc) +include_directories(fsw/platform_inc) + +aux_source_directory(fsw/src APP_SRC_FILES) + +# Create the app module +add_cfe_app(run_app ${APP_SRC_FILES}) + +target_link_libraries(run_app + ${PROTOBUF_LIBRARY} + protobuf-c +) \ No newline at end of file diff --git a/racs2_demos_on_spaceros/cFS/apps/run_app/fsw/mission_inc/run_app_perfids.h b/racs2_demos_on_spaceros/cFS/apps/run_app/fsw/mission_inc/run_app_perfids.h new file mode 100644 index 0000000..643eeec --- /dev/null +++ b/racs2_demos_on_spaceros/cFS/apps/run_app/fsw/mission_inc/run_app_perfids.h @@ -0,0 +1,7 @@ +#ifndef _run_app_perfids_h_ +#define _run_app_perfids_h_ + + +#define SAMPLE_APP_PERF_ID 91 + +#endif /* _run_app_perfids_h_ */ diff --git a/racs2_demos_on_spaceros/cFS/apps/run_app/fsw/platform_inc/run_app_msgids.h b/racs2_demos_on_spaceros/cFS/apps/run_app/fsw/platform_inc/run_app_msgids.h new file mode 100644 index 0000000..0a44407 --- /dev/null +++ b/racs2_demos_on_spaceros/cFS/apps/run_app/fsw/platform_inc/run_app_msgids.h @@ -0,0 +1,9 @@ +#ifndef _run_app_msgids_h_ +#define _run_app_msgids_h_ + +#define RUN_APP_CMD_MID 0x1892 +#define RUN_APP_SEND_HK_MID 0x1893 +#define RUN_APP_HK_TLM_MID 0x0893 +#define RACS2_BRIDGE_MID 0x1EFE + +#endif /* _run_app_msgids_h_ */ diff --git a/racs2_demos_on_spaceros/cFS/apps/run_app/fsw/src/RACS2Brdige_std_msgs.pb-c.c b/racs2_demos_on_spaceros/cFS/apps/run_app/fsw/src/RACS2Brdige_std_msgs.pb-c.c new file mode 100644 index 0000000..0512d65 --- /dev/null +++ b/racs2_demos_on_spaceros/cFS/apps/run_app/fsw/src/RACS2Brdige_std_msgs.pb-c.c @@ -0,0 +1,261 @@ +/* Generated by the protocol buffer compiler. DO NOT EDIT! */ +/* Generated from: RACS2Brdige_std_msgs.proto */ + +/* Do not generate deprecated warnings for self */ +#ifndef PROTOBUF_C__NO_DEPRECATED +#define PROTOBUF_C__NO_DEPRECATED +#endif + +#include "RACS2Brdige_std_msgs.pb-c.h" +void racs2_bridge_std_msgs__init + (RACS2BridgeStdMsgs *message) +{ + static const RACS2BridgeStdMsgs init_value = RACS2_BRIDGE_STD_MSGS__INIT; + *message = init_value; +} +size_t racs2_bridge_std_msgs__get_packed_size + (const RACS2BridgeStdMsgs *message) +{ + assert(message->base.descriptor == &racs2_bridge_std_msgs__descriptor); + return protobuf_c_message_get_packed_size ((const ProtobufCMessage*)(message)); +} +size_t racs2_bridge_std_msgs__pack + (const RACS2BridgeStdMsgs *message, + uint8_t *out) +{ + assert(message->base.descriptor == &racs2_bridge_std_msgs__descriptor); + return protobuf_c_message_pack ((const ProtobufCMessage*)message, out); +} +size_t racs2_bridge_std_msgs__pack_to_buffer + (const RACS2BridgeStdMsgs *message, + ProtobufCBuffer *buffer) +{ + assert(message->base.descriptor == &racs2_bridge_std_msgs__descriptor); + return protobuf_c_message_pack_to_buffer ((const ProtobufCMessage*)message, buffer); +} +RACS2BridgeStdMsgs * + racs2_bridge_std_msgs__unpack + (ProtobufCAllocator *allocator, + size_t len, + const uint8_t *data) +{ + return (RACS2BridgeStdMsgs *) + protobuf_c_message_unpack (&racs2_bridge_std_msgs__descriptor, + allocator, len, data); +} +void racs2_bridge_std_msgs__free_unpacked + (RACS2BridgeStdMsgs *message, + ProtobufCAllocator *allocator) +{ + if(!message) + return; + assert(message->base.descriptor == &racs2_bridge_std_msgs__descriptor); + protobuf_c_message_free_unpacked ((ProtobufCMessage*)message, allocator); +} +static const ProtobufCFieldDescriptor racs2_bridge_std_msgs__field_descriptors[14] = +{ + { + "bool_data", + 1, + PROTOBUF_C_LABEL_OPTIONAL, + PROTOBUF_C_TYPE_BOOL, + offsetof(RACS2BridgeStdMsgs, has_bool_data), + offsetof(RACS2BridgeStdMsgs, bool_data), + NULL, + NULL, + 0, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, + { + "float_data", + 2, + PROTOBUF_C_LABEL_OPTIONAL, + PROTOBUF_C_TYPE_FLOAT, + offsetof(RACS2BridgeStdMsgs, has_float_data), + offsetof(RACS2BridgeStdMsgs, float_data), + NULL, + NULL, + 0, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, + { + "double_data", + 3, + PROTOBUF_C_LABEL_OPTIONAL, + PROTOBUF_C_TYPE_DOUBLE, + offsetof(RACS2BridgeStdMsgs, has_double_data), + offsetof(RACS2BridgeStdMsgs, double_data), + NULL, + NULL, + 0, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, + { + "int32_data", + 4, + PROTOBUF_C_LABEL_OPTIONAL, + PROTOBUF_C_TYPE_INT32, + offsetof(RACS2BridgeStdMsgs, has_int32_data), + offsetof(RACS2BridgeStdMsgs, int32_data), + NULL, + NULL, + 0, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, + { + "int64_data", + 5, + PROTOBUF_C_LABEL_OPTIONAL, + PROTOBUF_C_TYPE_INT64, + offsetof(RACS2BridgeStdMsgs, has_int64_data), + offsetof(RACS2BridgeStdMsgs, int64_data), + NULL, + NULL, + 0, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, + { + "string_data", + 6, + PROTOBUF_C_LABEL_OPTIONAL, + PROTOBUF_C_TYPE_STRING, + 0, /* quantifier_offset */ + offsetof(RACS2BridgeStdMsgs, string_data), + NULL, + NULL, + 0, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, + { + "uint32_data", + 7, + PROTOBUF_C_LABEL_OPTIONAL, + PROTOBUF_C_TYPE_UINT32, + offsetof(RACS2BridgeStdMsgs, has_uint32_data), + offsetof(RACS2BridgeStdMsgs, uint32_data), + NULL, + NULL, + 0, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, + { + "uint64_data", + 8, + PROTOBUF_C_LABEL_OPTIONAL, + PROTOBUF_C_TYPE_UINT64, + offsetof(RACS2BridgeStdMsgs, has_uint64_data), + offsetof(RACS2BridgeStdMsgs, uint64_data), + NULL, + NULL, + 0, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, + { + "float32_array_data", + 9, + PROTOBUF_C_LABEL_REPEATED, + PROTOBUF_C_TYPE_FLOAT, + offsetof(RACS2BridgeStdMsgs, n_float32_array_data), + offsetof(RACS2BridgeStdMsgs, float32_array_data), + NULL, + NULL, + 0 | PROTOBUF_C_FIELD_FLAG_PACKED, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, + { + "float64_array_data", + 10, + PROTOBUF_C_LABEL_REPEATED, + PROTOBUF_C_TYPE_DOUBLE, + offsetof(RACS2BridgeStdMsgs, n_float64_array_data), + offsetof(RACS2BridgeStdMsgs, float64_array_data), + NULL, + NULL, + 0 | PROTOBUF_C_FIELD_FLAG_PACKED, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, + { + "int32_array_data", + 11, + PROTOBUF_C_LABEL_REPEATED, + PROTOBUF_C_TYPE_INT32, + offsetof(RACS2BridgeStdMsgs, n_int32_array_data), + offsetof(RACS2BridgeStdMsgs, int32_array_data), + NULL, + NULL, + 0 | PROTOBUF_C_FIELD_FLAG_PACKED, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, + { + "int64_array_data", + 12, + PROTOBUF_C_LABEL_REPEATED, + PROTOBUF_C_TYPE_INT64, + offsetof(RACS2BridgeStdMsgs, n_int64_array_data), + offsetof(RACS2BridgeStdMsgs, int64_array_data), + NULL, + NULL, + 0 | PROTOBUF_C_FIELD_FLAG_PACKED, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, + { + "uint32_array_data", + 13, + PROTOBUF_C_LABEL_REPEATED, + PROTOBUF_C_TYPE_UINT32, + offsetof(RACS2BridgeStdMsgs, n_uint32_array_data), + offsetof(RACS2BridgeStdMsgs, uint32_array_data), + NULL, + NULL, + 0 | PROTOBUF_C_FIELD_FLAG_PACKED, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, + { + "uint64_array_data", + 14, + PROTOBUF_C_LABEL_REPEATED, + PROTOBUF_C_TYPE_UINT64, + offsetof(RACS2BridgeStdMsgs, n_uint64_array_data), + offsetof(RACS2BridgeStdMsgs, uint64_array_data), + NULL, + NULL, + 0 | PROTOBUF_C_FIELD_FLAG_PACKED, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, +}; +static const unsigned racs2_bridge_std_msgs__field_indices_by_name[] = { + 0, /* field[0] = bool_data */ + 2, /* field[2] = double_data */ + 8, /* field[8] = float32_array_data */ + 9, /* field[9] = float64_array_data */ + 1, /* field[1] = float_data */ + 10, /* field[10] = int32_array_data */ + 3, /* field[3] = int32_data */ + 11, /* field[11] = int64_array_data */ + 4, /* field[4] = int64_data */ + 5, /* field[5] = string_data */ + 12, /* field[12] = uint32_array_data */ + 6, /* field[6] = uint32_data */ + 13, /* field[13] = uint64_array_data */ + 7, /* field[7] = uint64_data */ +}; +static const ProtobufCIntRange racs2_bridge_std_msgs__number_ranges[1 + 1] = +{ + { 1, 0 }, + { 0, 14 } +}; +const ProtobufCMessageDescriptor racs2_bridge_std_msgs__descriptor = +{ + PROTOBUF_C__MESSAGE_DESCRIPTOR_MAGIC, + "RACS2Bridge_std_msgs", + "RACS2BridgeStdMsgs", + "RACS2BridgeStdMsgs", + "", + sizeof(RACS2BridgeStdMsgs), + 14, + racs2_bridge_std_msgs__field_descriptors, + racs2_bridge_std_msgs__field_indices_by_name, + 1, racs2_bridge_std_msgs__number_ranges, + (ProtobufCMessageInit) racs2_bridge_std_msgs__init, + NULL,NULL,NULL /* reserved[123] */ +}; diff --git a/racs2_demos_on_spaceros/cFS/apps/run_app/fsw/src/RACS2Brdige_std_msgs.pb-c.h b/racs2_demos_on_spaceros/cFS/apps/run_app/fsw/src/RACS2Brdige_std_msgs.pb-c.h new file mode 100644 index 0000000..54a70aa --- /dev/null +++ b/racs2_demos_on_spaceros/cFS/apps/run_app/fsw/src/RACS2Brdige_std_msgs.pb-c.h @@ -0,0 +1,139 @@ +/* Generated by the protocol buffer compiler. DO NOT EDIT! */ +/* Generated from: RACS2Brdige_std_msgs.proto */ + +#ifndef PROTOBUF_C_RACS2Brdige_5fstd_5fmsgs_2eproto__INCLUDED +#define PROTOBUF_C_RACS2Brdige_5fstd_5fmsgs_2eproto__INCLUDED + +#include + +PROTOBUF_C__BEGIN_DECLS + +#if PROTOBUF_C_VERSION_NUMBER < 1000000 +# error This file was generated by a newer version of protoc-c which is incompatible with your libprotobuf-c headers. Please update your headers. +#elif 1003003 < PROTOBUF_C_MIN_COMPILER_VERSION +# error This file was generated by an older version of protoc-c which is incompatible with your libprotobuf-c headers. Please regenerate this file with a newer version of protoc-c. +#endif + + +typedef struct _RACS2BridgeStdMsgs RACS2BridgeStdMsgs; + + +/* --- enums --- */ + + +/* --- messages --- */ + +struct _RACS2BridgeStdMsgs +{ + ProtobufCMessage base; + /* + * -- std_msgs/Bool --------------------------- + */ + protobuf_c_boolean has_bool_data; + protobuf_c_boolean bool_data; + /* + * -- std_msgs/Float32 --------------------------- + */ + protobuf_c_boolean has_float_data; + float float_data; + /* + * -- std_msgs/Float64 --------------------------- + */ + protobuf_c_boolean has_double_data; + double double_data; + /* + * -- std_msgs/Int32 --------------------------- + */ + protobuf_c_boolean has_int32_data; + int32_t int32_data; + /* + * -- std_msgs/Int64 --------------------------- + */ + protobuf_c_boolean has_int64_data; + int64_t int64_data; + /* + * -- std_msgs/String --------------------------- + */ + char *string_data; + /* + * -- std_msgs/Uint32 --------------------------- + */ + protobuf_c_boolean has_uint32_data; + uint32_t uint32_data; + /* + * -- std_msgs/Uint64 --------------------------- + */ + protobuf_c_boolean has_uint64_data; + uint64_t uint64_data; + /* + * -- std_msgs/Float32MultiArray --------------------------- + */ + size_t n_float32_array_data; + float *float32_array_data; + /* + * -- std_msgs/Float64MultiArray --------------------------- + */ + size_t n_float64_array_data; + double *float64_array_data; + /* + * -- std_msgs/Int32MultiArray --------------------------- + */ + size_t n_int32_array_data; + int32_t *int32_array_data; + /* + * -- std_msgs/Int64MultiArray --------------------------- + */ + size_t n_int64_array_data; + int64_t *int64_array_data; + /* + * -- std_msgs/Uint32MultiArray --------------------------- + */ + size_t n_uint32_array_data; + uint32_t *uint32_array_data; + /* + * -- std_msgs/Uint64MultiArray --------------------------- + */ + size_t n_uint64_array_data; + uint64_t *uint64_array_data; +}; +#define RACS2_BRIDGE_STD_MSGS__INIT \ + { PROTOBUF_C_MESSAGE_INIT (&racs2_bridge_std_msgs__descriptor) \ + , 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, NULL, 0, 0, 0, 0, 0,NULL, 0,NULL, 0,NULL, 0,NULL, 0,NULL, 0,NULL } + + +/* RACS2BridgeStdMsgs methods */ +void racs2_bridge_std_msgs__init + (RACS2BridgeStdMsgs *message); +size_t racs2_bridge_std_msgs__get_packed_size + (const RACS2BridgeStdMsgs *message); +size_t racs2_bridge_std_msgs__pack + (const RACS2BridgeStdMsgs *message, + uint8_t *out); +size_t racs2_bridge_std_msgs__pack_to_buffer + (const RACS2BridgeStdMsgs *message, + ProtobufCBuffer *buffer); +RACS2BridgeStdMsgs * + racs2_bridge_std_msgs__unpack + (ProtobufCAllocator *allocator, + size_t len, + const uint8_t *data); +void racs2_bridge_std_msgs__free_unpacked + (RACS2BridgeStdMsgs *message, + ProtobufCAllocator *allocator); +/* --- per-message closures --- */ + +typedef void (*RACS2BridgeStdMsgs_Closure) + (const RACS2BridgeStdMsgs *message, + void *closure_data); + +/* --- services --- */ + + +/* --- descriptors --- */ + +extern const ProtobufCMessageDescriptor racs2_bridge_std_msgs__descriptor; + +PROTOBUF_C__END_DECLS + + +#endif /* PROTOBUF_C_RACS2Brdige_5fstd_5fmsgs_2eproto__INCLUDED */ diff --git a/racs2_demos_on_spaceros/cFS/apps/run_app/fsw/src/RACS2Bridge_geometry_msgs.pb-c.c b/racs2_demos_on_spaceros/cFS/apps/run_app/fsw/src/RACS2Bridge_geometry_msgs.pb-c.c new file mode 100644 index 0000000..d26ed5e --- /dev/null +++ b/racs2_demos_on_spaceros/cFS/apps/run_app/fsw/src/RACS2Bridge_geometry_msgs.pb-c.c @@ -0,0 +1,622 @@ +/* Generated by the protocol buffer compiler. DO NOT EDIT! */ +/* Generated from: RACS2Bridge_geometry_msgs.proto */ + +/* Do not generate deprecated warnings for self */ +#ifndef PROTOBUF_C__NO_DEPRECATED +#define PROTOBUF_C__NO_DEPRECATED +#endif + +#include "RACS2Bridge_geometry_msgs.pb-c.h" +void racs2_bridge_geometry_msgs__vector3__init + (RACS2BridgeGeometryMsgsVector3 *message) +{ + static const RACS2BridgeGeometryMsgsVector3 init_value = RACS2_BRIDGE_GEOMETRY_MSGS__VECTOR3__INIT; + *message = init_value; +} +size_t racs2_bridge_geometry_msgs__vector3__get_packed_size + (const RACS2BridgeGeometryMsgsVector3 *message) +{ + assert(message->base.descriptor == &racs2_bridge_geometry_msgs__vector3__descriptor); + return protobuf_c_message_get_packed_size ((const ProtobufCMessage*)(message)); +} +size_t racs2_bridge_geometry_msgs__vector3__pack + (const RACS2BridgeGeometryMsgsVector3 *message, + uint8_t *out) +{ + assert(message->base.descriptor == &racs2_bridge_geometry_msgs__vector3__descriptor); + return protobuf_c_message_pack ((const ProtobufCMessage*)message, out); +} +size_t racs2_bridge_geometry_msgs__vector3__pack_to_buffer + (const RACS2BridgeGeometryMsgsVector3 *message, + ProtobufCBuffer *buffer) +{ + assert(message->base.descriptor == &racs2_bridge_geometry_msgs__vector3__descriptor); + return protobuf_c_message_pack_to_buffer ((const ProtobufCMessage*)message, buffer); +} +RACS2BridgeGeometryMsgsVector3 * + racs2_bridge_geometry_msgs__vector3__unpack + (ProtobufCAllocator *allocator, + size_t len, + const uint8_t *data) +{ + return (RACS2BridgeGeometryMsgsVector3 *) + protobuf_c_message_unpack (&racs2_bridge_geometry_msgs__vector3__descriptor, + allocator, len, data); +} +void racs2_bridge_geometry_msgs__vector3__free_unpacked + (RACS2BridgeGeometryMsgsVector3 *message, + ProtobufCAllocator *allocator) +{ + if(!message) + return; + assert(message->base.descriptor == &racs2_bridge_geometry_msgs__vector3__descriptor); + protobuf_c_message_free_unpacked ((ProtobufCMessage*)message, allocator); +} +void racs2_bridge_geometry_msgs__twist__init + (RACS2BridgeGeometryMsgsTwist *message) +{ + static const RACS2BridgeGeometryMsgsTwist init_value = RACS2_BRIDGE_GEOMETRY_MSGS__TWIST__INIT; + *message = init_value; +} +size_t racs2_bridge_geometry_msgs__twist__get_packed_size + (const RACS2BridgeGeometryMsgsTwist *message) +{ + assert(message->base.descriptor == &racs2_bridge_geometry_msgs__twist__descriptor); + return protobuf_c_message_get_packed_size ((const ProtobufCMessage*)(message)); +} +size_t racs2_bridge_geometry_msgs__twist__pack + (const RACS2BridgeGeometryMsgsTwist *message, + uint8_t *out) +{ + assert(message->base.descriptor == &racs2_bridge_geometry_msgs__twist__descriptor); + return protobuf_c_message_pack ((const ProtobufCMessage*)message, out); +} +size_t racs2_bridge_geometry_msgs__twist__pack_to_buffer + (const RACS2BridgeGeometryMsgsTwist *message, + ProtobufCBuffer *buffer) +{ + assert(message->base.descriptor == &racs2_bridge_geometry_msgs__twist__descriptor); + return protobuf_c_message_pack_to_buffer ((const ProtobufCMessage*)message, buffer); +} +RACS2BridgeGeometryMsgsTwist * + racs2_bridge_geometry_msgs__twist__unpack + (ProtobufCAllocator *allocator, + size_t len, + const uint8_t *data) +{ + return (RACS2BridgeGeometryMsgsTwist *) + protobuf_c_message_unpack (&racs2_bridge_geometry_msgs__twist__descriptor, + allocator, len, data); +} +void racs2_bridge_geometry_msgs__twist__free_unpacked + (RACS2BridgeGeometryMsgsTwist *message, + ProtobufCAllocator *allocator) +{ + if(!message) + return; + assert(message->base.descriptor == &racs2_bridge_geometry_msgs__twist__descriptor); + protobuf_c_message_free_unpacked ((ProtobufCMessage*)message, allocator); +} +void racs2_bridge_geometry_msgs__init + (RACS2BridgeGeometryMsgs *message) +{ + static const RACS2BridgeGeometryMsgs init_value = RACS2_BRIDGE_GEOMETRY_MSGS__INIT; + *message = init_value; +} +size_t racs2_bridge_geometry_msgs__get_packed_size + (const RACS2BridgeGeometryMsgs *message) +{ + assert(message->base.descriptor == &racs2_bridge_geometry_msgs__descriptor); + return protobuf_c_message_get_packed_size ((const ProtobufCMessage*)(message)); +} +size_t racs2_bridge_geometry_msgs__pack + (const RACS2BridgeGeometryMsgs *message, + uint8_t *out) +{ + assert(message->base.descriptor == &racs2_bridge_geometry_msgs__descriptor); + return protobuf_c_message_pack ((const ProtobufCMessage*)message, out); +} +size_t racs2_bridge_geometry_msgs__pack_to_buffer + (const RACS2BridgeGeometryMsgs *message, + ProtobufCBuffer *buffer) +{ + assert(message->base.descriptor == &racs2_bridge_geometry_msgs__descriptor); + return protobuf_c_message_pack_to_buffer ((const ProtobufCMessage*)message, buffer); +} +RACS2BridgeGeometryMsgs * + racs2_bridge_geometry_msgs__unpack + (ProtobufCAllocator *allocator, + size_t len, + const uint8_t *data) +{ + return (RACS2BridgeGeometryMsgs *) + protobuf_c_message_unpack (&racs2_bridge_geometry_msgs__descriptor, + allocator, len, data); +} +void racs2_bridge_geometry_msgs__free_unpacked + (RACS2BridgeGeometryMsgs *message, + ProtobufCAllocator *allocator) +{ + if(!message) + return; + assert(message->base.descriptor == &racs2_bridge_geometry_msgs__descriptor); + protobuf_c_message_free_unpacked ((ProtobufCMessage*)message, allocator); +} +static const ProtobufCFieldDescriptor racs2_bridge_geometry_msgs__vector3__field_descriptors[3] = +{ + { + "x", + 1, + PROTOBUF_C_LABEL_REQUIRED, + PROTOBUF_C_TYPE_DOUBLE, + 0, /* quantifier_offset */ + offsetof(RACS2BridgeGeometryMsgsVector3, x), + NULL, + NULL, + 0, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, + { + "y", + 2, + PROTOBUF_C_LABEL_REQUIRED, + PROTOBUF_C_TYPE_DOUBLE, + 0, /* quantifier_offset */ + offsetof(RACS2BridgeGeometryMsgsVector3, y), + NULL, + NULL, + 0, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, + { + "z", + 3, + PROTOBUF_C_LABEL_REQUIRED, + PROTOBUF_C_TYPE_DOUBLE, + 0, /* quantifier_offset */ + offsetof(RACS2BridgeGeometryMsgsVector3, z), + NULL, + NULL, + 0, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, +}; +static const unsigned racs2_bridge_geometry_msgs__vector3__field_indices_by_name[] = { + 0, /* field[0] = x */ + 1, /* field[1] = y */ + 2, /* field[2] = z */ +}; +static const ProtobufCIntRange racs2_bridge_geometry_msgs__vector3__number_ranges[1 + 1] = +{ + { 1, 0 }, + { 0, 3 } +}; +const ProtobufCMessageDescriptor racs2_bridge_geometry_msgs__vector3__descriptor = +{ + PROTOBUF_C__MESSAGE_DESCRIPTOR_MAGIC, + "RACS2Bridge_geometry_msgs_Vector3", + "RACS2BridgeGeometryMsgsVector3", + "RACS2BridgeGeometryMsgsVector3", + "", + sizeof(RACS2BridgeGeometryMsgsVector3), + 3, + racs2_bridge_geometry_msgs__vector3__field_descriptors, + racs2_bridge_geometry_msgs__vector3__field_indices_by_name, + 1, racs2_bridge_geometry_msgs__vector3__number_ranges, + (ProtobufCMessageInit) racs2_bridge_geometry_msgs__vector3__init, + NULL,NULL,NULL /* reserved[123] */ +}; +static const ProtobufCFieldDescriptor racs2_bridge_geometry_msgs__twist__field_descriptors[2] = +{ + { + "linear", + 1, + PROTOBUF_C_LABEL_REQUIRED, + PROTOBUF_C_TYPE_MESSAGE, + 0, /* quantifier_offset */ + offsetof(RACS2BridgeGeometryMsgsTwist, linear), + &racs2_bridge_geometry_msgs__vector3__descriptor, + NULL, + 0, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, + { + "angular", + 2, + PROTOBUF_C_LABEL_REQUIRED, + PROTOBUF_C_TYPE_MESSAGE, + 0, /* quantifier_offset */ + offsetof(RACS2BridgeGeometryMsgsTwist, angular), + &racs2_bridge_geometry_msgs__vector3__descriptor, + NULL, + 0, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, +}; +static const unsigned racs2_bridge_geometry_msgs__twist__field_indices_by_name[] = { + 1, /* field[1] = angular */ + 0, /* field[0] = linear */ +}; +static const ProtobufCIntRange racs2_bridge_geometry_msgs__twist__number_ranges[1 + 1] = +{ + { 1, 0 }, + { 0, 2 } +}; +const ProtobufCMessageDescriptor racs2_bridge_geometry_msgs__twist__descriptor = +{ + PROTOBUF_C__MESSAGE_DESCRIPTOR_MAGIC, + "RACS2Bridge_geometry_msgs_Twist", + "RACS2BridgeGeometryMsgsTwist", + "RACS2BridgeGeometryMsgsTwist", + "", + sizeof(RACS2BridgeGeometryMsgsTwist), + 2, + racs2_bridge_geometry_msgs__twist__field_descriptors, + racs2_bridge_geometry_msgs__twist__field_indices_by_name, + 1, racs2_bridge_geometry_msgs__twist__number_ranges, + (ProtobufCMessageInit) racs2_bridge_geometry_msgs__twist__init, + NULL,NULL,NULL /* reserved[123] */ +}; +static const ProtobufCFieldDescriptor racs2_bridge_geometry_msgs__field_descriptors[26] = +{ + { + "accel_with_covariance_data", + 1, + PROTOBUF_C_LABEL_REPEATED, + PROTOBUF_C_TYPE_FLOAT, + offsetof(RACS2BridgeGeometryMsgs, n_accel_with_covariance_data), + offsetof(RACS2BridgeGeometryMsgs, accel_with_covariance_data), + NULL, + NULL, + 0 | PROTOBUF_C_FIELD_FLAG_PACKED, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, + { + "accel_with_covariance_stamped_data", + 2, + PROTOBUF_C_LABEL_REPEATED, + PROTOBUF_C_TYPE_FLOAT, + offsetof(RACS2BridgeGeometryMsgs, n_accel_with_covariance_stamped_data), + offsetof(RACS2BridgeGeometryMsgs, accel_with_covariance_stamped_data), + NULL, + NULL, + 0 | PROTOBUF_C_FIELD_FLAG_PACKED, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, + { + "point_stamped_data", + 3, + PROTOBUF_C_LABEL_REPEATED, + PROTOBUF_C_TYPE_FLOAT, + offsetof(RACS2BridgeGeometryMsgs, n_point_stamped_data), + offsetof(RACS2BridgeGeometryMsgs, point_stamped_data), + NULL, + NULL, + 0 | PROTOBUF_C_FIELD_FLAG_PACKED, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, + { + "inertia_stamped_data", + 4, + PROTOBUF_C_LABEL_REPEATED, + PROTOBUF_C_TYPE_FLOAT, + offsetof(RACS2BridgeGeometryMsgs, n_inertia_stamped_data), + offsetof(RACS2BridgeGeometryMsgs, inertia_stamped_data), + NULL, + NULL, + 0 | PROTOBUF_C_FIELD_FLAG_PACKED, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, + { + "quaternion_stamped_data", + 5, + PROTOBUF_C_LABEL_REPEATED, + PROTOBUF_C_TYPE_FLOAT, + offsetof(RACS2BridgeGeometryMsgs, n_quaternion_stamped_data), + offsetof(RACS2BridgeGeometryMsgs, quaternion_stamped_data), + NULL, + NULL, + 0 | PROTOBUF_C_FIELD_FLAG_PACKED, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, + { + "transform_stamped_data", + 6, + PROTOBUF_C_LABEL_REPEATED, + PROTOBUF_C_TYPE_FLOAT, + offsetof(RACS2BridgeGeometryMsgs, n_transform_stamped_data), + offsetof(RACS2BridgeGeometryMsgs, transform_stamped_data), + NULL, + NULL, + 0 | PROTOBUF_C_FIELD_FLAG_PACKED, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, + { + "inertia_data", + 7, + PROTOBUF_C_LABEL_REPEATED, + PROTOBUF_C_TYPE_FLOAT, + offsetof(RACS2BridgeGeometryMsgs, n_inertia_data), + offsetof(RACS2BridgeGeometryMsgs, inertia_data), + NULL, + NULL, + 0 | PROTOBUF_C_FIELD_FLAG_PACKED, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, + { + "pose_with_covariance_data", + 8, + PROTOBUF_C_LABEL_REPEATED, + PROTOBUF_C_TYPE_FLOAT, + offsetof(RACS2BridgeGeometryMsgs, n_pose_with_covariance_data), + offsetof(RACS2BridgeGeometryMsgs, pose_with_covariance_data), + NULL, + NULL, + 0 | PROTOBUF_C_FIELD_FLAG_PACKED, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, + { + "twist_covariance_stamped_data", + 9, + PROTOBUF_C_LABEL_REPEATED, + PROTOBUF_C_TYPE_FLOAT, + offsetof(RACS2BridgeGeometryMsgs, n_twist_covariance_stamped_data), + offsetof(RACS2BridgeGeometryMsgs, twist_covariance_stamped_data), + NULL, + NULL, + 0 | PROTOBUF_C_FIELD_FLAG_PACKED, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, + { + "wrench_data", + 10, + PROTOBUF_C_LABEL_REPEATED, + PROTOBUF_C_TYPE_FLOAT, + offsetof(RACS2BridgeGeometryMsgs, n_wrench_data), + offsetof(RACS2BridgeGeometryMsgs, wrench_data), + NULL, + NULL, + 0 | PROTOBUF_C_FIELD_FLAG_PACKED, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, + { + "accel_data", + 11, + PROTOBUF_C_LABEL_REPEATED, + PROTOBUF_C_TYPE_FLOAT, + offsetof(RACS2BridgeGeometryMsgs, n_accel_data), + offsetof(RACS2BridgeGeometryMsgs, accel_data), + NULL, + NULL, + 0 | PROTOBUF_C_FIELD_FLAG_PACKED, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, + { + "pose_with_covariance_stamped_data", + 12, + PROTOBUF_C_LABEL_REPEATED, + PROTOBUF_C_TYPE_FLOAT, + offsetof(RACS2BridgeGeometryMsgs, n_pose_with_covariance_stamped_data), + offsetof(RACS2BridgeGeometryMsgs, pose_with_covariance_stamped_data), + NULL, + NULL, + 0 | PROTOBUF_C_FIELD_FLAG_PACKED, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, + { + "accele_stamped_data", + 13, + PROTOBUF_C_LABEL_REPEATED, + PROTOBUF_C_TYPE_FLOAT, + offsetof(RACS2BridgeGeometryMsgs, n_accele_stamped_data), + offsetof(RACS2BridgeGeometryMsgs, accele_stamped_data), + NULL, + NULL, + 0 | PROTOBUF_C_FIELD_FLAG_PACKED, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, + { + "transform_data", + 14, + PROTOBUF_C_LABEL_REPEATED, + PROTOBUF_C_TYPE_FLOAT, + offsetof(RACS2BridgeGeometryMsgs, n_transform_data), + offsetof(RACS2BridgeGeometryMsgs, transform_data), + NULL, + NULL, + 0 | PROTOBUF_C_FIELD_FLAG_PACKED, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, + { + "point_data", + 15, + PROTOBUF_C_LABEL_REPEATED, + PROTOBUF_C_TYPE_FLOAT, + offsetof(RACS2BridgeGeometryMsgs, n_point_data), + offsetof(RACS2BridgeGeometryMsgs, point_data), + NULL, + NULL, + 0 | PROTOBUF_C_FIELD_FLAG_PACKED, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, + { + "polygon_stamped_dta", + 16, + PROTOBUF_C_LABEL_REPEATED, + PROTOBUF_C_TYPE_FLOAT, + offsetof(RACS2BridgeGeometryMsgs, n_polygon_stamped_dta), + offsetof(RACS2BridgeGeometryMsgs, polygon_stamped_dta), + NULL, + NULL, + 0 | PROTOBUF_C_FIELD_FLAG_PACKED, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, + { + "pose_array_data", + 17, + PROTOBUF_C_LABEL_REPEATED, + PROTOBUF_C_TYPE_FLOAT, + offsetof(RACS2BridgeGeometryMsgs, n_pose_array_data), + offsetof(RACS2BridgeGeometryMsgs, pose_array_data), + NULL, + NULL, + 0 | PROTOBUF_C_FIELD_FLAG_PACKED, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, + { + "vector3_stamped_data", + 18, + PROTOBUF_C_LABEL_REPEATED, + PROTOBUF_C_TYPE_FLOAT, + offsetof(RACS2BridgeGeometryMsgs, n_vector3_stamped_data), + offsetof(RACS2BridgeGeometryMsgs, vector3_stamped_data), + NULL, + NULL, + 0 | PROTOBUF_C_FIELD_FLAG_PACKED, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, + { + "pose_stamped_data", + 19, + PROTOBUF_C_LABEL_REPEATED, + PROTOBUF_C_TYPE_FLOAT, + offsetof(RACS2BridgeGeometryMsgs, n_pose_stamped_data), + offsetof(RACS2BridgeGeometryMsgs, pose_stamped_data), + NULL, + NULL, + 0 | PROTOBUF_C_FIELD_FLAG_PACKED, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, + { + "polygon_data", + 20, + PROTOBUF_C_LABEL_REPEATED, + PROTOBUF_C_TYPE_FLOAT, + offsetof(RACS2BridgeGeometryMsgs, n_polygon_data), + offsetof(RACS2BridgeGeometryMsgs, polygon_data), + NULL, + NULL, + 0 | PROTOBUF_C_FIELD_FLAG_PACKED, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, + { + "queaternion_data", + 21, + PROTOBUF_C_LABEL_REPEATED, + PROTOBUF_C_TYPE_FLOAT, + offsetof(RACS2BridgeGeometryMsgs, n_queaternion_data), + offsetof(RACS2BridgeGeometryMsgs, queaternion_data), + NULL, + NULL, + 0 | PROTOBUF_C_FIELD_FLAG_PACKED, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, + { + "point32_data", + 22, + PROTOBUF_C_LABEL_REPEATED, + PROTOBUF_C_TYPE_FLOAT, + offsetof(RACS2BridgeGeometryMsgs, n_point32_data), + offsetof(RACS2BridgeGeometryMsgs, point32_data), + NULL, + NULL, + 0 | PROTOBUF_C_FIELD_FLAG_PACKED, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, + { + "twist_with_covariance_data", + 23, + PROTOBUF_C_LABEL_REPEATED, + PROTOBUF_C_TYPE_FLOAT, + offsetof(RACS2BridgeGeometryMsgs, n_twist_with_covariance_data), + offsetof(RACS2BridgeGeometryMsgs, twist_with_covariance_data), + NULL, + NULL, + 0 | PROTOBUF_C_FIELD_FLAG_PACKED, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, + { + "pose_data", + 24, + PROTOBUF_C_LABEL_REPEATED, + PROTOBUF_C_TYPE_FLOAT, + offsetof(RACS2BridgeGeometryMsgs, n_pose_data), + offsetof(RACS2BridgeGeometryMsgs, pose_data), + NULL, + NULL, + 0 | PROTOBUF_C_FIELD_FLAG_PACKED, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, + { + "twist_stamped_data", + 25, + PROTOBUF_C_LABEL_REPEATED, + PROTOBUF_C_TYPE_FLOAT, + offsetof(RACS2BridgeGeometryMsgs, n_twist_stamped_data), + offsetof(RACS2BridgeGeometryMsgs, twist_stamped_data), + NULL, + NULL, + 0 | PROTOBUF_C_FIELD_FLAG_PACKED, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, + { + "pose_2d_data", + 26, + PROTOBUF_C_LABEL_REPEATED, + PROTOBUF_C_TYPE_FLOAT, + offsetof(RACS2BridgeGeometryMsgs, n_pose_2d_data), + offsetof(RACS2BridgeGeometryMsgs, pose_2d_data), + NULL, + NULL, + 0 | PROTOBUF_C_FIELD_FLAG_PACKED, /* flags */ + 0,NULL,NULL /* reserved1,reserved2, etc */ + }, +}; +static const unsigned racs2_bridge_geometry_msgs__field_indices_by_name[] = { + 10, /* field[10] = accel_data */ + 0, /* field[0] = accel_with_covariance_data */ + 1, /* field[1] = accel_with_covariance_stamped_data */ + 12, /* field[12] = accele_stamped_data */ + 6, /* field[6] = inertia_data */ + 3, /* field[3] = inertia_stamped_data */ + 21, /* field[21] = point32_data */ + 14, /* field[14] = point_data */ + 2, /* field[2] = point_stamped_data */ + 19, /* field[19] = polygon_data */ + 15, /* field[15] = polygon_stamped_dta */ + 25, /* field[25] = pose_2d_data */ + 16, /* field[16] = pose_array_data */ + 23, /* field[23] = pose_data */ + 18, /* field[18] = pose_stamped_data */ + 7, /* field[7] = pose_with_covariance_data */ + 11, /* field[11] = pose_with_covariance_stamped_data */ + 4, /* field[4] = quaternion_stamped_data */ + 20, /* field[20] = queaternion_data */ + 13, /* field[13] = transform_data */ + 5, /* field[5] = transform_stamped_data */ + 8, /* field[8] = twist_covariance_stamped_data */ + 24, /* field[24] = twist_stamped_data */ + 22, /* field[22] = twist_with_covariance_data */ + 17, /* field[17] = vector3_stamped_data */ + 9, /* field[9] = wrench_data */ +}; +static const ProtobufCIntRange racs2_bridge_geometry_msgs__number_ranges[1 + 1] = +{ + { 1, 0 }, + { 0, 26 } +}; +const ProtobufCMessageDescriptor racs2_bridge_geometry_msgs__descriptor = +{ + PROTOBUF_C__MESSAGE_DESCRIPTOR_MAGIC, + "RACS2Bridge_geometry_msgs", + "RACS2BridgeGeometryMsgs", + "RACS2BridgeGeometryMsgs", + "", + sizeof(RACS2BridgeGeometryMsgs), + 26, + racs2_bridge_geometry_msgs__field_descriptors, + racs2_bridge_geometry_msgs__field_indices_by_name, + 1, racs2_bridge_geometry_msgs__number_ranges, + (ProtobufCMessageInit) racs2_bridge_geometry_msgs__init, + NULL,NULL,NULL /* reserved[123] */ +}; diff --git a/racs2_demos_on_spaceros/cFS/apps/run_app/fsw/src/RACS2Bridge_geometry_msgs.pb-c.h b/racs2_demos_on_spaceros/cFS/apps/run_app/fsw/src/RACS2Bridge_geometry_msgs.pb-c.h new file mode 100644 index 0000000..f069dba --- /dev/null +++ b/racs2_demos_on_spaceros/cFS/apps/run_app/fsw/src/RACS2Bridge_geometry_msgs.pb-c.h @@ -0,0 +1,277 @@ +/* Generated by the protocol buffer compiler. DO NOT EDIT! */ +/* Generated from: RACS2Bridge_geometry_msgs.proto */ + +#ifndef PROTOBUF_C_RACS2Bridge_5fgeometry_5fmsgs_2eproto__INCLUDED +#define PROTOBUF_C_RACS2Bridge_5fgeometry_5fmsgs_2eproto__INCLUDED + +#include + +PROTOBUF_C__BEGIN_DECLS + +#if PROTOBUF_C_VERSION_NUMBER < 1000000 +# error This file was generated by a newer version of protoc-c which is incompatible with your libprotobuf-c headers. Please update your headers. +#elif 1003003 < PROTOBUF_C_MIN_COMPILER_VERSION +# error This file was generated by an older version of protoc-c which is incompatible with your libprotobuf-c headers. Please regenerate this file with a newer version of protoc-c. +#endif + + +typedef struct _RACS2BridgeGeometryMsgsVector3 RACS2BridgeGeometryMsgsVector3; +typedef struct _RACS2BridgeGeometryMsgsTwist RACS2BridgeGeometryMsgsTwist; +typedef struct _RACS2BridgeGeometryMsgs RACS2BridgeGeometryMsgs; + + +/* --- enums --- */ + + +/* --- messages --- */ + +struct _RACS2BridgeGeometryMsgsVector3 +{ + ProtobufCMessage base; + /* + * -- geometry_msgs/Vector3 ---------------------------- + */ + double x; + double y; + double z; +}; +#define RACS2_BRIDGE_GEOMETRY_MSGS__VECTOR3__INIT \ + { PROTOBUF_C_MESSAGE_INIT (&racs2_bridge_geometry_msgs__vector3__descriptor) \ + , 0, 0, 0 } + + +struct _RACS2BridgeGeometryMsgsTwist +{ + ProtobufCMessage base; + /* + * -- geometry_msgs/Twist ------------------------------ + */ + RACS2BridgeGeometryMsgsVector3 *linear; + RACS2BridgeGeometryMsgsVector3 *angular; +}; +#define RACS2_BRIDGE_GEOMETRY_MSGS__TWIST__INIT \ + { PROTOBUF_C_MESSAGE_INIT (&racs2_bridge_geometry_msgs__twist__descriptor) \ + , NULL, NULL } + + +struct _RACS2BridgeGeometryMsgs +{ + ProtobufCMessage base; + /* + * -- geometry_msgs/AccelWithCovariance ---------------- + */ + size_t n_accel_with_covariance_data; + float *accel_with_covariance_data; + /* + * -- geometry_msgs/AccelWithCovarianceStamped --------- + */ + size_t n_accel_with_covariance_stamped_data; + float *accel_with_covariance_stamped_data; + /* + * -- geometry_msgs/PointStamped ----------------------- + */ + size_t n_point_stamped_data; + float *point_stamped_data; + /* + * -- geometry_msgs/InertiaStamped --------------------- + */ + size_t n_inertia_stamped_data; + float *inertia_stamped_data; + /* + * -- geometry_msgs/QuaternionStamped ------------------ + */ + size_t n_quaternion_stamped_data; + float *quaternion_stamped_data; + /* + * -- geometry_msgs/TransformStamped ------------------- + */ + size_t n_transform_stamped_data; + float *transform_stamped_data; + /* + * -- geometry_msgs/Inertia ---------------------------- + */ + size_t n_inertia_data; + float *inertia_data; + /* + * -- geometry_msgs/PoseWithCovariance ----------------- + */ + size_t n_pose_with_covariance_data; + float *pose_with_covariance_data; + /* + * -- geometry_msgs/TwistWithCovarianceStamped --------- + */ + size_t n_twist_covariance_stamped_data; + float *twist_covariance_stamped_data; + /* + * -- geometry_msgs/Wrench ----------------------------- + */ + size_t n_wrench_data; + float *wrench_data; + /* + * -- geometry_msgs/Accel ------------------------------ + */ + size_t n_accel_data; + float *accel_data; + /* + * -- geometry_msgs/PoseWithCovarianceStamped ---------- + */ + size_t n_pose_with_covariance_stamped_data; + float *pose_with_covariance_stamped_data; + /* + * -- geometry_msgs/AccelStamped ----------------------- + */ + size_t n_accele_stamped_data; + float *accele_stamped_data; + /* + * -- geometry_msgs/Transform -------------------------- + */ + size_t n_transform_data; + float *transform_data; + /* + * -- geometry_msgs/Point ------------------------------ + */ + size_t n_point_data; + float *point_data; + /* + * -- geometry_msgs/PolygonStamped --------------------- + */ + size_t n_polygon_stamped_dta; + float *polygon_stamped_dta; + /* + * -- geometry_msgs/PoseArray -------------------------- + */ + size_t n_pose_array_data; + float *pose_array_data; + /* + * -- geometry_msgs/Vector3Stamped --------------------- + */ + size_t n_vector3_stamped_data; + float *vector3_stamped_data; + /* + * -- geometry_msgs/PoseStamped ------------------------ + */ + size_t n_pose_stamped_data; + float *pose_stamped_data; + /* + * -- geometry_msgs/Polygon ---------------------------- + */ + size_t n_polygon_data; + float *polygon_data; + /* + * -- geometry_msgs/Quaternion ------------------------- + */ + size_t n_queaternion_data; + float *queaternion_data; + /* + * -- geometry_msgs/Point32 ---------------------------- + */ + size_t n_point32_data; + float *point32_data; + /* + * -- geometry_msgs/TwistWithCovariance ---------------- + */ + size_t n_twist_with_covariance_data; + float *twist_with_covariance_data; + /* + * -- geometry_msgs/Pose ------------------------------- + */ + size_t n_pose_data; + float *pose_data; + /* + * -- geometry_msgs/TwistStamped ----------------------- + */ + size_t n_twist_stamped_data; + float *twist_stamped_data; + /* + * -- geometry_msgs/Pose2D ----------------------------- + */ + size_t n_pose_2d_data; + float *pose_2d_data; +}; +#define RACS2_BRIDGE_GEOMETRY_MSGS__INIT \ + { PROTOBUF_C_MESSAGE_INIT (&racs2_bridge_geometry_msgs__descriptor) \ + , 0,NULL, 0,NULL, 0,NULL, 0,NULL, 0,NULL, 0,NULL, 0,NULL, 0,NULL, 0,NULL, 0,NULL, 0,NULL, 0,NULL, 0,NULL, 0,NULL, 0,NULL, 0,NULL, 0,NULL, 0,NULL, 0,NULL, 0,NULL, 0,NULL, 0,NULL, 0,NULL, 0,NULL, 0,NULL, 0,NULL } + + +/* RACS2BridgeGeometryMsgsVector3 methods */ +void racs2_bridge_geometry_msgs__vector3__init + (RACS2BridgeGeometryMsgsVector3 *message); +size_t racs2_bridge_geometry_msgs__vector3__get_packed_size + (const RACS2BridgeGeometryMsgsVector3 *message); +size_t racs2_bridge_geometry_msgs__vector3__pack + (const RACS2BridgeGeometryMsgsVector3 *message, + uint8_t *out); +size_t racs2_bridge_geometry_msgs__vector3__pack_to_buffer + (const RACS2BridgeGeometryMsgsVector3 *message, + ProtobufCBuffer *buffer); +RACS2BridgeGeometryMsgsVector3 * + racs2_bridge_geometry_msgs__vector3__unpack + (ProtobufCAllocator *allocator, + size_t len, + const uint8_t *data); +void racs2_bridge_geometry_msgs__vector3__free_unpacked + (RACS2BridgeGeometryMsgsVector3 *message, + ProtobufCAllocator *allocator); +/* RACS2BridgeGeometryMsgsTwist methods */ +void racs2_bridge_geometry_msgs__twist__init + (RACS2BridgeGeometryMsgsTwist *message); +size_t racs2_bridge_geometry_msgs__twist__get_packed_size + (const RACS2BridgeGeometryMsgsTwist *message); +size_t racs2_bridge_geometry_msgs__twist__pack + (const RACS2BridgeGeometryMsgsTwist *message, + uint8_t *out); +size_t racs2_bridge_geometry_msgs__twist__pack_to_buffer + (const RACS2BridgeGeometryMsgsTwist *message, + ProtobufCBuffer *buffer); +RACS2BridgeGeometryMsgsTwist * + racs2_bridge_geometry_msgs__twist__unpack + (ProtobufCAllocator *allocator, + size_t len, + const uint8_t *data); +void racs2_bridge_geometry_msgs__twist__free_unpacked + (RACS2BridgeGeometryMsgsTwist *message, + ProtobufCAllocator *allocator); +/* RACS2BridgeGeometryMsgs methods */ +void racs2_bridge_geometry_msgs__init + (RACS2BridgeGeometryMsgs *message); +size_t racs2_bridge_geometry_msgs__get_packed_size + (const RACS2BridgeGeometryMsgs *message); +size_t racs2_bridge_geometry_msgs__pack + (const RACS2BridgeGeometryMsgs *message, + uint8_t *out); +size_t racs2_bridge_geometry_msgs__pack_to_buffer + (const RACS2BridgeGeometryMsgs *message, + ProtobufCBuffer *buffer); +RACS2BridgeGeometryMsgs * + racs2_bridge_geometry_msgs__unpack + (ProtobufCAllocator *allocator, + size_t len, + const uint8_t *data); +void racs2_bridge_geometry_msgs__free_unpacked + (RACS2BridgeGeometryMsgs *message, + ProtobufCAllocator *allocator); +/* --- per-message closures --- */ + +typedef void (*RACS2BridgeGeometryMsgsVector3_Closure) + (const RACS2BridgeGeometryMsgsVector3 *message, + void *closure_data); +typedef void (*RACS2BridgeGeometryMsgsTwist_Closure) + (const RACS2BridgeGeometryMsgsTwist *message, + void *closure_data); +typedef void (*RACS2BridgeGeometryMsgs_Closure) + (const RACS2BridgeGeometryMsgs *message, + void *closure_data); + +/* --- services --- */ + + +/* --- descriptors --- */ + +extern const ProtobufCMessageDescriptor racs2_bridge_geometry_msgs__vector3__descriptor; +extern const ProtobufCMessageDescriptor racs2_bridge_geometry_msgs__twist__descriptor; +extern const ProtobufCMessageDescriptor racs2_bridge_geometry_msgs__descriptor; + +PROTOBUF_C__END_DECLS + + +#endif /* PROTOBUF_C_RACS2Bridge_5fgeometry_5fmsgs_2eproto__INCLUDED */ diff --git a/racs2_demos_on_spaceros/cFS/apps/run_app/fsw/src/racs2_user_msg.h b/racs2_demos_on_spaceros/cFS/apps/run_app/fsw/src/racs2_user_msg.h new file mode 100644 index 0000000..84dc66a --- /dev/null +++ b/racs2_demos_on_spaceros/cFS/apps/run_app/fsw/src/racs2_user_msg.h @@ -0,0 +1,22 @@ +#ifndef _racs2_user_msg_h_ +#define _racs2_user_msg_h_ + +/* +** Type definition (user data format for racs2 bridge) +*/ +#define ROS2_TOPIC_NAME_LNGTH 32 +#define BODY_DATA_MAX_LNGTH 128 + +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + char ros2_topic_name[ROS2_TOPIC_NAME_LNGTH]; + uint8 body_data_length; + uint8 body_data[BODY_DATA_MAX_LNGTH]; + +} OS_PACK racs2_user_msg_t ; + +#define RACS2_USER_MSG_LNGTH sizeof ( racs2_user_msg_t ) +#define RACS2_BRIDGE_USER_LISTENER_LNGTH sizeof ( racs2_user_msg_t ) + +#endif /* _racs2_user_msg_h_ */ diff --git a/racs2_demos_on_spaceros/cFS/apps/run_app/fsw/src/run_app.c b/racs2_demos_on_spaceros/cFS/apps/run_app/fsw/src/run_app.c new file mode 100644 index 0000000..3b87cd9 --- /dev/null +++ b/racs2_demos_on_spaceros/cFS/apps/run_app/fsw/src/run_app.c @@ -0,0 +1,436 @@ +/* +** Include Files: +*/ + +#include "run_app.h" +#include "run_app_perfids.h" +#include "run_app_msgids.h" +#include "run_app_msg.h" +#include "racs2_user_msg.h" +#include "run_app_events.h" +#include "run_app_version.h" +#include "RACS2Bridge_geometry_msgs.pb-c.h" + +#include +#include +#include +#include +#include +#include + +/* +** global data +*/ + +sample_hk_tlm_t RUN_APP_HkTelemetryPkt; +racs2_user_msg_t RACS2_UserMsgPkt; +CFE_SB_PipeId_t RUN_APP_CommandPipe; +CFE_SB_MsgPtr_t RUN_APP_MsgPtr; + +typedef enum { + FORWARD, + BACKWARD, + TURN_RIGHT, + TURN_LEFT, + STOP +} Command; + +Command current_command = STOP; +struct termios original_termios; + +double twist_linear_x = 0.0; +double twist_linear_y = 0.0; +double twist_linear_z = 0.0; +double twist_angular_x = 0.0; +double twist_angular_y = 0.0; +double twist_angular_z = 0.0; + +static CFE_EVS_BinFilter_t SAMPLE_EventFilters[] = + { /* Event ID mask */ + {SAMPLE_STARTUP_INF_EID, 0x0000}, + {SAMPLE_COMMAND_ERR_EID, 0x0000}, + {SAMPLE_COMMANDNOP_INF_EID, 0x0000}, + {SAMPLE_COMMANDRST_INF_EID, 0x0000}, + }; + + +void send_command(Command cmd) { + switch (cmd) { + case FORWARD: + printf("Moving Forward\n"); + twist_linear_x = 2.0; + break; + case BACKWARD: + printf("Moving Backward\n"); + twist_linear_x = -2.0; + break; + case TURN_RIGHT: + printf("Turning Right\n"); + twist_linear_x = 1.0; + twist_angular_z = -0.4; + break; + case TURN_LEFT: + printf("Turning Left\n"); + twist_linear_x = 1.0; + twist_angular_z = 0.4; + break; + case STOP: + printf("Stopping\n"); + twist_linear_x = 0.0; + twist_linear_y = 0.0; + twist_linear_z = 0.0; + twist_angular_x = 0.0; + twist_angular_y = 0.0; + twist_angular_z = 0.0; + break; + } +} + +void process_input(char input) { + switch (input) { + case 'w': + current_command = FORWARD; + printf("###### Moving Forward ###### \n"); + twist_linear_x = 2.0; + break; + case 's': + current_command = BACKWARD; + printf("###### Moving Backward ###### \n"); + twist_linear_x = -2.0; + break; + case 'd': + current_command = TURN_RIGHT; + printf("###### Turning Right ###### \n"); + twist_linear_x = 1.0; + twist_angular_z = -0.4; + break; + case 'a': + current_command = TURN_LEFT; + printf("###### Turning Left ###### \n"); + twist_linear_x = 1.0; + twist_angular_z = 0.4; + break; + case 'x': + current_command = STOP; + printf("###### Stopping ###### \n"); + twist_linear_x = 0.0; + twist_linear_y = 0.0; + twist_linear_z = 0.0; + twist_angular_x = 0.0; + twist_angular_y = 0.0; + twist_angular_z = 0.0; + break; + default: + printf("!!!!! Invalid input !!!!! \n"); + return; + } +} + +void set_non_canonical_mode(int fd) { + struct termios termios; + + // 現在の端末設定を取得 + tcgetattr(fd, &original_termios); + + // 設定を変更 + termios = original_termios; + termios.c_lflag &= ~(ICANON | ECHO); // 非カノニカルモード、エコーを無効にする + + // 変更を適用 + tcsetattr(fd, TCSANOW, &termios); +} + +void restore_canonical_mode(int fd) { + tcsetattr(fd, TCSANOW, &original_termios); +} + +void signal_handler(int signum) { + // 端末設定を元に戻す + restore_canonical_mode(fileno(stdin)); + printf("\nTerminating program.\n"); + exit(0); +} + + +/** * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ +/* RUN_APP_Main() -- Application entry point and main process loop */ +/* */ +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * **/ +void RUN_APP_Main( void ) +{ + int32 status; + uint32 RunStatus = CFE_ES_RunStatus_APP_RUN; + + OS_printf("RUN_APP_Main starts.\n"); + + CFE_ES_PerfLogEntry(SAMPLE_APP_PERF_ID); + + SAMPLE_TAKLKER_Init(); + + struct termios original_termios; + char input; + signal(SIGINT, signal_handler); + + /* + ** RUN_APP Runloop + */ + + int fd = fileno(stdin); + set_non_canonical_mode(fd); + + int count = 0; + while (CFE_ES_RunLoop(&RunStatus) == true) + { + if (read(fd, &input, 1) == 1) { + process_input(input); + } + + CFE_ES_PerfLogExit(SAMPLE_APP_PERF_ID); + + // /* Pend on receipt of command packet -- timeout set to 500 millisecs */ + // status = CFE_SB_RcvMsg(&RUN_APP_MsgPtr, RUN_APP_CommandPipe, 500); + + CFE_ES_PerfLogEntry(SAMPLE_APP_PERF_ID); + + // if (status == CFE_SUCCESS) + // { + // RUN_APP_ProcessCommandPacket(); + // } + sleep(2); + + // send message + // set topic name + strcpy(RACS2_UserMsgPkt.ros2_topic_name, "/cmd_vel"); + // define serialized body data + uint8_t *buffer; + + RACS2BridgeGeometryMsgsVector3 linear = RACS2_BRIDGE_GEOMETRY_MSGS__VECTOR3__INIT; + RACS2BridgeGeometryMsgsVector3 angular = RACS2_BRIDGE_GEOMETRY_MSGS__VECTOR3__INIT; + RACS2BridgeGeometryMsgsTwist message = RACS2_BRIDGE_GEOMETRY_MSGS__TWIST__INIT; + + message.linear = &linear; + message.angular = &angular; + message.linear->x = twist_linear_x; + message.linear->y = twist_linear_y; + message.linear->z = twist_linear_z; + message.angular->x = twist_angular_x; + message.angular->y = twist_angular_y; + message.angular->z = twist_angular_z; + // message.linear->x = 2.0; + // message.linear->y = 0.0; + // message.linear->z = 0.0; + // message.angular->x = 0.0; + // message.angular->y = 0.0; + // message.angular->z = 0.0; + + OS_printf("RUN_APP: [Send][MsgID=0x%x][linear: x = %5.2f, y = %5.2f, z = %5.2f][angular: x = %5.2f, y = %5.2f, z = %5.2f]\n", + RACS2_BRIDGE_MID, + message.linear->x, + message.linear->y, + message.linear->z, + message.angular->x, + message.angular->y, + message.angular->z + ); + + size_t len = protobuf_c_message_get_packed_size((const ProtobufCMessage *) &message); + buffer=malloc(len); + size_t packed_len = protobuf_c_message_pack((const ProtobufCMessage *)&message, buffer); + + // set body data + memcpy(RACS2_UserMsgPkt.body_data, buffer, len); + // set body data length + RACS2_UserMsgPkt.body_data_length = len; + + // send data + CFE_SB_TimeStampMsg((CFE_SB_Msg_t *) &RACS2_UserMsgPkt); + status = CFE_SB_SendMsg((CFE_SB_Msg_t *) &RACS2_UserMsgPkt); + // OS_printf("RUN_APP: Sent message, MID = [0x%x], sample_command_count = %d\n", + // CFE_SB_GetMsgId((CFE_SB_MsgPtr_t) &RACS2_UserMsgPkt), + // RACS2_UserMsgPkt.sample_command_count + // ); + if (status != CFE_SUCCESS) { + OS_printf("RUN_APP: Error: sending is failed. status = 0x%x\n", status); + } + + free(buffer); + + count++; + } + + CFE_ES_ExitApp(RunStatus); + +} /* End of RUN_APP_Main() */ + +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ +/* */ +/* SAMPLE_TAKLKER_Init() -- initialization */ +/* */ +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * **/ +void SAMPLE_TAKLKER_Init(void) +{ + /* + ** Register the app with Executive services + */ + CFE_ES_RegisterApp() ; + + /* + ** Register the events + */ + CFE_EVS_Register(SAMPLE_EventFilters, + sizeof(SAMPLE_EventFilters)/sizeof(CFE_EVS_BinFilter_t), + CFE_EVS_EventFilter_BINARY); + + RUN_APP_ResetCounters(); + + CFE_SB_InitMsg(&RUN_APP_HkTelemetryPkt, + RUN_APP_HK_TLM_MID, + SAMPLE_APP_HK_TLM_LNGTH, true); + + CFE_SB_InitMsg(&RACS2_UserMsgPkt, RACS2_BRIDGE_MID, RACS2_USER_MSG_LNGTH, false); + + CFE_EVS_SendEvent (SAMPLE_STARTUP_INF_EID, CFE_EVS_EventType_INFORMATION, + "RUN_APP App Initialized. Version %d.%d.%d.%d", + SAMPLE_APP_MAJOR_VERSION, + SAMPLE_APP_MINOR_VERSION, + SAMPLE_APP_REVISION, + SAMPLE_APP_MISSION_REV); + +} /* End of SAMPLE_TAKLKER_Init() */ + +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * **/ +/* Name: RUN_APP_ProcessCommandPacket */ +/* */ +/* Purpose: */ +/* This routine will process any packet that is received on the RUN_APP */ +/* command pipe. */ +/* */ +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ +void RUN_APP_ProcessCommandPacket(void) +{ + CFE_SB_MsgId_t MsgId; + + MsgId = CFE_SB_GetMsgId(RUN_APP_MsgPtr); + + switch (MsgId) + { + case RUN_APP_CMD_MID: + RUN_APP_ProcessGroundCommand(); + break; + + case RUN_APP_SEND_HK_MID: + RUN_APP_ReportHousekeeping(); + break; + + default: + RUN_APP_HkTelemetryPkt.sample_command_error_count++; + CFE_EVS_SendEvent(SAMPLE_COMMAND_ERR_EID,CFE_EVS_EventType_ERROR, + "RUN_APP: invalid command packet,MID = 0x%x", MsgId); + break; + } + + return; + +} /* End RUN_APP_ProcessCommandPacket */ + +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * **/ +/* */ +/* RUN_APP_ProcessGroundCommand() -- RUN_APP ground commands */ +/* */ +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * **/ + +void RUN_APP_ProcessGroundCommand(void) +{ + uint16 CommandCode; + + CommandCode = CFE_SB_GetCmdCode(RUN_APP_MsgPtr); + + /* Process "known" RUN_APP app ground commands */ + switch (CommandCode) + { + case SAMPLE_APP_NOOP_CC: + RUN_APP_HkTelemetryPkt.sample_command_count++; + CFE_EVS_SendEvent(SAMPLE_COMMANDNOP_INF_EID, + CFE_EVS_EventType_INFORMATION, + "RUN_APP: NOOP command"); + break; + + case SAMPLE_APP_RESET_COUNTERS_CC: + RUN_APP_ResetCounters(); + break; + + /* default case already found during FC vs length test */ + default: + break; + } + return; + +} /* End of RUN_APP_ProcessGroundCommand() */ + +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * **/ +/* Name: RUN_APP_ReportHousekeeping */ +/* */ +/* Purpose: */ +/* This function is triggered in response to a task telemetry request */ +/* from the housekeeping task. This function will gather the Apps */ +/* telemetry, packetize it and send it to the housekeeping task via */ +/* the software bus */ +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ +void RUN_APP_ReportHousekeeping(void) +{ + CFE_SB_TimeStampMsg((CFE_SB_Msg_t *) &RUN_APP_HkTelemetryPkt); + CFE_SB_SendMsg((CFE_SB_Msg_t *) &RUN_APP_HkTelemetryPkt); + return; + +} /* End of RUN_APP_ReportHousekeeping() */ + +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * **/ +/* Name: RUN_APP_ResetCounters */ +/* */ +/* Purpose: */ +/* This function resets all the global counter variables that are */ +/* part of the task telemetry. */ +/* */ +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */ +void RUN_APP_ResetCounters(void) +{ + /* Status of commands processed by the RUN_APP App */ + RUN_APP_HkTelemetryPkt.sample_command_count = 0; + RUN_APP_HkTelemetryPkt.sample_command_error_count = 0; + + CFE_EVS_SendEvent(SAMPLE_COMMANDRST_INF_EID, CFE_EVS_EventType_INFORMATION, + "RUN_APP: RESET command"); + return; + +} /* End of RUN_APP_ResetCounters() */ + +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * **/ +/* */ +/* RUN_APP_VerifyCmdLength() -- Verify command packet length */ +/* */ +/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * **/ +bool RUN_APP_VerifyCmdLength(CFE_SB_MsgPtr_t msg, uint16 ExpectedLength) +{ + bool result = true; + + uint16 ActualLength = CFE_SB_GetTotalMsgLength(msg); + + /* + ** Verify the command packet length. + */ + if (ExpectedLength != ActualLength) + { + CFE_SB_MsgId_t MessageID = CFE_SB_GetMsgId(msg); + uint16 CommandCode = CFE_SB_GetCmdCode(msg); + + CFE_EVS_SendEvent(SAMPLE_LEN_ERR_EID, CFE_EVS_EventType_ERROR, + "Invalid msg length: ID = 0x%X, CC = %d, Len = %d, Expected = %d", + MessageID, CommandCode, ActualLength, ExpectedLength); + result = false; + RUN_APP_HkTelemetryPkt.sample_command_error_count++; + } + + return(result); + +} /* End of RUN_APP_VerifyCmdLength() */ + diff --git a/racs2_demos_on_spaceros/cFS/apps/run_app/fsw/src/run_app.h b/racs2_demos_on_spaceros/cFS/apps/run_app/fsw/src/run_app.h new file mode 100644 index 0000000..481095d --- /dev/null +++ b/racs2_demos_on_spaceros/cFS/apps/run_app/fsw/src/run_app.h @@ -0,0 +1,41 @@ +#ifndef _run_app_h_ +#define _run_app_h_ + +/* +** Required header files. +*/ +#include "cfe.h" +#include "cfe_error.h" +#include "cfe_evs.h" +#include "cfe_sb.h" +#include "cfe_es.h" + +#include +#include +#include + +/***********************************************************************/ + +#define SAMPLE_PIPE_DEPTH 32 + +/************************************************************************ +** Type Definitions +*************************************************************************/ + +/****************************************************************************/ +/* +** Local function prototypes. +** +** Note: Except for the entry point (RUN_APP_Main), these +** functions are not called from any other source module. +*/ +void RUN_APP_Main(void); +void SAMPLE_TAKLKER_Init(void); +void RUN_APP_ProcessCommandPacket(void); +void RUN_APP_ProcessGroundCommand(void); +void RUN_APP_ReportHousekeeping(void); +void RUN_APP_ResetCounters(void); + +bool RUN_APP_VerifyCmdLength(CFE_SB_MsgPtr_t msg, uint16 ExpectedLength); + +#endif /* _run_app_h_ */ diff --git a/racs2_demos_on_spaceros/cFS/apps/run_app/fsw/src/run_app_events.h b/racs2_demos_on_spaceros/cFS/apps/run_app/fsw/src/run_app_events.h new file mode 100644 index 0000000..5e324d1 --- /dev/null +++ b/racs2_demos_on_spaceros/cFS/apps/run_app/fsw/src/run_app_events.h @@ -0,0 +1,13 @@ +#ifndef _run_app_events_h_ +#define _run_app_events_h_ + + +#define SAMPLE_RESERVED_EID 20 +#define SAMPLE_STARTUP_INF_EID 21 +#define SAMPLE_COMMAND_ERR_EID 22 +#define SAMPLE_COMMANDNOP_INF_EID 23 +#define SAMPLE_COMMANDRST_INF_EID 24 +#define SAMPLE_INVALID_MSGID_ERR_EID 25 +#define SAMPLE_LEN_ERR_EID 26 + +#endif /* _run_app_events_h_ */ diff --git a/racs2_demos_on_spaceros/cFS/apps/run_app/fsw/src/run_app_msg.h b/racs2_demos_on_spaceros/cFS/apps/run_app/fsw/src/run_app_msg.h new file mode 100644 index 0000000..7e82b18 --- /dev/null +++ b/racs2_demos_on_spaceros/cFS/apps/run_app/fsw/src/run_app_msg.h @@ -0,0 +1,36 @@ +#ifndef _run_app_msg_h_ +#define _run_app_msg_h_ + +/* +** SAMPLE App command codes +*/ +#define SAMPLE_APP_NOOP_CC 0 +#define SAMPLE_APP_RESET_COUNTERS_CC 1 + +/*************************************************************************/ +/* +** Type definition (generic "no arguments" command) +*/ +typedef struct +{ + uint8 CmdHeader[CFE_SB_CMD_HDR_SIZE]; + +} SAMPLE_NoArgsCmd_t; + +/*************************************************************************/ +/* +** Type definition (SAMPLE App housekeeping) +*/ +typedef struct +{ + uint8 TlmHeader[CFE_SB_TLM_HDR_SIZE]; + uint8 sample_command_error_count; + uint8 sample_command_count; + uint8 spare[2]; + +} OS_PACK sample_hk_tlm_t ; + +#define SAMPLE_APP_HK_TLM_LNGTH sizeof ( sample_hk_tlm_t ) +#define RUN_APP_LISTENER_LNGTH sizeof ( sample_hk_tlm_t ) + +#endif /* _run_app_msg_h_ */ diff --git a/racs2_demos_on_spaceros/cFS/apps/run_app/fsw/src/run_app_version.h b/racs2_demos_on_spaceros/cFS/apps/run_app/fsw/src/run_app_version.h new file mode 100644 index 0000000..f6ea9ea --- /dev/null +++ b/racs2_demos_on_spaceros/cFS/apps/run_app/fsw/src/run_app_version.h @@ -0,0 +1,9 @@ +#ifndef _run_app_version_h_ +#define _run_app_version_h_ + +#define SAMPLE_APP_MAJOR_VERSION 1 +#define SAMPLE_APP_MINOR_VERSION 1 +#define SAMPLE_APP_REVISION 0 +#define SAMPLE_APP_MISSION_REV 0 + +#endif /* _run_app_version_h_ */ diff --git a/racs2_demos_on_spaceros/cFS/sample_defs/cpu1_cfe_es_startup.scr b/racs2_demos_on_spaceros/cFS/sample_defs/cpu1_cfe_es_startup.scr new file mode 100644 index 0000000..d53441a --- /dev/null +++ b/racs2_demos_on_spaceros/cFS/sample_defs/cpu1_cfe_es_startup.scr @@ -0,0 +1,34 @@ +CFE_APP, /cf/racs2_bridge_client.so, RACS2_BRIDGE_CLIENT_Main, RACS2_BRIDGE_CLIENT, 55, 16384, 0x0, 0; +CFE_APP, /cf/run_app.so, RUN_APP_Main, RUN_APP, 57, 16384, 0x0, 0; +! +! CFE_LIB, /cf/sample_lib.so, SAMPLE_LibInit, SAMPLE_LIB, 0, 0, 0x0, 0; +! CFE_APP, /cf/racs2_bridge_client.so, RACS2_BRIDGE_CLIENT_Main, RACS2_BRIDGE_CLIENT, 55, 16384, 0x0, 0; +! CFE_APP, /cf/sample_listener.so, SAMPLE_LISTENER_Main, SAMPLE_LISTENER, 56, 16384, 0x0, 0; +! CFE_APP, /cf/ci_lab.so, CI_Lab_AppMain, CI_LAB_APP, 60, 16384, 0x0, 0; +! CFE_APP, /cf/to_lab.so, TO_Lab_AppMain, TO_LAB_APP, 70, 16384, 0x0, 0; +! CFE_APP, /cf/sch_lab.so, SCH_Lab_AppMain, SCH_LAB_APP, 80, 16384, 0x0, 0; +! +! Startup script fields: +! 1. Object Type -- CFE_APP for an Application, or CFE_LIB for a library. +! 2. Path/Filename -- This is a cFE Virtual filename, not a vxWorks device/pathname +! 3. Entry Point -- This is the "main" function for Apps. +! 4. CFE Name -- The cFE name for the the APP or Library +! 5. Priority -- This is the Priority of the App, not used for Library +! 6. Stack Size -- This is the Stack size for the App, not used for the Library +! 7. Load Address -- This is the Optional Load Address for the App or Library. Currently not implemented +! so keep it at 0x0. +! 8. Exception Action -- This is the Action the cFE should take if the App has an exception. +! 0 = Just restart the Application +! Non-Zero = Do a cFE Processor Reset +! +! Other Notes: +! 1. The software will not try to parse anything after the first '!' character it sees. That +! is the End of File marker. +! 2. Common Application file extensions: +! Linux = .so ( ci.so ) +! OS X = .bundle ( ci.bundle ) +! Cygwin = .dll ( ci.dll ) +! vxWorks = .o ( ci.o ) +! RTEMS with S-record Loader = .s3r ( ci.s3r ) +! RTEMS with CEXP Loader = .o ( ci.o ) + diff --git a/racs2_demos_on_spaceros/cFS/sample_defs/racs2_bridge_config.txt b/racs2_demos_on_spaceros/cFS/sample_defs/racs2_bridge_config.txt new file mode 100644 index 0000000..9502c84 --- /dev/null +++ b/racs2_demos_on_spaceros/cFS/sample_defs/racs2_bridge_config.txt @@ -0,0 +1,2 @@ +wss_uri=localhost +wss_port=8765 \ No newline at end of file diff --git a/racs2_demos_on_spaceros/cFS/sample_defs/targets.cmake b/racs2_demos_on_spaceros/cFS/sample_defs/targets.cmake new file mode 100644 index 0000000..12e23c7 --- /dev/null +++ b/racs2_demos_on_spaceros/cFS/sample_defs/targets.cmake @@ -0,0 +1,100 @@ +###################################################################### +# +# Master config file for cFS target boards +# +# This file indicates the architecture and configuration of the +# target boards that will run core flight software. +# +# The following variables are defined per board, where is the +# CPU number starting with 1: +# +# TGT_NAME : the user-friendly name of the cpu. Should be simple +# word with no punctuation. This MUST be specified. +# TGT_APPLIST : list of applications to build and install on the CPU. +# These are built as dynamically-loaded applications and installed +# as files in the non-volatile storage of the target, and loaded +# at runtime via the startup script or commands. +# TGT_STATIC_APPLIST : list of applications to build and statically +# link with the CFE executable. This is similar to the "APPLIST" +# except the application is built with STATIC linkage, and it is +# included directly when linking the CFE core executable itself. +# No separate application file is generated for these apps. +# TGT_STATIC_SYMLIST : list of symbols to include in the OSAL static +# symbol lookup table. Each entry is a comma-separated pair containing +# the symbol name and virtual module/app name, such as +# My_C_Function_Name,MY_APP +# The first item must be a publicly-exposed C symbol name available to +# the linker at static link time, generally the entry point/main function +# of the a module or library (see STATIC_APPLIST). The second item is the +# module name that should match the name used in the CFE startup script +# (4th parameter). +# IMPORTANT: For this to work, the OS_STATIC_LOADER configuration option +# must be specified in the osconfig.h for that CPU. +# TGT_PSP_MODULELIST : additional PSP "modules" to link into the +# CFE executable for this target. These can be device drivers or +# other bits of modular PSP functionality that provide I/O or other +# low level functions. +# TGT_FILELIST : list of extra files to copy onto the target. No +# modifications of the file will be made. In order to differentiate +# between different versions of files with the same name, priority +# will be given to a file named _ to be installed +# as simply on that cpu (prefix will be removed). These +# files are intended to be copied to the non-volatile storage on the +# target for use during runtime. +# TGT_EMBED_FILELIST : list of extra files which are to be converted +# into data arrays and linked with/embedded into the CFE executable, +# so the content of the files can be available at runtime on systems +# that do not have run time non-volatile storage. The format of each +# list entry is a comma-separated pair of variable and file name: +# VARIABLE_NAME,FILE_NAME +# The binary contents of the file will subsequently be available as: +# extern const char VARIABLE_NAME_DATA[] and +# extern const unsigned long VARIABLE_NAME_SIZE +# The same prefix-based filename mapping as used on FILELIST is also +# employed here, allowing CPU-specific data files to be used. +# TGT_SYSTEM : the toolchain to use for building all code. This +# will map to a CMake toolchain file called "toolchain-" +# If not specified then it will default to "cpu" so that +# each CPU will have a dedicated toolchain file and no objects +# will be shared across CPUs. +# Otherwise any code built using the same toolchain may be +# copied to multiple CPUs for more efficient builds. +# TGT_PLATFORM : configuration for the CFE core to use for this +# cpu. This determines the cfe_platform_cfg.h to use during the +# build. Multiple files/components may be concatenated together +# allowing the config to be generated in a modular fashion. If +# not specified then it will be assumed as "default ". +# + +# The MISSION_NAME will be compiled into the target build data structure +# as well as being passed to "git describe" to filter the tags when building +# the version string. +SET(MISSION_NAME "SampleMission") + +# SPACECRAFT_ID gets compiled into the build data structure and the PSP may use it. +# should be an integer. +SET(SPACECRAFT_ID 42) + +# UI_INSTALL_SUBDIR indicates where the UI data files (included in some apps) should +# be copied during the install process. +SET(UI_INSTALL_SUBDIR "host/ui") + +# FT_INSTALL_SUBDIR indicates where the black box test data files (lua scripts) should +# be copied during the install process. +SET(FT_INSTALL_SUBDIR "host/functional-test") + +# Each target board can have its own HW arch selection and set of included apps +SET(TGT1_NAME cpu1) +SET(TGT1_APPLIST sample_app racs2_bridge_client run_app sample_lib ci_lab to_lab sch_lab) +SET(TGT1_FILELIST cfe_es_startup.scr racs2_bridge_config.txt) + +# CPU2/3 are duplicates of CPU1. These are not built by default anymore but are +# commented out to serve as an example of how one would configure multiple cpus. +#SET(TGT2_NAME cpu2) +#SET(TGT2_APPLIST sample_app ci_lab to_lab sch_lab) +#SET(TGT2_FILELIST cfe_es_startup.scr) + +#SET(TGT3_NAME cpu3) +#SET(TGT3_APPLIST sample_app ci_lab to_lab sch_lab) +#SET(TGT3_FILELIST cfe_es_startup.scr) + diff --git a/racs2_demos_on_spaceros/config/mars_rover_control.yaml b/racs2_demos_on_spaceros/config/mars_rover_control.yaml new file mode 100644 index 0000000..4390b1a --- /dev/null +++ b/racs2_demos_on_spaceros/config/mars_rover_control.yaml @@ -0,0 +1,116 @@ +controller_manager: + ros__parameters: + update_rate: 100 + + arm_joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + mast_joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + wheel_velocity_controller: + type: velocity_controllers/JointGroupVelocityController + + steer_position_controller: + type: joint_trajectory_controller/JointTrajectoryController + + wheel_tree_position_controller: + type: effort_controllers/JointGroupPositionController + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + +arm_joint_trajectory_controller: + ros__parameters: + joints: + - arm_01_joint + - arm_02_joint + - arm_03_joint + - arm_04_joint + - arm_tools_joint + interface_name: position + command_interfaces: + - position + state_interfaces: + - position + - velocity + +mast_joint_trajectory_controller: + ros__parameters: + joints: + - mast_p_joint + - mast_02_joint + - mast_cameras_joint + interface_name: position + command_interfaces: + - position + state_interfaces: + - position + - velocity + +wheel_velocity_controller: + ros__parameters: + joints: + - front_wheel_L_joint + - middle_wheel_L_joint + - back_wheel_L_joint + - front_wheel_R_joint + - middle_wheel_R_joint + - back_wheel_R_joint + interface_name: velocity + + +steer_position_controller: + ros__parameters: + joints: + - suspension_steer_F_L_joint + - suspension_steer_B_L_joint + - suspension_steer_F_R_joint + - suspension_steer_B_R_joint + interface_name: position + command_interfaces: + - position + state_interfaces: + - position + - velocity + +wheel_tree_position_controller: + ros__parameters: + joints: + - suspension_arm_F_L_joint + - suspension_arm_B_L_joint + - suspension_arm_F_R_joint + - suspension_arm_B_R_joint + command_interfaces: + - effort + state_interfaces: + - position + - velocity + - effort + gains: + suspension_arm_F_L_joint: + p: 2200.0 + i: 10.0 + d: 10.0 + suspension_arm_B_L_joint: + p: 4200.0 + i: 10.0 + d: 10.0 + suspension_arm_F_R_joint: + p: 2200.0 + i: 10.0 + d: 10.0 + suspension_arm_B_R_joint: + p: 4200.0 + i: 10.0 + d: 10.0 + enable: + suspension_arm_F_L_joint: + status: 1 + suspension_arm_B_L_joint: + status: 1 + suspension_arm_F_R_joint: + status: 1 + suspension_arm_B_R_joint: + status: 1 diff --git a/racs2_demos_on_spaceros/entrypoint.sh b/racs2_demos_on_spaceros/entrypoint.sh new file mode 100755 index 0000000..d1b51f2 --- /dev/null +++ b/racs2_demos_on_spaceros/entrypoint.sh @@ -0,0 +1,6 @@ +#!/bin/bash +set -e + +# Setup the Demo environment +source "${DEMO_DIR}/install/setup.bash" +exec "$@" diff --git a/racs2_demos_on_spaceros/launch/mars_rover.launch.py b/racs2_demos_on_spaceros/launch/mars_rover.launch.py new file mode 100644 index 0000000..82e9ebf --- /dev/null +++ b/racs2_demos_on_spaceros/launch/mars_rover.launch.py @@ -0,0 +1,194 @@ +from http.server import executable +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, RegisterEventHandler, IncludeLaunchDescription +from launch.substitutions import TextSubstitution, PathJoinSubstitution, LaunchConfiguration, Command +from launch_ros.actions import Node, SetParameter +from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.event_handlers import OnProcessExit, OnExecutionComplete +import os +from os import environ + +from ament_index_python.packages import get_package_share_directory + +import xacro + + + +# . ../spaceros_ws/install/setup.bash && . ../depends_ws/install/setup.bash +# rm -rf build install log && colcon build && . install/setup.bash + +def generate_launch_description(): + + mars_rover_demos_path = get_package_share_directory('mars_rover') + mars_rover_models_path = get_package_share_directory('simulation') + + env = {'IGN_GAZEBO_SYSTEM_PLUGIN_PATH': + ':'.join([environ.get('IGN_GAZEBO_SYSTEM_PLUGIN_PATH', default=''), + environ.get('LD_LIBRARY_PATH', default='')]), + 'IGN_GAZEBO_RESOURCE_PATH': + ':'.join([environ.get('IGN_GAZEBO_RESOURCE_PATH', default=''), mars_rover_demos_path])} + + urdf_model_path = os.path.join(mars_rover_models_path, 'models', 'curiosity_path', + 'urdf', 'curiosity_mars_rover.xacro') + mars_world_model = os.path.join(mars_rover_demos_path, 'worlds', 'mars_curiosity.world') + + doc = xacro.process_file(urdf_model_path) + robot_description = {'robot_description': doc.toxml()} + + arm_node = Node( + package="mars_rover", + executable="move_arm", + output='screen' + ) + + mast_node = Node( + package="mars_rover", + executable="move_mast", + output='screen' + ) + + wheel_node = Node( + package="mars_rover", + executable="move_wheel", + output='screen' + ) + + run_node = Node( + package="mars_rover", + executable="run_demo", + output='screen' + ) + + odom_node = Node( + package="mars_rover", + executable="odom_tf_publisher", + output='screen' + ) + + start_world = ExecuteProcess( + cmd=['ign gazebo', mars_world_model, '-r'], + output='screen', + additional_env=env, + shell=True + ) + + robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + parameters=[robot_description]) + + ros_gz_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + arguments=[ + '/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock', + '/model/curiosity_mars_rover/odometry@nav_msgs/msg/Odometry@ignition.msgs.Odometry', + '/scan@sensor_msgs/msg/LaserScan@ignition.msgs.LaserScan', + ], + output='screen') + + image_bridge = Node( + package='ros_gz_image', + executable='image_bridge', + arguments=['/image_raw', '/image_raw'], + output='screen') + + spawn = Node( + package='ros_ign_gazebo', executable='create', + arguments=[ + '-name', 'curiosity_mars_rover', + '-topic', robot_description, + '-z', '-7.5' + ], + output='screen' + + ) + + + ## Control Components + + component_state_msg = '{name: "IgnitionSystem", target_state: {id: 3, label: ""}}' + + ## a hack to resolve current bug + set_hardware_interface_active = ExecuteProcess( + cmd=['ros2', 'service', 'call', + 'controller_manager/set_hardware_component_state', + 'controller_manager_msgs/srv/SetHardwareComponentState', + component_state_msg] + ) + + load_joint_state_broadcaster = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'joint_state_broadcaster'], + output='screen' + ) + + load_arm_joint_traj_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'arm_joint_trajectory_controller'], + output='screen' + ) + + load_mast_joint_traj_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'mast_joint_trajectory_controller'], + output='screen' + ) + + load_wheel_joint_traj_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'wheel_velocity_controller'], + output='screen' + ) + + load_steer_joint_traj_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'steer_position_controller'], + output='screen' + ) + + load_suspension_joint_traj_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'wheel_tree_position_controller'], + output='screen' + ) + + return LaunchDescription([ + SetParameter(name='use_sim_time', value=True), + start_world, + robot_state_publisher, + spawn, + arm_node, + mast_node, + wheel_node, + # run_node, + odom_node, + ros_gz_bridge, + image_bridge, + + RegisterEventHandler( + OnProcessExit( + target_action=spawn, + on_exit=[set_hardware_interface_active], + ) + ), + RegisterEventHandler( + OnProcessExit( + target_action=set_hardware_interface_active, + on_exit=[load_joint_state_broadcaster], + ) + ), + RegisterEventHandler( + OnProcessExit( + target_action=load_joint_state_broadcaster, + on_exit=[load_arm_joint_traj_controller, + load_mast_joint_traj_controller, + load_wheel_joint_traj_controller, + load_steer_joint_traj_controller, + load_suspension_joint_traj_controller], + ) + ), + ]) diff --git a/racs2_demos_on_spaceros/mars_rover/__init__.py b/racs2_demos_on_spaceros/mars_rover/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/racs2_demos_on_spaceros/nodes/RACS2Bridge_geometry_msgs_pb2.py b/racs2_demos_on_spaceros/nodes/RACS2Bridge_geometry_msgs_pb2.py new file mode 100644 index 0000000..2ffdfea --- /dev/null +++ b/racs2_demos_on_spaceros/nodes/RACS2Bridge_geometry_msgs_pb2.py @@ -0,0 +1,374 @@ +# -*- coding: utf-8 -*- +# Generated by the protocol buffer compiler. DO NOT EDIT! +# source: RACS2Bridge_geometry_msgs.proto + +from google.protobuf import descriptor as _descriptor +from google.protobuf import message as _message +from google.protobuf import reflection as _reflection +from google.protobuf import symbol_database as _symbol_database +# @@protoc_insertion_point(imports) + +_sym_db = _symbol_database.Default() + + + + +DESCRIPTOR = _descriptor.FileDescriptor( + name='RACS2Bridge_geometry_msgs.proto', + package='', + syntax='proto2', + serialized_options=None, + create_key=_descriptor._internal_create_key, + serialized_pb=b'\n\x1fRACS2Bridge_geometry_msgs.proto\"D\n!RACS2Bridge_geometry_msgs_Vector3\x12\t\n\x01x\x18\x01 \x02(\x01\x12\t\n\x01y\x18\x02 \x02(\x01\x12\t\n\x01z\x18\x03 \x02(\x01\"\x8a\x01\n\x1fRACS2Bridge_geometry_msgs_Twist\x12\x32\n\x06linear\x18\x01 \x02(\x0b\x32\".RACS2Bridge_geometry_msgs_Vector3\x12\x33\n\x07\x61ngular\x18\x02 \x02(\x0b\x32\".RACS2Bridge_geometry_msgs_Vector3\"\xe9\x06\n\x19RACS2Bridge_geometry_msgs\x12&\n\x1a\x61\x63\x63\x65l_with_covariance_data\x18\x01 \x03(\x02\x42\x02\x10\x01\x12.\n\"accel_with_covariance_stamped_data\x18\x02 \x03(\x02\x42\x02\x10\x01\x12\x1e\n\x12point_stamped_data\x18\x03 \x03(\x02\x42\x02\x10\x01\x12 \n\x14inertia_stamped_data\x18\x04 \x03(\x02\x42\x02\x10\x01\x12#\n\x17quaternion_stamped_data\x18\x05 \x03(\x02\x42\x02\x10\x01\x12\"\n\x16transform_stamped_data\x18\x06 \x03(\x02\x42\x02\x10\x01\x12\x18\n\x0cinertia_data\x18\x07 \x03(\x02\x42\x02\x10\x01\x12%\n\x19pose_with_covariance_data\x18\x08 \x03(\x02\x42\x02\x10\x01\x12)\n\x1dtwist_covariance_stamped_data\x18\t \x03(\x02\x42\x02\x10\x01\x12\x17\n\x0bwrench_data\x18\n \x03(\x02\x42\x02\x10\x01\x12\x16\n\naccel_data\x18\x0b \x03(\x02\x42\x02\x10\x01\x12-\n!pose_with_covariance_stamped_data\x18\x0c \x03(\x02\x42\x02\x10\x01\x12\x1f\n\x13\x61\x63\x63\x65le_stamped_data\x18\r \x03(\x02\x42\x02\x10\x01\x12\x1a\n\x0etransform_data\x18\x0e \x03(\x02\x42\x02\x10\x01\x12\x16\n\npoint_data\x18\x0f \x03(\x02\x42\x02\x10\x01\x12\x1f\n\x13polygon_stamped_dta\x18\x10 \x03(\x02\x42\x02\x10\x01\x12\x1b\n\x0fpose_array_data\x18\x11 \x03(\x02\x42\x02\x10\x01\x12 \n\x14vector3_stamped_data\x18\x12 \x03(\x02\x42\x02\x10\x01\x12\x1d\n\x11pose_stamped_data\x18\x13 \x03(\x02\x42\x02\x10\x01\x12\x18\n\x0cpolygon_data\x18\x14 \x03(\x02\x42\x02\x10\x01\x12\x1c\n\x10queaternion_data\x18\x15 \x03(\x02\x42\x02\x10\x01\x12\x18\n\x0cpoint32_data\x18\x16 \x03(\x02\x42\x02\x10\x01\x12&\n\x1atwist_with_covariance_data\x18\x17 \x03(\x02\x42\x02\x10\x01\x12\x15\n\tpose_data\x18\x18 \x03(\x02\x42\x02\x10\x01\x12\x1e\n\x12twist_stamped_data\x18\x19 \x03(\x02\x42\x02\x10\x01\x12\x18\n\x0cpose_2d_data\x18\x1a \x03(\x02\x42\x02\x10\x01' +) + + + + +_RACS2BRIDGE_GEOMETRY_MSGS_VECTOR3 = _descriptor.Descriptor( + name='RACS2Bridge_geometry_msgs_Vector3', + full_name='RACS2Bridge_geometry_msgs_Vector3', + filename=None, + file=DESCRIPTOR, + containing_type=None, + create_key=_descriptor._internal_create_key, + fields=[ + _descriptor.FieldDescriptor( + name='x', full_name='RACS2Bridge_geometry_msgs_Vector3.x', index=0, + number=1, type=1, cpp_type=5, label=2, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='y', full_name='RACS2Bridge_geometry_msgs_Vector3.y', index=1, + number=2, type=1, cpp_type=5, label=2, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='z', full_name='RACS2Bridge_geometry_msgs_Vector3.z', index=2, + number=3, type=1, cpp_type=5, label=2, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + serialized_options=None, + is_extendable=False, + syntax='proto2', + extension_ranges=[], + oneofs=[ + ], + serialized_start=35, + serialized_end=103, +) + + +_RACS2BRIDGE_GEOMETRY_MSGS_TWIST = _descriptor.Descriptor( + name='RACS2Bridge_geometry_msgs_Twist', + full_name='RACS2Bridge_geometry_msgs_Twist', + filename=None, + file=DESCRIPTOR, + containing_type=None, + create_key=_descriptor._internal_create_key, + fields=[ + _descriptor.FieldDescriptor( + name='linear', full_name='RACS2Bridge_geometry_msgs_Twist.linear', index=0, + number=1, type=11, cpp_type=10, label=2, + has_default_value=False, default_value=None, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='angular', full_name='RACS2Bridge_geometry_msgs_Twist.angular', index=1, + number=2, type=11, cpp_type=10, label=2, + has_default_value=False, default_value=None, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + serialized_options=None, + is_extendable=False, + syntax='proto2', + extension_ranges=[], + oneofs=[ + ], + serialized_start=106, + serialized_end=244, +) + + +_RACS2BRIDGE_GEOMETRY_MSGS = _descriptor.Descriptor( + name='RACS2Bridge_geometry_msgs', + full_name='RACS2Bridge_geometry_msgs', + filename=None, + file=DESCRIPTOR, + containing_type=None, + create_key=_descriptor._internal_create_key, + fields=[ + _descriptor.FieldDescriptor( + name='accel_with_covariance_data', full_name='RACS2Bridge_geometry_msgs.accel_with_covariance_data', index=0, + number=1, type=2, cpp_type=6, label=3, + has_default_value=False, default_value=[], + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=b'\020\001', file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='accel_with_covariance_stamped_data', full_name='RACS2Bridge_geometry_msgs.accel_with_covariance_stamped_data', index=1, + number=2, type=2, cpp_type=6, label=3, + has_default_value=False, default_value=[], + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=b'\020\001', file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='point_stamped_data', full_name='RACS2Bridge_geometry_msgs.point_stamped_data', index=2, + number=3, type=2, cpp_type=6, label=3, + has_default_value=False, default_value=[], + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=b'\020\001', file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='inertia_stamped_data', full_name='RACS2Bridge_geometry_msgs.inertia_stamped_data', index=3, + number=4, type=2, cpp_type=6, label=3, + has_default_value=False, default_value=[], + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=b'\020\001', file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='quaternion_stamped_data', full_name='RACS2Bridge_geometry_msgs.quaternion_stamped_data', index=4, + number=5, type=2, cpp_type=6, label=3, + has_default_value=False, default_value=[], + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=b'\020\001', file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='transform_stamped_data', full_name='RACS2Bridge_geometry_msgs.transform_stamped_data', index=5, + number=6, type=2, cpp_type=6, label=3, + has_default_value=False, default_value=[], + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=b'\020\001', file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='inertia_data', full_name='RACS2Bridge_geometry_msgs.inertia_data', index=6, + number=7, type=2, cpp_type=6, label=3, + has_default_value=False, default_value=[], + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=b'\020\001', file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='pose_with_covariance_data', full_name='RACS2Bridge_geometry_msgs.pose_with_covariance_data', index=7, + number=8, type=2, cpp_type=6, label=3, + has_default_value=False, default_value=[], + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=b'\020\001', file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='twist_covariance_stamped_data', full_name='RACS2Bridge_geometry_msgs.twist_covariance_stamped_data', index=8, + number=9, type=2, cpp_type=6, label=3, + has_default_value=False, default_value=[], + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=b'\020\001', file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='wrench_data', full_name='RACS2Bridge_geometry_msgs.wrench_data', index=9, + number=10, type=2, cpp_type=6, label=3, + has_default_value=False, default_value=[], + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=b'\020\001', file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='accel_data', full_name='RACS2Bridge_geometry_msgs.accel_data', index=10, + number=11, type=2, cpp_type=6, label=3, + has_default_value=False, default_value=[], + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=b'\020\001', file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='pose_with_covariance_stamped_data', full_name='RACS2Bridge_geometry_msgs.pose_with_covariance_stamped_data', index=11, + number=12, type=2, cpp_type=6, label=3, + has_default_value=False, default_value=[], + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=b'\020\001', file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='accele_stamped_data', full_name='RACS2Bridge_geometry_msgs.accele_stamped_data', index=12, + number=13, type=2, cpp_type=6, label=3, + has_default_value=False, default_value=[], + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=b'\020\001', file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='transform_data', full_name='RACS2Bridge_geometry_msgs.transform_data', index=13, + number=14, type=2, cpp_type=6, label=3, + has_default_value=False, default_value=[], + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=b'\020\001', file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='point_data', full_name='RACS2Bridge_geometry_msgs.point_data', index=14, + number=15, type=2, cpp_type=6, label=3, + has_default_value=False, default_value=[], + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=b'\020\001', file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='polygon_stamped_dta', full_name='RACS2Bridge_geometry_msgs.polygon_stamped_dta', index=15, + number=16, type=2, cpp_type=6, label=3, + has_default_value=False, default_value=[], + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=b'\020\001', file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='pose_array_data', full_name='RACS2Bridge_geometry_msgs.pose_array_data', index=16, + number=17, type=2, cpp_type=6, label=3, + has_default_value=False, default_value=[], + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=b'\020\001', file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='vector3_stamped_data', full_name='RACS2Bridge_geometry_msgs.vector3_stamped_data', index=17, + number=18, type=2, cpp_type=6, label=3, + has_default_value=False, default_value=[], + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=b'\020\001', file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='pose_stamped_data', full_name='RACS2Bridge_geometry_msgs.pose_stamped_data', index=18, + number=19, type=2, cpp_type=6, label=3, + has_default_value=False, default_value=[], + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=b'\020\001', file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='polygon_data', full_name='RACS2Bridge_geometry_msgs.polygon_data', index=19, + number=20, type=2, cpp_type=6, label=3, + has_default_value=False, default_value=[], + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=b'\020\001', file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='queaternion_data', full_name='RACS2Bridge_geometry_msgs.queaternion_data', index=20, + number=21, type=2, cpp_type=6, label=3, + has_default_value=False, default_value=[], + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=b'\020\001', file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='point32_data', full_name='RACS2Bridge_geometry_msgs.point32_data', index=21, + number=22, type=2, cpp_type=6, label=3, + has_default_value=False, default_value=[], + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=b'\020\001', file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='twist_with_covariance_data', full_name='RACS2Bridge_geometry_msgs.twist_with_covariance_data', index=22, + number=23, type=2, cpp_type=6, label=3, + has_default_value=False, default_value=[], + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=b'\020\001', file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='pose_data', full_name='RACS2Bridge_geometry_msgs.pose_data', index=23, + number=24, type=2, cpp_type=6, label=3, + has_default_value=False, default_value=[], + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=b'\020\001', file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='twist_stamped_data', full_name='RACS2Bridge_geometry_msgs.twist_stamped_data', index=24, + number=25, type=2, cpp_type=6, label=3, + has_default_value=False, default_value=[], + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=b'\020\001', file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='pose_2d_data', full_name='RACS2Bridge_geometry_msgs.pose_2d_data', index=25, + number=26, type=2, cpp_type=6, label=3, + has_default_value=False, default_value=[], + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=b'\020\001', file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + serialized_options=None, + is_extendable=False, + syntax='proto2', + extension_ranges=[], + oneofs=[ + ], + serialized_start=247, + serialized_end=1120, +) + +_RACS2BRIDGE_GEOMETRY_MSGS_TWIST.fields_by_name['linear'].message_type = _RACS2BRIDGE_GEOMETRY_MSGS_VECTOR3 +_RACS2BRIDGE_GEOMETRY_MSGS_TWIST.fields_by_name['angular'].message_type = _RACS2BRIDGE_GEOMETRY_MSGS_VECTOR3 +DESCRIPTOR.message_types_by_name['RACS2Bridge_geometry_msgs_Vector3'] = _RACS2BRIDGE_GEOMETRY_MSGS_VECTOR3 +DESCRIPTOR.message_types_by_name['RACS2Bridge_geometry_msgs_Twist'] = _RACS2BRIDGE_GEOMETRY_MSGS_TWIST +DESCRIPTOR.message_types_by_name['RACS2Bridge_geometry_msgs'] = _RACS2BRIDGE_GEOMETRY_MSGS +_sym_db.RegisterFileDescriptor(DESCRIPTOR) + +RACS2Bridge_geometry_msgs_Vector3 = _reflection.GeneratedProtocolMessageType('RACS2Bridge_geometry_msgs_Vector3', (_message.Message,), { + 'DESCRIPTOR' : _RACS2BRIDGE_GEOMETRY_MSGS_VECTOR3, + '__module__' : 'RACS2Bridge_geometry_msgs_pb2' + # @@protoc_insertion_point(class_scope:RACS2Bridge_geometry_msgs_Vector3) + }) +_sym_db.RegisterMessage(RACS2Bridge_geometry_msgs_Vector3) + +RACS2Bridge_geometry_msgs_Twist = _reflection.GeneratedProtocolMessageType('RACS2Bridge_geometry_msgs_Twist', (_message.Message,), { + 'DESCRIPTOR' : _RACS2BRIDGE_GEOMETRY_MSGS_TWIST, + '__module__' : 'RACS2Bridge_geometry_msgs_pb2' + # @@protoc_insertion_point(class_scope:RACS2Bridge_geometry_msgs_Twist) + }) +_sym_db.RegisterMessage(RACS2Bridge_geometry_msgs_Twist) + +RACS2Bridge_geometry_msgs = _reflection.GeneratedProtocolMessageType('RACS2Bridge_geometry_msgs', (_message.Message,), { + 'DESCRIPTOR' : _RACS2BRIDGE_GEOMETRY_MSGS, + '__module__' : 'RACS2Bridge_geometry_msgs_pb2' + # @@protoc_insertion_point(class_scope:RACS2Bridge_geometry_msgs) + }) +_sym_db.RegisterMessage(RACS2Bridge_geometry_msgs) + + +_RACS2BRIDGE_GEOMETRY_MSGS.fields_by_name['accel_with_covariance_data']._options = None +_RACS2BRIDGE_GEOMETRY_MSGS.fields_by_name['accel_with_covariance_stamped_data']._options = None +_RACS2BRIDGE_GEOMETRY_MSGS.fields_by_name['point_stamped_data']._options = None +_RACS2BRIDGE_GEOMETRY_MSGS.fields_by_name['inertia_stamped_data']._options = None +_RACS2BRIDGE_GEOMETRY_MSGS.fields_by_name['quaternion_stamped_data']._options = None +_RACS2BRIDGE_GEOMETRY_MSGS.fields_by_name['transform_stamped_data']._options = None +_RACS2BRIDGE_GEOMETRY_MSGS.fields_by_name['inertia_data']._options = None +_RACS2BRIDGE_GEOMETRY_MSGS.fields_by_name['pose_with_covariance_data']._options = None +_RACS2BRIDGE_GEOMETRY_MSGS.fields_by_name['twist_covariance_stamped_data']._options = None +_RACS2BRIDGE_GEOMETRY_MSGS.fields_by_name['wrench_data']._options = None +_RACS2BRIDGE_GEOMETRY_MSGS.fields_by_name['accel_data']._options = None +_RACS2BRIDGE_GEOMETRY_MSGS.fields_by_name['pose_with_covariance_stamped_data']._options = None +_RACS2BRIDGE_GEOMETRY_MSGS.fields_by_name['accele_stamped_data']._options = None +_RACS2BRIDGE_GEOMETRY_MSGS.fields_by_name['transform_data']._options = None +_RACS2BRIDGE_GEOMETRY_MSGS.fields_by_name['point_data']._options = None +_RACS2BRIDGE_GEOMETRY_MSGS.fields_by_name['polygon_stamped_dta']._options = None +_RACS2BRIDGE_GEOMETRY_MSGS.fields_by_name['pose_array_data']._options = None +_RACS2BRIDGE_GEOMETRY_MSGS.fields_by_name['vector3_stamped_data']._options = None +_RACS2BRIDGE_GEOMETRY_MSGS.fields_by_name['pose_stamped_data']._options = None +_RACS2BRIDGE_GEOMETRY_MSGS.fields_by_name['polygon_data']._options = None +_RACS2BRIDGE_GEOMETRY_MSGS.fields_by_name['queaternion_data']._options = None +_RACS2BRIDGE_GEOMETRY_MSGS.fields_by_name['point32_data']._options = None +_RACS2BRIDGE_GEOMETRY_MSGS.fields_by_name['twist_with_covariance_data']._options = None +_RACS2BRIDGE_GEOMETRY_MSGS.fields_by_name['pose_data']._options = None +_RACS2BRIDGE_GEOMETRY_MSGS.fields_by_name['twist_stamped_data']._options = None +_RACS2BRIDGE_GEOMETRY_MSGS.fields_by_name['pose_2d_data']._options = None +# @@protoc_insertion_point(module_scope) diff --git a/racs2_demos_on_spaceros/nodes/move_arm b/racs2_demos_on_spaceros/nodes/move_arm new file mode 100644 index 0000000..3cd7b7d --- /dev/null +++ b/racs2_demos_on_spaceros/nodes/move_arm @@ -0,0 +1,68 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from builtin_interfaces.msg import Duration + +from std_msgs.msg import String, Float64 +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint +from std_srvs.srv import Empty + +class MoveArm(Node): + + def __init__(self): + super().__init__('arm_node') + self.arm_publisher_ = self.create_publisher(JointTrajectory, '/arm_joint_trajectory_controller/joint_trajectory', 10) + self.open_srv = self.create_service(Empty, 'open_arm', self.open_arm_callback) + self.close_srv = self.create_service(Empty, 'close_arm', self.close_arm_callback) + + + self.open = True + + + def open_arm_callback(self, request, response): + self.open = True + traj = JointTrajectory() + traj.joint_names = ["arm_01_joint", "arm_02_joint", "arm_03_joint", "arm_04_joint", "arm_tools_joint"] + + point1 = JointTrajectoryPoint() + point1.positions = [0.0,-0.5,3.0,1.0,0.0] + point1.time_from_start = Duration(sec=4) + + traj.points.append(point1) + self.arm_publisher_.publish(traj) + + + return response + + def close_arm_callback(self, request, response): + self.open = False + traj = JointTrajectory() + traj.joint_names = ["arm_01_joint", "arm_02_joint", "arm_03_joint", "arm_04_joint", "arm_tools_joint"] + + point1 = JointTrajectoryPoint() + point1.positions = [-1.57,-0.4,-1.1,-1.57,-1.57] + point1.time_from_start = Duration(sec=4) + + traj.points.append(point1) + self.arm_publisher_.publish(traj) + + return response + + + +def main(args=None): + rclpy.init(args=args) + + arm_node = MoveArm() + + rclpy.spin(arm_node) + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + arm_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/racs2_demos_on_spaceros/nodes/move_mast b/racs2_demos_on_spaceros/nodes/move_mast new file mode 100644 index 0000000..c29ab45 --- /dev/null +++ b/racs2_demos_on_spaceros/nodes/move_mast @@ -0,0 +1,84 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from builtin_interfaces.msg import Duration + +from std_msgs.msg import String, Float64 +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint +from std_srvs.srv import Empty + +class MastArm(Node): + + def __init__(self): + super().__init__('mast_node') + self.mast_publisher_ = self.create_publisher(JointTrajectory, '/mast_joint_trajectory_controller/joint_trajectory', 10) + self.mast_open_srv = self.create_service(Empty, 'mast_open', self.mast_open_callback) + self.mast_close_srv = self.create_service(Empty, 'mast_close', self.mast_close_callback) + self.mast_rotate_srv = self.create_service(Empty, 'mast_rotate', self.mast_rotate_callback) + + + def mast_open_callback(self, request, response): + traj = JointTrajectory() + traj.joint_names = ["mast_p_joint", "mast_02_joint", "mast_cameras_joint"] + + point = JointTrajectoryPoint() + point.positions = [0.0,-0.5,0.0] + point.time_from_start = Duration(sec=1) + + traj.points.append(point) + self.mast_publisher_.publish(traj) + return response + + def mast_close_callback(self, request, response): + traj = JointTrajectory() + traj.joint_names = ["mast_p_joint", "mast_02_joint", "mast_cameras_joint"] + + point = JointTrajectoryPoint() + point.positions = [1.57,-1.57,0.0] + point.time_from_start = Duration(sec=1) + + traj.points.append(point) + self.mast_publisher_.publish(traj) + return response + + def mast_rotate_callback(self, request, response): + traj = JointTrajectory() + traj.joint_names = ["mast_p_joint", "mast_02_joint", "mast_cameras_joint"] + + point1 = JointTrajectoryPoint() + point1.positions = [0.0,-1.57,0.0] + point1.time_from_start = Duration(sec=2) + traj.points.append(point1) + + point2 = JointTrajectoryPoint() + point2.positions = [0.0,-3.14,0.0] + point2.time_from_start = Duration(sec=4) + traj.points.append(point2) + + point3 = JointTrajectoryPoint() + point3.positions = [0.0,-6.28,0.0] + point3.time_from_start = Duration(sec=6) + traj.points.append(point3) + + self.mast_publisher_.publish(traj) + return response + + + +def main(args=None): + rclpy.init(args=args) + + mast_node = MastArm() + + rclpy.spin(mast_node) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + mast_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/racs2_demos_on_spaceros/nodes/move_wheel b/racs2_demos_on_spaceros/nodes/move_wheel new file mode 100644 index 0000000..8bf4b11 --- /dev/null +++ b/racs2_demos_on_spaceros/nodes/move_wheel @@ -0,0 +1,147 @@ +#!/usr/bin/env python3 + +from pickle import FALSE +import rclpy +from rclpy.node import Node +from builtin_interfaces.msg import Duration + +from std_msgs.msg import String, Float64MultiArray +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint +from geometry_msgs.msg import Twist +import math +from racs2_msg.msg import RACS2UserMsg +from RACS2Bridge_geometry_msgs_pb2 import RACS2Bridge_geometry_msgs_Twist + + +class MoveWheel(Node): + + def __init__(self): + super().__init__('wheel_node') + self.wheel_publisher_ = self.create_publisher(Float64MultiArray, '/wheel_velocity_controller/commands', 10) + self.steer_publisher_ = self.create_publisher(JointTrajectory, '/steer_position_controller/joint_trajectory', 10) + self.suspension_publisher_ = self.create_publisher(Float64MultiArray, '/wheel_tree_position_controller/commands',10) + timer_period = 0.1 # seconds + self.timer = self.create_timer(timer_period, self.timer_callback) + + + # self.vel_sub = self.create_subscription(Twist, '/cmd_vel', self.vel_callback, 10) + self.vel_sub = self.create_subscription(RACS2UserMsg, '/cmd_vel', self.vel_callback, 10) + self.curr_vel = Twist() + self.last_vel = Twist() + self.should_steer = False + + def vel_callback(self, msg): + self.get_logger().info('Subscribing: [/cmd_vel]') + message = RACS2Bridge_geometry_msgs_Twist() + message.ParseFromString(b''.join(msg.body_data)) + self.get_logger().info(f"message: {message}") + twist_msg = Twist() + twist_msg.linear.x = message.linear.x + twist_msg.linear.y = message.linear.y + twist_msg.linear.z = message.linear.z + twist_msg.angular.x = message.angular.x + twist_msg.angular.y = message.angular.y + twist_msg.angular.z = message.angular.z + if abs(self.last_vel.angular.z - self.curr_vel.angular.z) > 0.01 and self.should_steer is False: + self.last_vel = Twist() + self.last_vel.linear.x = self.curr_vel.linear.x + self.last_vel.angular.z = self.curr_vel.angular.z + self.should_steer = True + + # self.curr_vel = msg + self.curr_vel = twist_msg + self.get_logger().info(f"self.curr_vel: {self.curr_vel}") + + def set_wheel_common_speed(self, vel): + + target_vel = Float64MultiArray() + target_vel.data = [vel, vel*1.5, vel, -vel, -vel*1.5, -vel] + + # self.get_logger().info(f'Publishing: "{target_vel.data}"') + self.wheel_publisher_.publish(target_vel) + + + def map_angular_to_steering(self, angular_speed) -> float: + if abs(angular_speed) < 1e-3: + return 0.0 + + #max 0.6 min -0.6 + angular_speed = min(0.6, max(angular_speed, -0.6)) + return (angular_speed/abs(angular_speed))*(-25 * abs(angular_speed) + 17.5) + + + def set_steering(self, turn_rad): + #Ideal Ackerman Steering + target_steer = JointTrajectory() + # target_steer.header.stamp = self.get_clock().now().to_msg() + target_steer.joint_names = ["suspension_steer_F_L_joint", "suspension_steer_B_L_joint", + "suspension_steer_F_R_joint", "suspension_steer_B_R_joint"] + steer_point = JointTrajectoryPoint() + + if abs(turn_rad) < 1e-3: + steer_point.positions = [0.0, 0.0, 0.0, 0.0] + else: + R = abs(turn_rad) #Turning radius + + L = 2.08157 #Chassis Length + T = 1.53774 #Chassis Width + + alpha_i = math.atan(L/(R - (T/2))) + alpha_o = math.atan(L/(R + (T/2))) + + if alpha_i > 0.6: + alpha_i = 0.6 + + if alpha_o > 0.6: + alpha_o = 0.6 + + alpha_i = round(alpha_i, 2) + alpha_o = round(alpha_o, 2) + + if turn_rad > 0.0: + steer_point.positions = [alpha_i, -alpha_i, alpha_o, -alpha_o] + else: + steer_point.positions = [-alpha_o, alpha_o, -alpha_i, alpha_i] + + + steer_point.time_from_start = Duration(sec=1) + + target_steer.points.append(steer_point) + + self.steer_publisher_.publish(target_steer) + + + def set_suspension(self, sus_val=None): + + target_val = Float64MultiArray() + if sus_val is None: + target_val.data = [0.3,-0.1,0.3,-0.1] + self.suspension_publisher_.publish(target_val) + + + def timer_callback(self): + self.set_wheel_common_speed(self.curr_vel.linear.x) + self.set_suspension() + + if self.should_steer: + steering_val = self.map_angular_to_steering(self.curr_vel.angular.z) + self.set_steering(steering_val) + self.should_steer = False + + +def main(args=None): + rclpy.init(args=args) + + wheel_node = MoveWheel() + + rclpy.spin(wheel_node) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + wheel_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/racs2_demos_on_spaceros/nodes/odom_tf_publisher b/racs2_demos_on_spaceros/nodes/odom_tf_publisher new file mode 100644 index 0000000..ca5c95f --- /dev/null +++ b/racs2_demos_on_spaceros/nodes/odom_tf_publisher @@ -0,0 +1,66 @@ +#!/usr/bin/env python3 + +# adapted from ROS 2 tutorial +# https://docs.ros.org/en/rolling/Tutorials/Intermediate/Tf2/Writing-A-Tf2-Broadcaster-Py.html + +import rclpy + +from nav_msgs.msg import Odometry +from geometry_msgs.msg import TransformStamped +from rclpy.node import Node +from tf2_ros import TransformBroadcaster + +class OdomTFBroadcaster(Node): + + def __init__(self): + super().__init__('odom_tf_publisher') + + # Initialize the transform broadcaster + self.tf_broadcaster = TransformBroadcaster(self) + + # Subscribe to Odometry topic + # TODO: make the topic name a parameter instead of hard-coded + self.subscription = self.create_subscription( + Odometry, + f'/model/curiosity_mars_rover/odometry', + self.handle_odometry, + 1) + self.subscription # prevent unused variable warning + + def handle_odometry(self, msg): + tf = TransformStamped() + + # Read message content and assign it to + # corresponding tf variables + tf.header.stamp = msg.header.stamp + tf.header.frame_id = msg.header.frame_id + tf.child_frame_id = msg.child_frame_id + + # get translation coordinates from the message pose + tf.transform.translation.x = msg.pose.pose.position.x + tf.transform.translation.y = msg.pose.pose.position.y + tf.transform.translation.z = msg.pose.pose.position.z + + # get rotation from the message pose + tf.transform.rotation.x = msg.pose.pose.orientation.x + tf.transform.rotation.y = msg.pose.pose.orientation.y + tf.transform.rotation.z = msg.pose.pose.orientation.z + tf.transform.rotation.w = msg.pose.pose.orientation.w + + # Send the transformation + self.tf_broadcaster.sendTransform(tf) + + +def main(): + rclpy.init() + node = OdomTFBroadcaster() + node.get_logger().info('Starting odometry_tf_publisher node') + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/racs2_demos_on_spaceros/nodes/run_demo b/racs2_demos_on_spaceros/nodes/run_demo new file mode 100644 index 0000000..6df8d17 --- /dev/null +++ b/racs2_demos_on_spaceros/nodes/run_demo @@ -0,0 +1,86 @@ +#!/usr/bin/env python3 + + +import rclpy +from rclpy.node import Node + +from std_msgs.msg import String, Float64MultiArray +from geometry_msgs.msg import Twist +import math +from random import randint +from std_srvs.srv import Empty + + +# A node for performing combination of motions from different moving parts of the Curiosity rover. +class RunDemo(Node): + def __init__(self) -> None: + super().__init__('run_node') + self.motion_publisher_ = self.create_publisher(Twist, '/cmd_vel', 10) + self.forward_srv = self.create_service(Empty, 'move_forward', self.move_forward_callback) + self.stop_srv = self.create_service(Empty, 'move_stop', self.move_stop_callback) + self.left_srv = self.create_service(Empty, 'turn_left', self.turn_left_callback) + self.right_srv = self.create_service(Empty, 'turn_right', self.turn_right_callback) + self.stopped = True + + timer_period = 0.1 + self.timer = self.create_timer(timer_period, self.timer_callback) + + self.curr_action = Twist() + + def timer_callback(self): + if (not self.stopped): + self.motion_publisher_.publish(self.curr_action) + + def move_forward_callback(self, request, response): + self.get_logger().info("Moving forward") + action = Twist() + action.linear.x = 2.0 + self.curr_action = action + self.stopped = False + return response + + def move_stop_callback(self, request, response): + # stop timer from publishing + self.stopped = True + self.get_logger().info("Stopping") + self.curr_action = Twist() + # publish once to ensure we stop + self.motion_publisher_.publish(self.curr_action) + return response + + def turn_left_callback(self, request, response): + self.get_logger().info("Turning left") + action = Twist() + action.linear.x = 1.0 + action.angular.z = 0.4 + self.curr_action = action + self.stopped = False + return response + + def turn_right_callback(self, request, response): + self.get_logger().info("Turning right") + self.stopped = False + action = Twist() + action.linear.x = 1.0 + action.angular.z = -0.4 + self.curr_action = action + self.stopped = False + return response + + +def main(args=None): + rclpy.init(args=args) + + run_demo = RunDemo() + + rclpy.spin(run_demo) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + run_demo.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/racs2_demos_on_spaceros/nodes/test_node b/racs2_demos_on_spaceros/nodes/test_node new file mode 100644 index 0000000..8318215 --- /dev/null +++ b/racs2_demos_on_spaceros/nodes/test_node @@ -0,0 +1,41 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node + +from std_msgs.msg import String + + +class MinimalPublisher(Node): + + def __init__(self): + super().__init__('minimal_publisher') + self.publisher_ = self.create_publisher(String, 'topic', 10) + timer_period = 0.5 # seconds + self.timer = self.create_timer(timer_period, self.timer_callback) + self.i = 0 + + def timer_callback(self): + msg = String() + msg.data = 'Hello World: %d' % self.i + self.publisher_.publish(msg) + self.get_logger().info('Publishing: "%s"' % msg.data) + self.i += 1 + + +def main(args=None): + rclpy.init(args=args) + + minimal_publisher = MinimalPublisher() + + rclpy.spin(minimal_publisher) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + minimal_publisher.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/racs2_demos_on_spaceros/package.xml b/racs2_demos_on_spaceros/package.xml new file mode 100644 index 0000000..f526fb0 --- /dev/null +++ b/racs2_demos_on_spaceros/package.xml @@ -0,0 +1,42 @@ + + + + mars_rover + 0.0.1 + A RACS2 Bridge demo for Mars rover + jaxa + Apache-2.0 + + ament_cmake + ament_cmake_python + + rclcpp + rclpy + simulation + + ament_index_python + control_msgs + diff_drive_controller + effort_controllers + geometry_msgs + hardware_interface + ign_ros2_control + imu_sensor_broadcaster + joint_state_broadcaster + joint_trajectory_controller + launch + launch_ros + robot_state_publisher + ros_ign_gazebo + ros2controlcli + std_msgs + velocity_controllers + xacro + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/racs2_demos_on_spaceros/racs2_demo_on_spaceros_nodes.png b/racs2_demos_on_spaceros/racs2_demo_on_spaceros_nodes.png new file mode 100644 index 0000000..f0bf063 Binary files /dev/null and b/racs2_demos_on_spaceros/racs2_demo_on_spaceros_nodes.png differ diff --git a/racs2_demos_on_spaceros/racs2_demos_on_spaceros/__init__.py b/racs2_demos_on_spaceros/racs2_demos_on_spaceros/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/racs2_demos_on_spaceros/run.sh b/racs2_demos_on_spaceros/run.sh new file mode 100755 index 0000000..5b36331 --- /dev/null +++ b/racs2_demos_on_spaceros/run.sh @@ -0,0 +1,16 @@ +#!/usr/bin/env bash + +# Runs a docker container with the image created by build.bash +# Requires: +# docker +# an X server + +IMG_NAME=jaxa/racs2_demos_on_spaceros + +# Replace `/` with `_` to comply with docker container naming +# And append `_runtime` +CONTAINER_NAME="$(tr '/' '_' <<< "$IMG_NAME")" + +# Start the container +docker run --rm -it --name $CONTAINER_NAME --network host \ + -e DISPLAY -e TERM -e QT_X11_NO_MITSHM=1 $IMG_NAME diff --git a/racs2_demos_on_spaceros/worlds/mars_curiosity.world b/racs2_demos_on_spaceros/worlds/mars_curiosity.world new file mode 100644 index 0000000..a5c036f --- /dev/null +++ b/racs2_demos_on_spaceros/worlds/mars_curiosity.world @@ -0,0 +1,64 @@ + + + + + + + 3D View + false + docked + + ogre2 + scene + 0.9 0.753 0.66 + -5.0 0.0 -6.0 0.0 0.0 0.0 + + + + + floating + + + + + + + + + + ogre2 + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + 0 0 -3.711 + + + model://curiosity_path + curiosity_path + 0 0 0 0 0 0 + + + +