diff --git a/.travis.yml b/.travis.yml deleted file mode 100644 index 4a3a410..0000000 --- a/.travis.yml +++ /dev/null @@ -1,25 +0,0 @@ -# This config file for Travis CI utilizes ros-industrial/industrial_ci package. -# For more info for the package, see https://github.com/ros-industrial/industrial_ci/blob/master/README.rst -sudo: required -dist: trusty -services: - - docker -language: generic -compiler: - - gcc -notifications: - email: false - -env: - global: - - CATKIN_PARALLEL_TEST_JOBS=-p1 - - ROS_PARALLEL_TEST_JOBS=-j1 - matrix: - - ROS_DISTRO="indigo" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu - - ROS_DISTRO="kinetic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu - - ROS_DISTRO="melodic" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu -install: - - wget -qO- -O EPOS_Library.zip http://academy.maxonjapan.co.jp/wp-content/uploads/manual/epos/EPOS_Linux_Library_E.zip && unzip EPOS_Library.zip && cd EPOS_Linux_Library && sudo bash ./install.sh - - git clone https://github.com/ros-industrial/industrial_ci.git .ci_config -script: - - source .ci_config/travis.sh diff --git a/README.md b/README.md index 6efff63..c2e4cd0 100644 --- a/README.md +++ b/README.md @@ -1,25 +1,30 @@ # Maxon EPOS Package -This is a ROS package for Maxon motor & EPOS driver. +This is a ROS/ROS2 package for Maxon motor & EPOS driver. ## Support Status This package is **WIP** now.
v1.0.0 supports multiple EPOS device.
## How to use +> **WARNING** --- The package maxon_epos_example **has NOT** been migrated to ROS2.
+ +In the package maxon_epos_driver you can find an example of launch file and configuration file for runing one motor.
+ +> **VERY IMPORTANT** - Do not set the name of the node when using a launch file to execute the maxon_bringup node. This will rename all rclcpp::Node variables that it node generates and it will be unable to get the motor connection parameters correctly. ### Install Maxon Linux driver -Firstly you must install maxon official linux driver.
-For detail, please look at [this official pdf](http://academy.maxonjapan.co.jp/wp-content/uploads/manual/epos/EPOS_Command_Library.pdf) +Firstly you must install maxon official linux driver. [Download EPOS Linux Library here](https://www.maxongroup.com/medias/sys_master/root/8994700394526/EPOS-Linux-Library-En.zip)
+For detail, please look at [this official pdf](https://www.maxongroup.com/medias/sys_master/8823917281310.pdf) ### How to build Then you have to download this package into your ROS workspace. ```bash -$ cd ~/catkin_ws/src -$ git clone https://github.com/iwata-lab/maxon_epos_ros.git -$ cd ~/catkin_ws -$ catkin_make -$ source ~/catkin_ws/devel/setup.bash +$ cd ~/yourworkspace_ws/src +$ git clone https://github.com/cristinaluna/maxon_epos_ros.git +$ cd ~/yourworkspace_ws +$ colcon build +$ source ~/yourworkspace_ws/install/setup.bash ``` ### How to use nodes @@ -27,26 +32,26 @@ $ source ~/catkin_ws/devel/setup.bash You can run nodes by console.
`maxon_bringup` node reads params from ROS parameter server. ```bash -$ rosrun maxon_epos_driver maxon_bringup +$ ros2 run maxon_epos_driver maxon_bringup ``` Otherwise, you can refer `maxon_epos_example` package's launch file. #### Topics -- `~//get_state` (msg: [maxon_epos_msgs/MotorState](maxon_epos_msgs/msg/MotorState.msg)) +- `~//get_state` (msg: [maxon_epos_msgs/msg/MotorState](maxon_epos_msgs/msg/MotorState.msg)) This topic keep publishing motor current state. -- `~//set_state` (msg: [maxon_epos_msgs/MotorState](maxon_epos_msgs/msg/MotorState.msg)) +- `~//set_state` (msg: [maxon_epos_msgs/msg/MotorState](maxon_epos_msgs/msg/MotorState.msg)) When you send the goal to epos, use this topic. -- `~/get_all_state` (msg: [maxon_epos_msgs/MotorStates](maxon_epos_msgs/msg/MotorStates.msg)) +- `~/get_all_state` (msg: [maxon_epos_msgs/msg/MotorStates](maxon_epos_msgs/msg/MotorStates.msg)) This topic keep publishing all motors current states. -- `~/set_all_state` (msg: [maxon_epos_msgs/MotorStates](maxon_epos_msgs/msg/MotorStates.msg)) +- `~/set_all_state` (msg: [maxon_epos_msgs/msg/MotorStates](maxon_epos_msgs/msg/MotorStates.msg)) When you send the goals to all epos motors, use this topic. diff --git a/maxon_epos_driver/CMakeLists.txt b/maxon_epos_driver/CMakeLists.txt index 4a55490..5510ae5 100644 --- a/maxon_epos_driver/CMakeLists.txt +++ b/maxon_epos_driver/CMakeLists.txt @@ -1,127 +1,37 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(maxon_epos_driver) -## Compile as C++11, supported in ROS Kinetic and newer -add_compile_options(-std=c++11) +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - roscpp - std_msgs - maxon_epos_msgs -) -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - INCLUDE_DIRS include - LIBRARIES maxon_epos_driver - CATKIN_DEPENDS message_runtime roscpp std_msgs - # DEPENDS system_lib +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(std_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(maxon_epos_msgs REQUIRED) + +set(DEPENDENCIES + "geometry_msgs" + "rclcpp" + "std_msgs" + "maxon_epos_msgs" ) -########### -## Build ## -########### -## Specify additional locations of header files -## Your package locations should be listed before other locations include_directories( include - ${catkin_INCLUDE_DIRS} ) ## Declare a C++ library -add_library(${PROJECT_NAME} +add_library(${PROJECT_NAME} SHARED src/lib/EposMotor.cpp src/lib/EposManager.cpp src/lib/Device.cpp @@ -131,81 +41,42 @@ add_library(${PROJECT_NAME} src/lib/control/EposProfileVelocityMode.cpp src/lib/control/EposCurrentMode.cpp ) +ament_target_dependencies(maxon_epos_driver + rclcpp_components + rclcpp + geometry_msgs + std_msgs + maxon_epos_msgs +) +target_link_libraries(maxon_epos_driver -lEposCmd) +ament_export_libraries(maxon_epos_driver) -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide add_executable(maxon_bringup src/node/maxon_bringup.cpp) +ament_target_dependencies(maxon_bringup + rclcpp_components + rclcpp + geometry_msgs + std_msgs + maxon_epos_msgs +) +target_link_libraries(maxon_bringup maxon_epos_driver -lEposCmd) -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") -## Add cmake target dependencies of the executable -## same as for the library above -add_dependencies(maxon_bringup ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +install(DIRECTORY include/ + DESTINATION include) +ament_export_include_directories(include) -## Specify libraries to link a library or executable target against -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} - EposCmd -) -target_link_libraries(maxon_bringup - ${PROJECT_NAME} - ${catkin_LIBRARIES} -) +install(TARGETS ${PROJECT_NAME} + DESTINATION lib) + +install(TARGETS maxon_bringup + DESTINATION lib/${PROJECT_NAME}) + +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME}/) + +install(DIRECTORY config + DESTINATION share/${PROJECT_NAME}/) + +ament_package() -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables and/or libraries for installation -# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_maxon_epos_sample.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/maxon_epos_driver/config/test_1motor.yaml b/maxon_epos_driver/config/test_1motor.yaml new file mode 100644 index 0000000..09ee731 --- /dev/null +++ b/maxon_epos_driver/config/test_1motor.yaml @@ -0,0 +1,39 @@ +maxon_bringup: + ros__parameters: + motor_names: ["motor1"] + +motor1: + ros__parameters: + # epos's node information (must be enough to identify the node) + device: 'EPOS4' # default: 'EPOS4' + protocol_stack: 'CANopen' # default: 'MAXON SERIAL V2' + interface: 'USB' # default: 'USB' + port: 'USB0' # default: 'USB0' + node_id: 1 # default: 0 (invalid) + + # communication settings (optional) + # 'device' is a set of types of device, protocol_stack, interface, and port. + # order of node initialization is that of commandline arguments. + baudrate: 1000000 # default: 0 (keep current baudrate) + timeout: 500 # [ms], default: 0 (keep current timeout) + + # general parameters (optional) + use_ros_unit: true # use ros standard units (rad, rad/s, Nm) in hardware interfaces + # or epos standard units (quad count of encoder pulse(qc), rpm, mNm) + # (default: false) + + # epos's operation mode (required) + control_mode: "profile_position" + + # encoder parameters (required) + encoder: + type: 1 # 1: INC 3CH, 2: INC 2CH, 4: SSI ABS BIN, 5: SSI ABS GRY + resolution: 1024 # encoder resolution + gear_ratio: 243 # gear ratio of motor + inverted_polarity: false + + # profile position mode configs (optional) + position_profile: + velocity: 10000 # [rpm] + acceleration: 8000 # [rpm/s] + deceleration: 9000 # [rpm/s] \ No newline at end of file diff --git a/maxon_epos_driver/include/maxon_epos_driver/Definitions.h b/maxon_epos_driver/include/maxon_epos_driver/Definitions.h index 7d92e85..bef8e23 100644 --- a/maxon_epos_driver/include/maxon_epos_driver/Definitions.h +++ b/maxon_epos_driver/include/maxon_epos_driver/Definitions.h @@ -20,7 +20,11 @@ int CreateCommunication(); int DeleteCommunication(); -// INITIALISATION FUNCTIONS +// Data Recorder + int CreateDataRecorderCmdManager(void* KeyHandle); + int DeleteDataRecorderCmdManager(); + + // INITIALISATION FUNCTIONS #define Initialisation_DllExport extern "C" #define HelpFunctions_DllExport extern "C" // CONFIGURATION FUNCTIONS @@ -40,6 +44,8 @@ #define MasterEncoderMode_DllExport extern "C" #define StepDirectionMode_DllExport extern "C" #define InputsOutputs_DllExport extern "C" +// DATA RECORDING FUNCTIONS + #define DataRecording_DllExport extern "C" // LOW LAYER FUNCTIONS #define CanLayer_DllExport extern "C" @@ -95,10 +101,14 @@ //Motor Configuration_DllExport int VCS_SetMotorType(void* KeyHandle, unsigned short NodeId, unsigned short MotorType, unsigned int* pErrorCode); Configuration_DllExport int VCS_SetDcMotorParameter(void* KeyHandle, unsigned short NodeId, unsigned short NominalCurrent, unsigned short MaxOutputCurrent, unsigned short ThermalTimeConstant, unsigned int* pErrorCode); + Configuration_DllExport int VCS_SetDcMotorParameterEx(void* KeyHandle, unsigned short NodeId, unsigned int NominalCurrent, unsigned int MaxOutputCurrent, unsigned short ThermalTimeConstant, unsigned int* pErrorCode); Configuration_DllExport int VCS_SetEcMotorParameter(void* KeyHandle, unsigned short NodeId, unsigned short NominalCurrent, unsigned short MaxOutputCurrent, unsigned short ThermalTimeConstant, unsigned char NbOfPolePairs, unsigned int* pErrorCode); + Configuration_DllExport int VCS_SetEcMotorParameterEx(void* KeyHandle, unsigned short NodeId, unsigned int NominalCurrent, unsigned int MaxOutputCurrent, unsigned short ThermalTimeConstant, unsigned char NbOfPolePairs, unsigned int* pErrorCode); Configuration_DllExport int VCS_GetMotorType(void* KeyHandle, unsigned short NodeId, unsigned short* pMotorType, unsigned int* pErrorCode); Configuration_DllExport int VCS_GetDcMotorParameter(void* KeyHandle, unsigned short NodeId, unsigned short* pNominalCurrent, unsigned short* pMaxOutputCurrent, unsigned short* pThermalTimeConstant, unsigned int* pErrorCode); + Configuration_DllExport int VCS_GetDcMotorParameterEx(void* KeyHandle, unsigned short NodeId, unsigned int* pNominalCurrent, unsigned int* pMaxOutputCurrent, unsigned short* pThermalTimeConstant, unsigned int* pErrorCode); Configuration_DllExport int VCS_GetEcMotorParameter(void* KeyHandle, unsigned short NodeId, unsigned short* pNominalCurrent, unsigned short* pMaxOutputCurrent, unsigned short* pThermalTimeConstant, unsigned char* pNbOfPolePairs, unsigned int* pErrorCode); + Configuration_DllExport int VCS_GetEcMotorParameterEx(void* KeyHandle, unsigned short NodeId, unsigned int* pNominalCurrent, unsigned int* pMaxOutputCurrent, unsigned short* pThermalTimeConstant, unsigned char* pNbOfPolePairs, unsigned int* pErrorCode); //Sensor Configuration_DllExport int VCS_SetSensorType(void* KeyHandle, unsigned short NodeId, unsigned short SensorType, unsigned int* pErrorCode); @@ -106,11 +116,13 @@ Configuration_DllExport int VCS_SetHallSensorParameter(void* KeyHandle, unsigned short NodeId, int InvertedPolarity, unsigned int* pErrorCode); Configuration_DllExport int VCS_SetSsiAbsEncoderParameter(void* KeyHandle, unsigned short NodeId, unsigned short DataRate, unsigned short NbOfMultiTurnDataBits, unsigned short NbOfSingleTurnDataBits, int InvertedPolarity, unsigned int* pErrorCode); Configuration_DllExport int VCS_SetSsiAbsEncoderParameterEx(void* KeyHandle, unsigned short NodeId, unsigned short DataRate, unsigned short NbOfMultiTurnDataBits, unsigned short NbOfSingleTurnDataBits, unsigned short NbOfSpecialDataBits, int InvertedPolarity, unsigned short Timeout, unsigned short PowerupTime, unsigned int* pErrorCode); + Configuration_DllExport int VCS_SetSsiAbsEncoderParameterEx2(void* KeyHandle, unsigned short NodeId, unsigned short DataRate, unsigned short NbOfSpecialDataBitsLeading, unsigned short NbOfMultiTurnDataBits, unsigned short NbOfMultiTurnPositionBits, unsigned short NbOfSingleTurnDataBits, unsigned short NbOfSingleTurnPositionBits, unsigned short NbOfSpecialDataBitsTrailing, int InvertedPolarity, unsigned short Timeout, unsigned short PowerupTime, int CheckFrame, int ReferenceReset, unsigned int* pErrorCode); Configuration_DllExport int VCS_GetSensorType(void* KeyHandle, unsigned short NodeId, unsigned short* pSensorType, unsigned int* pErrorCode); Configuration_DllExport int VCS_GetIncEncoderParameter(void* KeyHandle, unsigned short NodeId, unsigned int* pEncoderResolution, int* pInvertedPolarity, unsigned int* pErrorCode); Configuration_DllExport int VCS_GetHallSensorParameter(void* KeyHandle, unsigned short NodeId, int* pInvertedPolarity, unsigned int* pErrorCode); Configuration_DllExport int VCS_GetSsiAbsEncoderParameter(void* KeyHandle, unsigned short NodeId, unsigned short* pDataRate, unsigned short* pNbOfMultiTurnDataBits, unsigned short* pNbOfSingleTurnDataBits, int* pInvertedPolarity, unsigned int* pErrorCode); Configuration_DllExport int VCS_GetSsiAbsEncoderParameterEx(void* KeyHandle, unsigned short NodeId, unsigned short* pDataRate, unsigned short* pNbOfMultiTurnDataBits, unsigned short* pNbOfSingleTurnDataBits, unsigned short* pNbOfSpecialDataBits, int* pInvertedPolarity, unsigned short* pTimeout, unsigned short* pPowerupTime, unsigned int* pErrorCode); + Configuration_DllExport int VCS_GetSsiAbsEncoderParameterEx2(void* KeyHandle, unsigned short NodeId, unsigned short* pDataRate, unsigned short* pNbOfSpecialDataBitsLeading, unsigned short* pNbOfMultiTurnDataBits, unsigned short* pNbOfMultiTurnPositionBits, unsigned short* pNbOfSingleTurnDataBits, unsigned short* pNbOfSingleTurnPositionBits, unsigned short* pNbOfSpecialDataBitsTrailing, int* pInvertedPolarity, unsigned short* pTimeout, unsigned short* pPowerupTime, int* pCheckFrame, int* pReferenceReset, unsigned int* pErrorCode); //Safety Configuration_DllExport int VCS_SetMaxFollowingError(void* KeyHandle, unsigned short NodeId, unsigned int MaxFollowingError, unsigned int* pErrorCode); @@ -179,7 +191,9 @@ MotionInfo_DllExport int VCS_GetVelocityIs(void* KeyHandle, unsigned short NodeId, int* pVelocityIs, unsigned int* pErrorCode); MotionInfo_DllExport int VCS_GetVelocityIsAveraged(void* KeyHandle, unsigned short NodeId, int* pVelocityIsAveraged, unsigned int* pErrorCode); MotionInfo_DllExport int VCS_GetCurrentIs(void* KeyHandle, unsigned short NodeId, short* pCurrentIs, unsigned int* pErrorCode); + MotionInfo_DllExport int VCS_GetCurrentIsEx(void* KeyHandle, unsigned short NodeId, int* pCurrentIs, unsigned int* pErrorCode); MotionInfo_DllExport int VCS_GetCurrentIsAveraged(void* KeyHandle, unsigned short NodeId, short* pCurrentIsAveraged, unsigned int* pErrorCode); + MotionInfo_DllExport int VCS_GetCurrentIsAveragedEx(void* KeyHandle, unsigned short NodeId, int* pCurrentIsAveraged, unsigned int* pErrorCode); MotionInfo_DllExport int VCS_WaitForTargetReached(void* KeyHandle, unsigned short NodeId, unsigned int Timeout, unsigned int* pErrorCode); //Profile Position Mode @@ -252,7 +266,9 @@ //Current Mode CurrentMode_DllExport int VCS_ActivateCurrentMode(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); CurrentMode_DllExport int VCS_SetCurrentMust(void* KeyHandle, unsigned short NodeId, short CurrentMust, unsigned int* pErrorCode); + CurrentMode_DllExport int VCS_SetCurrentMustEx(void* KeyHandle, unsigned short NodeId, int CurrentMust, unsigned int* pErrorCode); CurrentMode_DllExport int VCS_GetCurrentMust(void* KeyHandle, unsigned short NodeId, short* pCurrentMust, unsigned int* pErrorCode); + CurrentMode_DllExport int VCS_GetCurrentMustEx(void* KeyHandle, unsigned short NodeId, int* pCurrentMust, unsigned int* pErrorCode); //Advanced Functions CurrentMode_DllExport int VCS_ActivateAnalogCurrentSetpoint(void* KeyHandle, unsigned short NodeId, unsigned short AnalogInputNumber, float Scaling, short Offset, unsigned int* pErrorCode); @@ -280,7 +296,7 @@ InputsOutputs_DllExport int VCS_GetAnalogInputState(void* KeyHandle, unsigned short NodeId, unsigned short Configuration, long* pStateValue, unsigned int* pErrorCode); InputsOutputs_DllExport int VCS_SetAnalogOutput(void* KeyHandle, unsigned short NodeId, unsigned short OutputNumber, unsigned short AnalogValue, unsigned int* pErrorCode); InputsOutputs_DllExport int VCS_SetAnalogOutputVoltage(void* KeyHandle, unsigned short NodeId, unsigned short OutputNumber, long VoltageValue, unsigned int* pErrorCode); - InputsOutputs_DllExport int VCS_SetAnalogOutputState(void* KeyHandle, unsigned short NodeId, unsigned short Configuration, long StateValue, unsigned int* pErrorCode); + InputsOutputs_DllExport int VCS_SetAnalogOutputState(void* KeyHandle, unsigned short NodeId, unsigned short Configuration, long StateValue, unsigned int* pErrorCode); //Position Compare InputsOutputs_DllExport int VCS_SetPositionCompareParameter(void* KeyHandle, unsigned short NodeId, unsigned char OperationalMode, unsigned char IntervalMode, unsigned char DirectionDependency, unsigned short IntervalWidth, unsigned short IntervalRepetitions, unsigned short PulseWidth, unsigned int* pErrorCode); @@ -300,6 +316,33 @@ InputsOutputs_DllExport int VCS_ReadPositionMarkerCapturedPosition(void* KeyHandle, unsigned short NodeId, unsigned short CounterIndex, long* pCapturedPosition, unsigned int* pErrorCode); InputsOutputs_DllExport int VCS_ResetPositionMarkerCounter(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); +//******************************************************************************************************************* +// DATA RECORDING FUNCTIONS +//******************************************************************************************************************* + + //DataRecorder Setup + DataRecording_DllExport int VCS_SetRecorderParameter(void* KeyHandle, unsigned short NodeId, unsigned short SamplingPeriod, unsigned short NbOfPrecedingSamples, unsigned int* pErrorCode); + DataRecording_DllExport int VCS_GetRecorderParameter(void* KeyHandle, unsigned short NodeId, unsigned short* pSamplingPeriod, unsigned short* pNbOfPrecedingSamples, unsigned int* pErrorCode); + DataRecording_DllExport int VCS_EnableTrigger(void* KeyHandle, unsigned short NodeId, unsigned char TriggerType, unsigned int* pErrorCode); + DataRecording_DllExport int VCS_DisableAllTriggers(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); + DataRecording_DllExport int VCS_ActivateChannel(void* KeyHandle, unsigned short NodeId, unsigned char ChannelNumber, unsigned short ObjectIndex, unsigned char ObjectSubIndex, unsigned char ObjectSize, unsigned int* pErrorCode); + DataRecording_DllExport int VCS_DeactivateAllChannels(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); + + //DataRecorder Status + DataRecording_DllExport int VCS_StartRecorder(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); + DataRecording_DllExport int VCS_StopRecorder(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); + DataRecording_DllExport int VCS_ForceTrigger(void* KeyHandle, unsigned short NodeId, unsigned int* pErrorCode); + DataRecording_DllExport int VCS_IsRecorderRunning(void* KeyHandle, unsigned short NodeId, int* pRunning, unsigned int* pErrorCode); + DataRecording_DllExport int VCS_IsRecorderTriggered(void* KeyHandle, unsigned short NodeId, int* pTriggered, unsigned int* pErrorCode); + + //DataRecorder Data + DataRecording_DllExport int VCS_ReadChannelVectorSize(void* KeyHandle, unsigned short NodeId, unsigned int* pVectorSize, unsigned int* pErrorCode); + DataRecording_DllExport int VCS_ReadChannelDataVector(void* KeyHandle, unsigned short NodeId, unsigned char ChannelNumber, unsigned char* pDataVectorBuffer, unsigned int VectorBufferSize, unsigned int* pErrorCode); + + //Advanced Functions + DataRecording_DllExport int VCS_ReadDataBuffer(void* KeyHandle, unsigned short NodeId, unsigned char* pDataBuffer, unsigned int BufferSizeToRead, unsigned int* pBufferSizeRead, unsigned short* pVectorStartOffset, unsigned short* pMaxNbOfSamples, unsigned short* pNbOfRecordedSamples, unsigned int* pErrorCode); + DataRecording_DllExport int VCS_ExtractChannelDataVector(void* KeyHandle, unsigned short NodeId, unsigned char ChannelNumber, unsigned char* pDataBuffer, unsigned int BufferSize, unsigned char* pDataVector, unsigned int VectorSize, unsigned short VectorStartOffset, unsigned short MaxNbOfSamples, unsigned short NbOfRecordedSamples, unsigned int* pErrorCode); + /************************************************************************************************************************************* * LOW LAYER FUNCTIONS *************************************************************************************************************************************/ @@ -523,3 +566,4 @@ const unsigned short EG_DLPC_MAIN_LOOP_FILTER_COEFFICIENT_E = 124; #endif //_H_LINUX_EPOSCMD_ + diff --git a/maxon_epos_driver/include/maxon_epos_driver/Device.hpp b/maxon_epos_driver/include/maxon_epos_driver/Device.hpp index 181db37..cc61bed 100644 --- a/maxon_epos_driver/include/maxon_epos_driver/Device.hpp +++ b/maxon_epos_driver/include/maxon_epos_driver/Device.hpp @@ -12,7 +12,7 @@ #include #include #include -#include "maxon_epos_driver/utils/Macros.hpp" +#include "utils/Macros.hpp" /** * @brief Information of device @@ -90,7 +90,7 @@ class NodeHandle : public DeviceHandle { */ struct CompareNodeInfo { - bool operator()(const NodeInfo &a, const NodeInfo &b) + bool operator()(const NodeInfo &a, const NodeInfo &b) const { if (a.device_name != b.device_name) { return a.device_name < b.device_name; diff --git a/maxon_epos_driver/include/maxon_epos_driver/EposManager.hpp b/maxon_epos_driver/include/maxon_epos_driver/EposManager.hpp index 8123335..b65b6b1 100644 --- a/maxon_epos_driver/include/maxon_epos_driver/EposManager.hpp +++ b/maxon_epos_driver/include/maxon_epos_driver/EposManager.hpp @@ -10,9 +10,9 @@ #include #include -#include +#include #include "maxon_epos_driver/EposMotor.hpp" -#include "maxon_epos_msgs/MotorStates.h" +#include "maxon_epos_msgs/msg/motor_states.hpp" class EposManager { @@ -20,17 +20,17 @@ class EposManager { EposManager(); virtual ~EposManager(); - bool init(ros::NodeHandle &root_nh, ros::NodeHandle &motors_nh, + bool init(rclcpp::Node &root_nh, rclcpp::Node &motors_nh, const std::vector &motor_names); void read(); - void write(const maxon_epos_msgs::MotorStates::ConstPtr& msg); + void write(const maxon_epos_msgs::msg::MotorStates::SharedPtr msg); private: std::vector> m_motors; - ros::Publisher m_all_motor_publisher; - ros::Subscriber m_all_motor_subscriber; + rclcpp::Publisher::SharedPtr m_all_motor_publisher; + rclcpp::Subscription::SharedPtr m_all_motor_subscriber; }; #endif // _EposManager_HPP diff --git a/maxon_epos_driver/include/maxon_epos_driver/EposMotor.hpp b/maxon_epos_driver/include/maxon_epos_driver/EposMotor.hpp index 3625923..b13111f 100644 --- a/maxon_epos_driver/include/maxon_epos_driver/EposMotor.hpp +++ b/maxon_epos_driver/include/maxon_epos_driver/EposMotor.hpp @@ -9,11 +9,11 @@ #define _EposMotor_HPP #include -#include -#include +#include +#include "std_msgs/msg/float32.hpp" #include "maxon_epos_driver/Device.hpp" #include "maxon_epos_driver/control/ControlModeBase.hpp" -#include "maxon_epos_msgs/MotorState.h" +#include "maxon_epos_msgs/msg/motor_state.hpp" class EposMotor { @@ -21,26 +21,26 @@ class EposMotor { EposMotor(); virtual ~EposMotor(); - void init(ros::NodeHandle &root_nh, ros::NodeHandle &motor_nh, + void init(rclcpp::Node &root_nh, rclcpp::Node &motor_nh, const std::string &motor_name); - maxon_epos_msgs::MotorState read(); + maxon_epos_msgs::msg::MotorState read(); void write(const double position, const double velocity, const double current); private: - void initEposDeviceHandle(ros::NodeHandle &motor_nh); + void initEposDeviceHandle(rclcpp::Node &motor_nh); void initDeviceError(); - void initProtocolStackChanges(ros::NodeHandle &motor_nh); - void initControlMode(ros::NodeHandle &root_nh, ros::NodeHandle &motor_nh); - void initEncoderParams(ros::NodeHandle &motor_nh); - void initProfilePosition(ros::NodeHandle &motor_nh); - void initMiscParams(ros::NodeHandle &motor_nh); + void initProtocolStackChanges(rclcpp::Node &motor_nh); + void initControlMode(rclcpp::Node &root_nh, rclcpp::Node &motor_nh); + void initEncoderParams(rclcpp::Node &motor_nh); + void initProfilePosition(rclcpp::Node &motor_nh); + void initMiscParams(rclcpp::Node &motor_nh); double ReadPosition(); double ReadVelocity(); double ReadCurrent(); - void writeCallback(const maxon_epos_msgs::MotorState::ConstPtr &msg); + void writeCallback(const maxon_epos_msgs::msg::MotorState::SharedPtr msg); private: @@ -58,8 +58,8 @@ class EposMotor { double m_effort; double m_current; - ros::Publisher m_state_publisher; - ros::Subscriber m_state_subscriber; + rclcpp::Publisher::SharedPtr m_state_publisher; + rclcpp::Subscription::SharedPtr m_state_subscriber; int m_max_qc; bool m_use_ros_unit; diff --git a/maxon_epos_driver/include/maxon_epos_driver/control/ControlModeBase.hpp b/maxon_epos_driver/include/maxon_epos_driver/control/ControlModeBase.hpp index 96ce49b..bf8fff9 100644 --- a/maxon_epos_driver/include/maxon_epos_driver/control/ControlModeBase.hpp +++ b/maxon_epos_driver/include/maxon_epos_driver/control/ControlModeBase.hpp @@ -10,14 +10,14 @@ #include #include -#include +#include #include "maxon_epos_driver/Device.hpp" class ControlModeBase { public: virtual ~ControlModeBase(); - virtual void init(ros::NodeHandle &motor_nh, NodeHandle &node_handle); + virtual void init(rclcpp::Node &motor_nh, NodeHandle &node_handle); // activate operation mode virtual void activate() = 0; @@ -28,6 +28,7 @@ class ControlModeBase { // write commands of operation mode virtual void write(const double position, const double velocity, const double current) = 0; + rclcpp::Node::SharedPtr nh; protected: bool m_use_ros_unit; NodeHandle m_epos_handle; diff --git a/maxon_epos_driver/include/maxon_epos_driver/control/EposCurrentMode.hpp b/maxon_epos_driver/include/maxon_epos_driver/control/EposCurrentMode.hpp index d69b62f..ca9b05b 100644 --- a/maxon_epos_driver/include/maxon_epos_driver/control/EposCurrentMode.hpp +++ b/maxon_epos_driver/include/maxon_epos_driver/control/EposCurrentMode.hpp @@ -10,14 +10,14 @@ #include "maxon_epos_driver/control/ControlModeBase.hpp" #include "maxon_epos_driver/Device.hpp" -#include +#include class EposCurrentMode : public ControlModeBase { public: virtual ~EposCurrentMode(); - virtual void init(ros::NodeHandle &motor_nh, NodeHandle &node_handle); + virtual void init(rclcpp::Node &motor_nh, NodeHandle &node_handle); virtual void activate(); virtual void read(); virtual void write(const double position, const double velocity, const double current); diff --git a/maxon_epos_driver/include/maxon_epos_driver/control/EposProfilePositionMode.hpp b/maxon_epos_driver/include/maxon_epos_driver/control/EposProfilePositionMode.hpp index 51a2b9e..fd2f459 100644 --- a/maxon_epos_driver/include/maxon_epos_driver/control/EposProfilePositionMode.hpp +++ b/maxon_epos_driver/include/maxon_epos_driver/control/EposProfilePositionMode.hpp @@ -10,14 +10,14 @@ #include "maxon_epos_driver/control/ControlModeBase.hpp" #include "maxon_epos_driver/Device.hpp" -#include +#include class EposProfilePositionMode : public ControlModeBase { public: virtual ~EposProfilePositionMode(); - virtual void init(ros::NodeHandle &motor_nh, NodeHandle &node_handle); + virtual void init(rclcpp::Node &motor_nh, NodeHandle &node_handle); virtual void activate(); virtual void read(); virtual void write(const double position, const double velocity, const double current); diff --git a/maxon_epos_driver/include/maxon_epos_driver/control/EposProfileVelocityMode.hpp b/maxon_epos_driver/include/maxon_epos_driver/control/EposProfileVelocityMode.hpp index ec5ec55..2047015 100644 --- a/maxon_epos_driver/include/maxon_epos_driver/control/EposProfileVelocityMode.hpp +++ b/maxon_epos_driver/include/maxon_epos_driver/control/EposProfileVelocityMode.hpp @@ -10,14 +10,14 @@ #include "maxon_epos_driver/control/ControlModeBase.hpp" #include "maxon_epos_driver/Device.hpp" -#include +#include class EposProfileVelocityMode : public ControlModeBase { public: virtual ~EposProfileVelocityMode(); - virtual void init(ros::NodeHandle &motor_nh, NodeHandle &node_handle); + virtual void init(rclcpp::Node &motor_nh, NodeHandle &node_handle); virtual void activate(); virtual void read(); virtual void write(const double position, const double velocity, const double current); diff --git a/maxon_epos_driver/include/maxon_epos_driver/utils/Macros.hpp b/maxon_epos_driver/include/maxon_epos_driver/utils/Macros.hpp index e10b63b..829fae3 100644 --- a/maxon_epos_driver/include/maxon_epos_driver/utils/Macros.hpp +++ b/maxon_epos_driver/include/maxon_epos_driver/utils/Macros.hpp @@ -8,8 +8,8 @@ #ifndef _Macros_HPP #define _Macros_HPP -#include "maxon_epos_driver/utils/EposException.hpp" -#include "maxon_epos_driver/Definitions.h" +#include "EposException.hpp" +#include "../Definitions.h" // range of node_id [1, 127] diff --git a/maxon_epos_driver/launch/test_1motor.launch.py b/maxon_epos_driver/launch/test_1motor.launch.py new file mode 100644 index 0000000..692a9f5 --- /dev/null +++ b/maxon_epos_driver/launch/test_1motor.launch.py @@ -0,0 +1,21 @@ +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + config = os.path.join( + get_package_share_directory('maxon_epos_driver'), + 'config', + 'test_1motor.yaml' + ) + + return LaunchDescription([ + Node( + package='maxon_epos_driver', + executable='maxon_bringup', # Please DON'T add name to this node + parameters=[config] + ) + ]) \ No newline at end of file diff --git a/maxon_epos_driver/package.xml b/maxon_epos_driver/package.xml index f395833..e379910 100644 --- a/maxon_epos_driver/package.xml +++ b/maxon_epos_driver/package.xml @@ -1,5 +1,5 @@ - + maxon_epos_driver 0.0.0 The maxon_epos_driver package @@ -11,14 +11,15 @@ Ryutaro Matsumoto - catkin - roscpp + + ament_cmake + rclcpp std_msgs maxon_epos_msgs - roscpp + rclcpp std_msgs message_runtime - roscpp + rclcpp std_msgs maxon_epos_msgs @@ -26,6 +27,7 @@ + ament_cmake diff --git a/maxon_epos_driver/src/lib/Device.cpp b/maxon_epos_driver/src/lib/Device.cpp index 386da36..938c4b6 100644 --- a/maxon_epos_driver/src/lib/Device.cpp +++ b/maxon_epos_driver/src/lib/Device.cpp @@ -10,7 +10,7 @@ #include "maxon_epos_driver/utils/Macros.hpp" #include "maxon_epos_driver/utils/EposException.hpp" -#include +#include #include #include @@ -152,10 +152,10 @@ void* DeviceHandle::OpenSubDevice(const DeviceInfo &device_info, const std::shar */ void DeviceHandle::CloseDevice(void* raw_device_ptr) { - ROS_INFO("Called CloseDevice"); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Called CloseDevice"); unsigned int error_code; if (VCS_CloseDevice(raw_device_ptr, &error_code) == VCS_FALSE) { - ROS_ERROR_STREAM("CloseDevice (" + EposException::ToErrorInfo(error_code) + ")"); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("rclcpp"), "CloseDevice (" + EposException::ToErrorInfo(error_code) + ")"); } } @@ -166,10 +166,10 @@ void DeviceHandle::CloseDevice(void* raw_device_ptr) */ void DeviceHandle::CloseSubDevice(void *raw_device_ptr) { - ROS_INFO("Called CloseSubDevice"); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Called CloseSubDevice"); unsigned int error_code; if (VCS_CloseSubDevice(raw_device_ptr, &error_code) == VCS_FALSE) { - ROS_ERROR_STREAM("CloseSubDevice (" + EposException::ToErrorInfo(error_code) + ")"); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("rclcpp"), "CloseSubDevice (" + EposException::ToErrorInfo(error_code) + ")"); } } @@ -285,7 +285,7 @@ NodeHandle HandleManager::CreateEposHandle(const DeviceInfo &device_info, const throw EposException("Invalid node_id"); } - NodeInfo node_info(device_info, node_id); + const NodeInfo node_info(device_info, node_id); // record existing handles static std::map, CompareNodeInfo> existing_node_handles; @@ -293,7 +293,7 @@ NodeHandle HandleManager::CreateEposHandle(const DeviceInfo &device_info, const // check if handle already exists const std::shared_ptr existing_handle(existing_node_handles[node_info].lock()); if (existing_handle) { - ROS_INFO("Found in existing handles"); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Found in existing handles"); return *existing_handle; } @@ -310,7 +310,7 @@ NodeHandle HandleManager::CreateEposHandle(const DeviceInfo &device_info, const return *sub_handle; } } catch (const EposException &e) { - ROS_ERROR_STREAM(e.what()); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("rclcpp"), e.what()); throw EposException("Create EposHandle (Could not identify node)"); } } diff --git a/maxon_epos_driver/src/lib/EposManager.cpp b/maxon_epos_driver/src/lib/EposManager.cpp index f7b50a7..797e4fe 100644 --- a/maxon_epos_driver/src/lib/EposManager.cpp +++ b/maxon_epos_driver/src/lib/EposManager.cpp @@ -6,8 +6,8 @@ */ #include "maxon_epos_driver/EposManager.hpp" -#include "maxon_epos_msgs/MotorState.h" -#include "maxon_epos_msgs/MotorStates.h" +#include "maxon_epos_msgs/msg/motor_state.h" +#include "maxon_epos_msgs/msg/motor_states.h" #include @@ -31,40 +31,41 @@ EposManager::~EposManager() = default; * * @return */ -bool EposManager::init(ros::NodeHandle &root_nh, ros::NodeHandle &motors_nh, +bool EposManager::init(rclcpp::Node &root_nh, rclcpp::Node &motors_nh, const std::vector &motor_names) { BOOST_FOREACH (const std::string &motor_name, motor_names) { - ROS_INFO_STREAM("Loading Epos: " << motor_name); + RCLCPP_INFO_STREAM(rclcpp::get_logger("rclcpp"), "Loading Epos: " << motor_name); // Copy constructor => ns = motors_nh's namespace + / + motor_name - ros::NodeHandle motor_nh(motors_nh, motor_name); + rclcpp::Node motora_nh(motor_name); + RCLCPP_INFO(motora_nh.get_logger(), "Created new node: %s", motora_nh.get_name()); std::shared_ptr motor(new EposMotor()); - motor->init(root_nh, motor_nh, motor_name); + motor->init(root_nh, motora_nh, motor_name); m_motors.push_back(motor); } - m_all_motor_publisher = motors_nh.advertise("get_all_states", 100); - m_all_motor_subscriber = motors_nh.subscribe("set_all_states", 100, &EposManager::write, this); + m_all_motor_publisher = motors_nh.create_publisher("get_all_states", 100); + m_all_motor_subscriber = motors_nh.create_subscription("set_all_states", 100, std::bind(&EposManager::write, this, std::placeholders::_1)); return true; } void EposManager::read() { - maxon_epos_msgs::MotorStates msg; + maxon_epos_msgs::msg::MotorStates msg; BOOST_FOREACH (const std::shared_ptr &motor, m_motors) { msg.states.push_back(motor->read()); } - m_all_motor_publisher.publish(msg); + m_all_motor_publisher->publish(msg); } -void EposManager::write(const maxon_epos_msgs::MotorStates::ConstPtr& msg) +void EposManager::write(const maxon_epos_msgs::msg::MotorStates::SharedPtr msg) { - for (int i = 0; i < m_motors.size(); i++) { - maxon_epos_msgs::MotorState state = msg->states[i]; - ROS_DEBUG_STREAM("Send: " << state.position); + for (uint i = 0; i < m_motors.size(); i++) { + maxon_epos_msgs::msg::MotorState state = msg->states[i]; + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("rclcpp"), "Send: " << state.position); m_motors[i]->write(state.position, state.velocity, state.current); } } diff --git a/maxon_epos_driver/src/lib/EposMotor.cpp b/maxon_epos_driver/src/lib/EposMotor.cpp index 7d1b44f..1fc1f83 100644 --- a/maxon_epos_driver/src/lib/EposMotor.cpp +++ b/maxon_epos_driver/src/lib/EposMotor.cpp @@ -13,8 +13,10 @@ #include "maxon_epos_driver/control/EposProfileVelocityMode.hpp" #include "maxon_epos_driver/control/EposCurrentMode.hpp" -#include "maxon_epos_msgs/MotorState.h" +#include "maxon_epos_msgs/msg/motor_state.hpp" + +// TODO check if only declare_parameter is necessary /** * @brief Constructor */ @@ -31,12 +33,12 @@ EposMotor::~EposMotor() try { VCS_NODE_COMMAND_NO_ARGS(SetDisableState, m_epos_handle); } catch (const EposException &e) { - ROS_ERROR_STREAM(e.what()); + RCLCPP_ERROR_STREAM(m_control_mode->nh->get_logger(), e.what()); } } -void EposMotor::init(ros::NodeHandle &root_nh, ros::NodeHandle &motor_nh, const std::string &motor_name) +void EposMotor::init(rclcpp::Node &root_nh, rclcpp::Node &motor_nh, const std::string &motor_name) { m_motor_name = motor_name; initEposDeviceHandle(motor_nh); @@ -52,11 +54,11 @@ void EposMotor::init(ros::NodeHandle &root_nh, ros::NodeHandle &motor_nh, const VCS_NODE_COMMAND_NO_ARGS(SetEnableState, m_epos_handle); - m_state_publisher = motor_nh.advertise("get_state", 100); - m_state_subscriber = motor_nh.subscribe("set_state", 100, &EposMotor::writeCallback, this); + m_state_publisher = motor_nh.create_publisher("get_state", 100); + m_state_subscriber = motor_nh.create_subscription("set_state", 100, std::bind(&EposMotor::writeCallback, this, std::placeholders::_1)); } -maxon_epos_msgs::MotorState EposMotor::read() +maxon_epos_msgs::msg::MotorState EposMotor::read() { try { if (m_control_mode) { @@ -66,14 +68,14 @@ maxon_epos_msgs::MotorState EposMotor::read() m_velocity = ReadVelocity(); m_current = ReadCurrent(); } catch (const EposException &e) { - ROS_ERROR_STREAM(e.what()); + RCLCPP_ERROR_STREAM(m_control_mode->nh->get_logger(), e.what()); } - maxon_epos_msgs::MotorState msg; + maxon_epos_msgs::msg::MotorState msg; msg.motor_name = m_motor_name; msg.position = m_position; msg.velocity = m_velocity; msg.current = m_current; - m_state_publisher.publish(msg); + m_state_publisher->publish(msg); return msg; } @@ -84,11 +86,11 @@ void EposMotor::write(const double position, const double velocity, const double m_control_mode->write(position, velocity, current); } } catch (const EposException &e) { - ROS_ERROR_STREAM(e.what()); + RCLCPP_ERROR_STREAM(m_control_mode->nh->get_logger(), e.what()); } } -void EposMotor::writeCallback(const maxon_epos_msgs::MotorState::ConstPtr &msg) +void EposMotor::writeCallback(const maxon_epos_msgs::msg::MotorState::SharedPtr msg) { write(msg->position, msg->velocity, msg->current); } @@ -96,15 +98,28 @@ void EposMotor::writeCallback(const maxon_epos_msgs::MotorState::ConstPtr &msg) /** * @brief Initialize Epos Node Handle * - * @param motor_nh ros NodeHandle of motor + * @param motor_nh rclcpp Node of motor */ -void EposMotor::initEposDeviceHandle(ros::NodeHandle &motor_nh) +void EposMotor::initEposDeviceHandle(rclcpp::Node &motor_nh) { - const DeviceInfo device_info(motor_nh.param("device", "EPOS4"), - motor_nh.param("protocol_stack", "MAXON SERIAL V2"), - motor_nh.param("interface", "USB"), - motor_nh.param("port", "USB0")); - const unsigned short node_id(motor_nh.param("node_id", 0)); + std::string device, protocol_stack, interface, port; + motor_nh.declare_parameter("device", "EPOS4"); + motor_nh.declare_parameter("protocol_stack","MAXON SERIAL V2"); + motor_nh.declare_parameter("interface", "USB"); + motor_nh.declare_parameter("port", "USB0"); + motor_nh.get_parameter("device", device); + motor_nh.get_parameter("protocol_stack", protocol_stack); + motor_nh.get_parameter("interface", interface); + motor_nh.get_parameter("port", port); + const DeviceInfo device_info(device, protocol_stack, interface, port); + const unsigned short node_id(motor_nh.declare_parameter("node_id", 0)); + RCLCPP_WARN(motor_nh.get_logger(), "Node name: %s", motor_nh.get_name()); + RCLCPP_WARN(motor_nh.get_logger(), "Node namespace: %s", motor_nh.get_namespace()); + // motor_nh.declare_parameter("node_id", 0); + // unsigned short node_id; + // motor_nh.get_parameter("node_id", node_id); + + // create epos handle m_epos_handle = HandleManager::CreateEposHandle(device_info, node_id); @@ -121,7 +136,7 @@ void EposMotor::initDeviceError() for (int i = 1; i <= num_of_device_errors; i++) { unsigned int device_error_code; VCS_NODE_COMMAND(GetDeviceErrorCode, m_epos_handle, i, &device_error_code); - ROS_WARN_STREAM("EPOS Device Error: 0x" << std::hex << device_error_code); + RCLCPP_WARN_STREAM(m_control_mode->nh->get_logger(),"EPOS Device Error: 0x" << std::hex << device_error_code); } // Clear Errors @@ -138,11 +153,11 @@ void EposMotor::initDeviceError() * * @param motor_nh ros NodeHandle of motor */ -void EposMotor::initProtocolStackChanges(ros::NodeHandle &motor_nh) +void EposMotor::initProtocolStackChanges(rclcpp::Node &motor_nh) { // load values from ros parameter server - const unsigned int baudrate(motor_nh.param("baudrate", 0)); - const unsigned int timeout(motor_nh.param("timeout", 0)); + const unsigned int baudrate = (uint) motor_nh.declare_parameter("baudrate", 0); + const unsigned int timeout = (uint) motor_nh.declare_parameter("timeout", 0); if (baudrate == 0 && timeout == 0) { return; } @@ -164,9 +179,9 @@ void EposMotor::initProtocolStackChanges(ros::NodeHandle &motor_nh) * @param root_nh NodeHandle of TopLevel * @param motor_nh NodeHandle of motor */ -void EposMotor::initControlMode(ros::NodeHandle &root_nh, ros::NodeHandle &motor_nh) +void EposMotor::initControlMode(rclcpp::Node &root_nh, rclcpp::Node &motor_nh) { - const std::string control_mode(motor_nh.param("control_mode", "profile_position")); + const std::string control_mode(motor_nh.declare_parameter("control_mode", "profile_position")); if (control_mode == "profile_position") { m_control_mode.reset(new EposProfilePositionMode()); } else if (control_mode == "profile_velocity") { @@ -184,23 +199,29 @@ void EposMotor::initControlMode(ros::NodeHandle &root_nh, ros::NodeHandle &motor * * @param motor_nh NodeHandle of motor */ -void EposMotor::initEncoderParams(ros::NodeHandle &motor_nh) +void EposMotor::initEncoderParams(rclcpp::Node &motor_nh) { - ros::NodeHandle encoder_nh(motor_nh, "encoder"); + rclcpp::Node encoder_nh("encoder", motor_nh.get_name()); - const int type(encoder_nh.param("type", 0)); + int type, resolution, gear_ratio; + encoder_nh.declare_parameter("type",0); + encoder_nh.get_parameter("type", type); VCS_NODE_COMMAND(SetSensorType, m_epos_handle, type); if (type == 1 || type == 2) { // Incremental Encoder - const int resolution(encoder_nh.param("resolution", 0)); - const int gear_ratio(encoder_nh.param("gear_ratio", 0)); + encoder_nh.declare_parameter("resolution",0); + encoder_nh.declare_parameter("gear_ratio",0); + encoder_nh.get_parameter("resolution", resolution); + encoder_nh.get_parameter("gear_ratio", gear_ratio); if (resolution == 0 || gear_ratio == 0) { throw EposException("Please set parameter 'resolution' and 'gear_ratio'"); } - const bool inverted_polarity(encoder_nh.param("inverted_polarity", false)); + bool inverted_polarity; + encoder_nh.declare_parameter("inverted_poloarity",false); + encoder_nh.get_parameter("inverted_polarity", inverted_polarity); if (inverted_polarity) { - ROS_INFO_STREAM(m_motor_name + ": Inverted polarity is True"); + RCLCPP_INFO_STREAM(m_control_mode->nh->get_logger(), m_motor_name + ": Inverted polarity is True"); } VCS_NODE_COMMAND(SetIncEncoderParameter, m_epos_handle, resolution, inverted_polarity); @@ -208,15 +229,15 @@ void EposMotor::initEncoderParams(ros::NodeHandle &motor_nh) } else if (type == 4 || type == 5) { // SSI Abs Encoder - bool inverted_polarity; + bool inverted_polarity = encoder_nh.declare_parameter("inverted_polarity", false); int data_rate, number_of_multiturn_bits, number_of_singleturn_bits; - encoder_nh.param("inverted_polarity", inverted_polarity, false); - if (encoder_nh.hasParam("data_rate") && encoder_nh.hasParam("number_of_singleturn_bits") && encoder_nh.hasParam("number_of_multiturn_bits")) { - encoder_nh.getParam("data_rate", data_rate); - encoder_nh.getParam("number_of_multiturn_bits", number_of_multiturn_bits); - encoder_nh.getParam("number_of_singleturn_bits", number_of_singleturn_bits); + + if (encoder_nh.has_parameter("data_rate") && encoder_nh.has_parameter("number_of_singleturn_bits") && encoder_nh.has_parameter("number_of_multiturn_bits")) { + encoder_nh.get_parameter("data_rate", data_rate); + encoder_nh.get_parameter("number_of_multiturn_bits", number_of_multiturn_bits); + encoder_nh.get_parameter("number_of_singleturn_bits", number_of_singleturn_bits); } else { - ROS_ERROR("Please set 'data_rate', 'number_of_singleturn_bits', and 'number_of_multiturn_bits'"); + RCLCPP_ERROR(m_control_mode->nh->get_logger(), "Please set 'data_rate', 'number_of_singleturn_bits', and 'number_of_multiturn_bits'"); } VCS_NODE_COMMAND(SetSsiAbsEncoderParameter, m_epos_handle, data_rate, number_of_multiturn_bits, number_of_singleturn_bits, inverted_polarity); m_max_qc = inverted_polarity ? -(1 << number_of_singleturn_bits) : (1 << number_of_singleturn_bits); @@ -226,14 +247,14 @@ void EposMotor::initEncoderParams(ros::NodeHandle &motor_nh) } } -void EposMotor::initProfilePosition(ros::NodeHandle &motor_nh) +void EposMotor::initProfilePosition(rclcpp::Node &motor_nh) { - ros::NodeHandle profile_position_nh(motor_nh, "profile_position"); - if (profile_position_nh.hasParam("velocity")) { + rclcpp::Node profile_position_nh("profile_position", motor_nh.get_name()); + if (profile_position_nh.has_parameter("velocity")) { int velocity, acceleration, deceleration; - profile_position_nh.getParam("velocity", velocity); - profile_position_nh.getParam("acceleration", acceleration); - profile_position_nh.getParam("deceleration", deceleration); + profile_position_nh.get_parameter("velocity", velocity); + profile_position_nh.get_parameter("acceleration", acceleration); + profile_position_nh.get_parameter("deceleration", deceleration); VCS_NODE_COMMAND(SetPositionProfile, m_epos_handle, velocity, acceleration, deceleration); } } @@ -243,10 +264,10 @@ void EposMotor::initProfilePosition(ros::NodeHandle &motor_nh) * * @param motor_nh NodeHandle of motor */ -void EposMotor::initMiscParams(ros::NodeHandle &motor_nh) +void EposMotor::initMiscParams(rclcpp::Node &motor_nh) { // use ros unit or default epos unit - motor_nh.param("use_ros_unit", m_use_ros_unit, false); + m_use_ros_unit = motor_nh.declare_parameter("use_ros_unit", false); } diff --git a/maxon_epos_driver/src/lib/control/ControlModeBase.cpp b/maxon_epos_driver/src/lib/control/ControlModeBase.cpp index 40a0a04..7634e9e 100644 --- a/maxon_epos_driver/src/lib/control/ControlModeBase.cpp +++ b/maxon_epos_driver/src/lib/control/ControlModeBase.cpp @@ -13,8 +13,9 @@ ControlModeBase::~ControlModeBase() {} -void ControlModeBase::init(ros::NodeHandle &motor_nh, NodeHandle &node_handle) +void ControlModeBase::init(rclcpp::Node &motor_nh, NodeHandle &node_handle) { + nh = std::make_shared(motor_nh.get_name()); m_epos_handle = node_handle; - m_use_ros_unit = motor_nh.param("use_ros_unit", false); + m_use_ros_unit = nh->declare_parameter("use_ros_unit", false); } diff --git a/maxon_epos_driver/src/lib/control/EposCurrentMode.cpp b/maxon_epos_driver/src/lib/control/EposCurrentMode.cpp index 3e14acb..c288b9d 100644 --- a/maxon_epos_driver/src/lib/control/EposCurrentMode.cpp +++ b/maxon_epos_driver/src/lib/control/EposCurrentMode.cpp @@ -11,7 +11,7 @@ EposCurrentMode::~EposCurrentMode() {} -void EposCurrentMode::init(ros::NodeHandle &motor_nh, NodeHandle &node_handle) +void EposCurrentMode::init(rclcpp::Node &motor_nh, NodeHandle &node_handle) { ControlModeBase::init(motor_nh, node_handle); } diff --git a/maxon_epos_driver/src/lib/control/EposProfilePositionMode.cpp b/maxon_epos_driver/src/lib/control/EposProfilePositionMode.cpp index 4fe3c00..1faf300 100644 --- a/maxon_epos_driver/src/lib/control/EposProfilePositionMode.cpp +++ b/maxon_epos_driver/src/lib/control/EposProfilePositionMode.cpp @@ -11,27 +11,41 @@ EposProfilePositionMode::~EposProfilePositionMode() {} -void EposProfilePositionMode::init(ros::NodeHandle &motor_nh, NodeHandle &node_handle) +void EposProfilePositionMode::init(rclcpp::Node &motor_nh, NodeHandle &node_handle) { ControlModeBase::init(motor_nh, node_handle); if (m_use_ros_unit) { - ros::NodeHandle encoder_nh(motor_nh, "encoder"); - const int type(encoder_nh.param("type", 0)); + rclcpp::Node encoder_nh("encoder", motor_nh.get_name()); + encoder_nh.declare_parameter("type",0); + int type, resolution, gear_ratio; + encoder_nh.get_parameter("type", type); + // const int type(encoder_nh.get_parameter("type", 0)); if (type == 1 || type == 2) { - const int resolution(encoder_nh.param("resolution", 0)); - const int gear_ratio(encoder_nh.param("gear_ratio", 0)); + // const int resolution(encoder_nh.get_parameter("resolution", 0)); + // const int gear_ratio(encoder_nh.get_parameter("gear_ratio", 0)); + encoder_nh.declare_parameter("resolution",0); + encoder_nh.declare_parameter("gear_ratio",0); + encoder_nh.get_parameter("resolution", resolution); + encoder_nh.get_parameter("gear_ratio", gear_ratio); if (resolution == 0 || gear_ratio == 0) { throw EposException("Please set parameter 'resolution' and 'gear_ratio'"); } - const bool inverted_polarity(encoder_nh.param("inverted_poloarity", false)); + // const bool inverted_polarity(encoder_nh.get_parameter("inverted_poloarity", false)); + bool inverted_polarity; + encoder_nh.declare_parameter("inverted_poloarity",false); + encoder_nh.get_parameter("inverted_polarity", inverted_polarity); m_max_qc = 4 * resolution * gear_ratio; } else if (type == 4 || type == 5) { int number_of_singleturn_bits; bool inverted_polarity; - encoder_nh.param("number_of_singleturn_bits", number_of_singleturn_bits); - encoder_nh.param("inverted_poloarity", inverted_polarity); + // encoder_nh.get_parameter("number_of_singleturn_bits", number_of_singleturn_bits); + // encoder_nh.get_parameter("inverted_poloarity", inverted_polarity); + encoder_nh.declare_parameter("number_of_singleturn_bits",0); + encoder_nh.declare_parameter("inverted_poloarity",false); + encoder_nh.get_parameter("number_of_singleturn_bits", number_of_singleturn_bits); + encoder_nh.get_parameter("inverted_poloarity", inverted_polarity); m_max_qc = inverted_polarity ? -(1 << number_of_singleturn_bits) : (1 << number_of_singleturn_bits); } else { throw EposException("Invalid Encoder Type: " + std::to_string(type) + ")"); @@ -50,13 +64,13 @@ void EposProfilePositionMode::read() void EposProfilePositionMode::write(const double position, const double velocity, const double current) { int quad_count; - ROS_DEBUG_STREAM("Target Position: " << position); - ROS_DEBUG_STREAM("Encoder Resolution: " << m_max_qc); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("encoder"), "Target Position: " << position); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("encoder"), "Encoder Resolution: " << m_max_qc); if (m_use_ros_unit) { quad_count = static_cast((position / (2 * M_PI)) * m_max_qc); } else { quad_count = static_cast(position); } - ROS_DEBUG_STREAM("Send Quad Count: " << quad_count); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("encoder"), "Send Quad Count: " << quad_count); VCS_NODE_COMMAND(MoveToPosition, m_epos_handle, quad_count, /* absolute */true, /* overwrite */true); } diff --git a/maxon_epos_driver/src/lib/control/EposProfileVelocityMode.cpp b/maxon_epos_driver/src/lib/control/EposProfileVelocityMode.cpp index fa4088d..3b0e6a7 100644 --- a/maxon_epos_driver/src/lib/control/EposProfileVelocityMode.cpp +++ b/maxon_epos_driver/src/lib/control/EposProfileVelocityMode.cpp @@ -10,7 +10,7 @@ EposProfileVelocityMode::~EposProfileVelocityMode() {} -void EposProfileVelocityMode::init(ros::NodeHandle &motor_nh, NodeHandle &node_handle) +void EposProfileVelocityMode::init(rclcpp::Node &motor_nh, NodeHandle &node_handle) { ControlModeBase::init(motor_nh, node_handle); } diff --git a/maxon_epos_driver/src/node/maxon_bringup.cpp b/maxon_epos_driver/src/node/maxon_bringup.cpp index a48088c..e426dd7 100644 --- a/maxon_epos_driver/src/node/maxon_bringup.cpp +++ b/maxon_epos_driver/src/node/maxon_bringup.cpp @@ -1,45 +1,47 @@ /** * @file maxon_bringup * @brief - * @author arwtyxouymz + * @author arwtyxouymz -> revised by cristinaluna * @date 2019-05-24 17:49:41 */ -#include +#include #include #include #include "maxon_epos_driver/EposManager.hpp" int main(int argc, char** argv) { - ros::init(argc, argv, "maxon_bringup"); - ros::NodeHandle nh; - ros::NodeHandle private_nh("~"); + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr nh = std::make_shared("maxon_bringup"); + rclcpp::Node private_nh("maxon_bringup_private"); + nh->declare_parameter>("motor_names", {}); std::vector motor_names; - if (!private_nh.getParam("motor_names", motor_names)) { - ROS_FATAL("Failed to load motor_names"); + nh->get_parameter>("motor_names", motor_names); + if (motor_names.size() <= 0) { + RCLCPP_FATAL(nh->get_logger(), "Failed to load motor_names"); return 1; } - ros::Rate sleep_rate(50); + rclcpp::Rate sleep_rate(50); EposManager manager; - if (!manager.init(nh, private_nh, motor_names)) + if (!manager.init(*nh, private_nh, motor_names)) { - ROS_FATAL("Failed to initialize EposManager"); + RCLCPP_FATAL(nh->get_logger(), "Failed to initialize EposManager"); return 1; } - ROS_INFO("Motors Initialized"); + RCLCPP_INFO(nh->get_logger(), "Motors Initialized"); - ros::AsyncSpinner spinner(1); - spinner.start(); + // rclcpp::AsyncSpinner spinner(1); + // spinner.start(); - while (ros::ok()) { + while (rclcpp::ok()) { manager.read(); sleep_rate.sleep(); } - spinner.stop(); + // spinner.stop(); return 0; } diff --git a/maxon_epos_example/CMakeLists.txt b/maxon_epos_example/CMakeLists.txt index 303e6b0..050b235 100644 --- a/maxon_epos_example/CMakeLists.txt +++ b/maxon_epos_example/CMakeLists.txt @@ -1,197 +1,64 @@ -cmake_minimum_required(VERSION 2.8.3) -project(maxon_epos_example) +cmake_minimum_required(VERSION 3.5) +project(maxon_epos_example_cpp) -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - maxon_epos_driver -) -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES maxon_epos_example -# CATKIN_DEPENDS maxon_epos_driver -# DEPENDS system_lib -) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() -########### -## Build ## -########### -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( -# include - ${catkin_INCLUDE_DIRS} +# Find dependencies +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) + + +set(dependencies + rclcpp + rclcpp_components + sensor_msgs + std_msgs ) -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/maxon_epos_example.cpp -# ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -add_executable(${PROJECT_NAME}_node node/sample.cpp) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -target_link_libraries(${PROJECT_NAME}_node - ${catkin_LIBRARIES} +include_directories( + maxon_epos_driver + maxon_epos_msgs + ${ament_INCLUDE_DIRS} ) -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables and/or libraries for installation -# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_maxon_epos_example.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) +# Macro to create nodes easily +macro(add_node node_name node_src) + + add_executable("${node_name}" "${node_src}") + ament_target_dependencies("${node_name}" ${dependencies}) + +endmacro() + +add_node(${PROJECT_NAME}_node node/sample.cpp) + + +install(DIRECTORY include/ + DESTINATION include) + +install(TARGETS ${PROJECT_NAME}_node + DESTINATION lib) + +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME}/) + +install(DIRECTORY config + DESTINATION share/${PROJECT_NAME}/) + +ament_package() \ No newline at end of file diff --git a/maxon_epos_example/launch/example_maxon_epos.launch b/maxon_epos_example/launch/example_maxon_epos_launch.xml similarity index 100% rename from maxon_epos_example/launch/example_maxon_epos.launch rename to maxon_epos_example/launch/example_maxon_epos_launch.xml diff --git a/maxon_epos_example/node/sample.cpp b/maxon_epos_example/node/sample.cpp index 0c561df..957348e 100644 --- a/maxon_epos_example/node/sample.cpp +++ b/maxon_epos_example/node/sample.cpp @@ -5,21 +5,21 @@ * @date 2019-06-05 23:51:24 */ -#include -#include +#include +#include "std_msgs/msg/Float32MultiArray.hpp" int main(int argc, char** argv) { - ros::init(argc, argv, "sample"); - ros::NodeHandle nh; + rclcpp::init(argc, argv); + rclcpp::Node::SharedPtr nh("sample"); - ros::Publisher pub = nh.advertise("/maxon_bringup/all_position", 1000); - std_msgs::Float32MultiArray msg = std_msgs::Float32MultiArray(); + rclcpp::Publisher::SharedPtr pub = nh->create_publisher("/maxon_bringup/all_position", 1000); + std_msgs::msg::Float32MultiArray msg = std_msgs::msg::Float32MultiArray(); msg.data.push_back(300000); msg.data.push_back(-200000); - ros::Rate sleep_rate(50); - while (ros::ok()) { + rclcpp::Rate sleep_rate(50); + while (rclcpp::ok()) { pub.publish(msg); sleep_rate.sleep(); } diff --git a/maxon_epos_example/package.xml b/maxon_epos_example/package.xml index 9ee7b53..100e47c 100644 --- a/maxon_epos_example/package.xml +++ b/maxon_epos_example/package.xml @@ -1,6 +1,6 @@ - - maxon_epos_example + + maxon_epos_example_cpp 0.0.0 The maxon_epos_example package @@ -13,13 +13,19 @@ Ryutaro Matsumoto - catkin - maxon_epos_driver + ament_cmake + std_msgs + rclcpp + maxon_epos_msgs + rclcpp + maxon_epos_msgs + + maxon_epos_driver - + ament_cmake diff --git a/maxon_epos_msgs/CMakeLists.txt b/maxon_epos_msgs/CMakeLists.txt index 818fc17..7ed6e97 100644 --- a/maxon_epos_msgs/CMakeLists.txt +++ b/maxon_epos_msgs/CMakeLists.txt @@ -1,198 +1,27 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(maxon_epos_msgs) -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - message_generation - std_msgs -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -add_message_files( - FILES - MotorState.msg - MotorStates.msg -) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -generate_messages( - DEPENDENCIES - std_msgs -) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) +find_package(rosidl_default_generators REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(std_msgs REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(rosidl_default_generators REQUIRED) -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES maxon_epos_msgs - CATKIN_DEPENDS message_runtime std_msgs -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations include_directories( -# include - ${catkin_INCLUDE_DIRS} + msg ) -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/maxon_epos_msgs.cpp -# ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/maxon_epos_msgs_node.cpp) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} -# ) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables and/or libraries for installation -# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/MotorState.msg" + "msg/MotorStates.msg" + DEPENDENCIES builtin_interfaces std_msgs + ) -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_maxon_epos_msgs.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() + ament_export_dependencies(rosidl_default_runtime) -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) + ament_package() diff --git a/maxon_epos_msgs/msg/MotorState.msg b/maxon_epos_msgs/msg/MotorState.msg index a3e8cbb..e042b4d 100644 --- a/maxon_epos_msgs/msg/MotorState.msg +++ b/maxon_epos_msgs/msg/MotorState.msg @@ -1,5 +1,5 @@ -Header header +std_msgs/Header header string motor_name diff --git a/maxon_epos_msgs/msg/MotorStates.msg b/maxon_epos_msgs/msg/MotorStates.msg index 9d94af4..065d1fd 100644 --- a/maxon_epos_msgs/msg/MotorStates.msg +++ b/maxon_epos_msgs/msg/MotorStates.msg @@ -1,4 +1,4 @@ -Header header +std_msgs/Header header MotorState[] states diff --git a/maxon_epos_msgs/package.xml b/maxon_epos_msgs/package.xml index 73be8be..c8dd90d 100644 --- a/maxon_epos_msgs/package.xml +++ b/maxon_epos_msgs/package.xml @@ -1,30 +1,26 @@ - - maxon_epos_msgs - 0.0.0 - The maxon_epos_msgs package + + maxon_epos_msgs + 0.0.0 + The maxon_epos_msgs package - Ryutaro Matsumoto + Ryutaro Matsumoto - MIT + MIT - Ryutaro Matsumoto + Ryutaro Matsumoto + ament_cmake + rosidl_default_generators - catkin - message_generation - std_msgs - message_generation - std_msgs - std_msgs - message_runtime + message_generation + rosidl_default_runtime + rosidl_interface_packages - - - - - + + ament_cmake + diff --git a/maxon_epos_ros/CMakeLists.txt b/maxon_epos_ros/CMakeLists.txt deleted file mode 100644 index 6ed1606..0000000 --- a/maxon_epos_ros/CMakeLists.txt +++ /dev/null @@ -1,195 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(maxon_epos_ros) - -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES maxon_epos_ros -# CATKIN_DEPENDS other_catkin_pkg -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( -# include -# ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/maxon_epos_ros.cpp -# ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/maxon_epos_ros_node.cpp) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} -# ) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables and/or libraries for installation -# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_maxon_epos_ros.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/maxon_epos_ros/package.xml b/maxon_epos_ros/package.xml deleted file mode 100644 index 4d5f70c..0000000 --- a/maxon_epos_ros/package.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - maxon_epos_ros - 0.0.0 - The maxon_epos_ros package - - Ryutaro Matsumoto - - MIT - - Ryutaro Matsumoto - - - catkin - - - - - - - diff --git a/maxon_epos_ros/CHANGELOG.rst b/maxon_epos_ros2_cpp/CHANGELOG.rst similarity index 100% rename from maxon_epos_ros/CHANGELOG.rst rename to maxon_epos_ros2_cpp/CHANGELOG.rst diff --git a/maxon_epos_ros2_cpp/CMakeLists.txt b/maxon_epos_ros2_cpp/CMakeLists.txt new file mode 100644 index 0000000..4759afd --- /dev/null +++ b/maxon_epos_ros2_cpp/CMakeLists.txt @@ -0,0 +1,60 @@ +cmake_minimum_required(VERSION 3.5) +project(maxon_epos_ros2_cpp) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + + +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_auto REQUIRED) +ament_auto_find_build_dependencies() + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) + + +set(dependencies + rclcpp + rclcpp_components + sensor_msgs + std_msgs +) + +include_directories( + maxon_epos_msgs + maxon_epos_driver + maxon_epos_example +) + +# Macro to create nodes easily +macro(add_node node_name node_src) + + add_executable("${node_name}" "${node_src}") + ament_target_dependencies("${node_name}" ${dependencies}) + +endmacro() + +# Create Cpp executable, create executable nodes here +# add_node(maxon_epos_driver ../maxon_epos_driver/src/node/maxon_bringup.cpp) +#add_executable(maxon_epos_driver src/node/maxon_bringup.cpp) + +ament_export_dependencies(rclcpp) + +#target_link_libraries(${PROJECT_NAME} maxon_epos_driver) + + +ament_package() + + diff --git a/maxon_epos_ros2_cpp/package.xml b/maxon_epos_ros2_cpp/package.xml new file mode 100644 index 0000000..147e1c4 --- /dev/null +++ b/maxon_epos_ros2_cpp/package.xml @@ -0,0 +1,28 @@ + + + maxon_epos_ros2_cpp + 0.0.0 + The maxon_epos_ros2 package + + Ryutaro Matsumoto + Cristina Luna + + MIT + + Ryutaro Matsumoto + + + ament_cmake + rosidl_default_runtime + message_package + maxon_epos_msgs + maxon_epos_driver + maxon_epos_example + + + + + ament_cmake + + +