From cacd0ff6ae757070034be154ac2a32e021bad42a Mon Sep 17 00:00:00 2001 From: supertick Date: Thu, 16 May 2024 05:37:41 -0700 Subject: [PATCH 01/14] inmoov_msgs converted --- inmoov_msgs/CMakeLists.txt | 33 +++++++++++++++++---------------- inmoov_msgs/package.xml | 25 +++++++++++++++---------- 2 files changed, 32 insertions(+), 26 deletions(-) diff --git a/inmoov_msgs/CMakeLists.txt b/inmoov_msgs/CMakeLists.txt index 2556b80..0575b2e 100755 --- a/inmoov_msgs/CMakeLists.txt +++ b/inmoov_msgs/CMakeLists.txt @@ -1,25 +1,26 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(inmoov_msgs) -find_package(catkin REQUIRED - COMPONENTS message_generation std_msgs) +find_package(ament_cmake REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) -add_message_files( - FILES - MotorCommand.msg - MotorStatus.msg - SmartServoStatus.msg +set(msg_files + "msg/MotorCommand.msg" + "msg/MotorStatus.msg" + "msg/SmartServoStatus.msg" ) -add_service_files( - FILES - MotorParameter.srv +set(srv_files + "srv/MotorParameter.srv" ) -generate_messages( - DEPENDENCIES std_msgs +rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_files} + ${srv_files} + DEPENDENCIES builtin_interfaces std_msgs ) -catkin_package( - CATKIN_DEPENDS message_runtime std_msgs -) +ament_export_dependencies(rosidl_default_runtime) +ament_package() diff --git a/inmoov_msgs/package.xml b/inmoov_msgs/package.xml index 0543a6b..8673148 100755 --- a/inmoov_msgs/package.xml +++ b/inmoov_msgs/package.xml @@ -1,23 +1,28 @@ - + inmoov_msgs 0.1.0 Common messages used throughout inmoov_motor stack. - Alan Timm + Greg Perry BSD - https://www.facebook.com/alansrobotlab - https://github.com/alansrobotlab/inmoov_ros + https://discord.gg/8NYKAUTxmZ + https://github.com/MyRobotLab/myrobotlab/issues - Alan Timm + Greg Perry - catkin + ament_cmake + rosidl_interface_packages - message_generation - std_msgs - message_runtime - std_msgs + builtin_interfaces + rosidl_default_generators + rosidl_default_runtime + std_msgs + + ament_cmake + rosidl_interface_packages + From 6e364bcee7d8dd996aa78d6f72dbb6e6dd4140ef Mon Sep 17 00:00:00 2001 From: supertick Date: Thu, 16 May 2024 05:41:19 -0700 Subject: [PATCH 02/14] gitignore --- .gitignore | 50 ++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 50 insertions(+) diff --git a/.gitignore b/.gitignore index 04698ee..cdbcaac 100644 --- a/.gitignore +++ b/.gitignore @@ -147,3 +147,53 @@ cython_debug/ # VS Code settings .vscode/ + +# ROS2 build artifacts +/build/ +/install/ +/log/ + +# ROS2 colcon files +.colcon/ + +# ROS2 specific files +.ros/ + +# Python bytecode +__pycache__/ +*.py[cod] + +# MacOS specific files +.DS_Store + +# Vim swap files +*.swp + +# Temporary files +*.tmp +*.bak +*.backup +*.orig +*.rej +*.~* + +# Editor directories and files +.idea/ +.vscode/ +*.sublime-project +*.sublime-workspace + +# Backup files +*~ +*.bak +*.sav + +# Environment specific files +.env +.venv + +# IDE specific files +.project +.cproject +.settings/ +.classpath From 719d4375a458dc16f837f4037354fe51e7611bc7 Mon Sep 17 00:00:00 2001 From: supertick Date: Thu, 16 May 2024 06:21:59 -0700 Subject: [PATCH 03/14] firt package.xml of inmoov_bringup --- inmoov_bringup/package.xml | 59 ++++++++++++-------------------------- 1 file changed, 18 insertions(+), 41 deletions(-) diff --git a/inmoov_bringup/package.xml b/inmoov_bringup/package.xml index c4bb50c..22b97ea 100644 --- a/inmoov_bringup/package.xml +++ b/inmoov_bringup/package.xml @@ -1,50 +1,27 @@ - - + inmoov_bringup - 0.0.0 - The inmoov_bringup package + 0.1.0 + + Bringup launch files and configurations for InMoov robot. + + Greg Perry - - - - grey + BSD + https://discord.gg/8NYKAUTxmZ + https://github.com/MyRobotLab/myrobotlab/issues - - - - TODO + Greg Perry + ament_cmake + rclcpp + std_msgs + inmoov_msgs + inmoov_description + robot_state_publisher + rviz2 - - - - - - - - - - - - - - - - - - - - - - - - catkin - - - - - + ament_cmake From 130ddbbc770b471ffba23271f8a374d2a5ec2489 Mon Sep 17 00:00:00 2001 From: supertick Date: Thu, 16 May 2024 06:33:20 -0700 Subject: [PATCH 04/14] step closer with inmoov_bringup --- README.md | 10 + display_launch.py | 59 ++++++ inmoov_bringup/CMakeLists.txt | 211 +++----------------- inmoov_bringup/launch/bringup.launch.py | 86 ++++++++ inmoov_bringup/package.xml | 1 + inmoov_description/CMakeLists.txt | 191 ++---------------- inmoov_description/launch/display.launch.py | 32 +++ inmoov_description/package.xml | 60 ++---- 8 files changed, 248 insertions(+), 402 deletions(-) create mode 100644 display_launch.py create mode 100644 inmoov_bringup/launch/bringup.launch.py create mode 100644 inmoov_description/launch/display.launch.py diff --git a/README.md b/README.md index 020192e..8769061 100644 --- a/README.md +++ b/README.md @@ -205,3 +205,13 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON) https://github.com/paul-shuvo/iai_kinect2_opencv4 ``` + + +``` +sudo apt install ros-humble-moveit +``` + +rosserial is gone ... + +New fancy +https://micro.ros.org/ \ No newline at end of file diff --git a/display_launch.py b/display_launch.py new file mode 100644 index 0000000..f7792e9 --- /dev/null +++ b/display_launch.py @@ -0,0 +1,59 @@ +import os +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument, ExecuteProcess +from launch.substitutions import LaunchConfiguration, Command, FindExecutable, PathJoinSubstitution +from launch.conditions import IfCondition + +def generate_launch_description(): + # Get the path to the package + pkg_inmoov_description = FindExecutable.find('inmoov_description') + + # Declare the launch arguments + declare_model_arg = DeclareLaunchArgument('model', default_value='') + declare_gui_arg = DeclareLaunchArgument('gui', default_value='False') + + # Set up the robot description + robot_description = { + 'robot_description': Command([ + PathJoinSubstitution([FindExecutable.find('xacro'), 'xacro ']), + PathJoinSubstitution([pkg_inmoov_description, 'robots', 'inmoov.urdf.xacro']) + ]) + } + + # Nodes + joint_state_publisher_node = Node( + package='joint_state_publisher', + executable='joint_state_publisher', + name='joint_state_publisher', + parameters=[{'source_list': ['rviz_command'], 'rate': 20}], + condition=IfCondition(LaunchConfiguration('gui')) + ) + robot_state_publisher_node = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher' + ) + joint_state_publisher_gui_node = Node( + package='joint_state_publisher_gui', + executable='joint_state_publisher_gui', + name='joint_state_publisher_gui' + ) + rviz_node = Node( + package='rviz2', + executable='rviz2', + name='rviz', + arguments=['-d', PathJoinSubstitution([pkg_inmoov_description, 'urdf.rviz'])], + required=True + ) + + # Return the launch description + return LaunchDescription([ + declare_model_arg, + declare_gui_arg, + robot_description, + joint_state_publisher_node, + robot_state_publisher_node, + joint_state_publisher_gui_node, + rviz_node + ]) diff --git a/inmoov_bringup/CMakeLists.txt b/inmoov_bringup/CMakeLists.txt index 83b76f5..f0a802f 100644 --- a/inmoov_bringup/CMakeLists.txt +++ b/inmoov_bringup/CMakeLists.txt @@ -1,192 +1,31 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(inmoov_bringup) -## Add support for C++11, supported in ROS Kinetic and newer -# add_definitions(-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 run_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 run_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 run_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 you 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 inmoov_bringup -# CATKIN_DEPENDS other_catkin_pkg -# DEPENDS system_lib +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(inmoov_msgs REQUIRED) +find_package(inmoov_description REQUIRED) +find_package(robot_state_publisher REQUIRED) +find_package(rviz2 REQUIRED) +# find_package(micro_ros_agent REQUIRED) + +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME}/launch ) -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -# include_directories(include) - -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/inmoov_bringup.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/inmoov_bringup_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 ## -############# +install(DIRECTORY config + DESTINATION share/${PROJECT_NAME}/config +) -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_inmoov_bringup.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() +# Install Python scripts and make them executable +install(PROGRAMS + nodes/joint_command_dispatcher.py + nodes/joint_status_dispatcher.py + nodes/motor_status_dispatcher.py + tools/enable_manager/enable_manager.py + tools/rviz_manager/rviz_manager.py + DESTINATION lib/${PROJECT_NAME} +) -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) +ament_package() diff --git a/inmoov_bringup/launch/bringup.launch.py b/inmoov_bringup/launch/bringup.launch.py new file mode 100644 index 0000000..5315424 --- /dev/null +++ b/inmoov_bringup/launch/bringup.launch.py @@ -0,0 +1,86 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction +from launch_ros.actions import Node +from launch.substitutions import LaunchConfiguration + +def generate_launch_description(): + config_file = os.path.join(get_package_share_directory('inmoov_bringup'), 'config', 'config.yaml') + + return LaunchDescription([ + DeclareLaunchArgument( + 'config_file', + default_value=config_file, + description='Path to the config file' + ), + + # Commented out hardware-related nodes for later use + # GroupAction([ + # Node( + # package='micro_ros_agent', + # executable='micro_ros_agent', + # name='micro_ros_agent_00', + # namespace='ns_servobus/00', + # output='screen', + # arguments=['serial', '--dev', '/dev/ttyACM0'] + # ), + # ]), + + # GroupAction([ + # Node( + # package='micro_ros_agent', + # executable='micro_ros_agent', + # name='micro_ros_agent_01', + # namespace='ns_servobus/01', + # output='screen', + # arguments=['serial', '--dev', '/dev/ttyACM1'] + # ), + # ]), + + # GroupAction([ + # Node( + # package='micro_ros_agent', + # executable='micro_ros_agent', + # name='micro_ros_agent_02', + # namespace='ns_servobus/02', + # output='screen', + # arguments=['serial', '--dev', '/dev/ttyACM2'] + # ), + # ]), + + Node( + package='inmoov_bringup', + executable='joint_command_dispatcher.py', + name='joint_command_dispatcher', + respawn=True + ), + + Node( + package='inmoov_bringup', + executable='joint_status_dispatcher.py', + name='joint_status_dispatcher', + respawn=True + ), + + Node( + package='inmoov_bringup', + executable='motor_status_dispatcher.py', + name='motor_status_dispatcher', + respawn=True + ), + + Node( + package='inmoov_bringup', + executable='enable_manager.py', + name='enable_manager', + respawn=True + ), + + Node( + package='inmoov_bringup', + executable='rviz_manager.py', + name='rviz_manager', + respawn=True + ), + ]) diff --git a/inmoov_bringup/package.xml b/inmoov_bringup/package.xml index 22b97ea..b6391e8 100644 --- a/inmoov_bringup/package.xml +++ b/inmoov_bringup/package.xml @@ -20,6 +20,7 @@ inmoov_description robot_state_publisher rviz2 + ament_cmake diff --git a/inmoov_description/CMakeLists.txt b/inmoov_description/CMakeLists.txt index 6478ca6..f17fa63 100644 --- a/inmoov_description/CMakeLists.txt +++ b/inmoov_description/CMakeLists.txt @@ -1,181 +1,24 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(inmoov_description) -## 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) +find_package(ament_cmake REQUIRED) +find_package(urdf REQUIRED) +find_package(xacro REQUIRED) +find_package(gazebo_ros REQUIRED) +find_package(rviz2 REQUIRED) +find_package(robot_state_publisher 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 run_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 run_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 run_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 you 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 inmoov_description -# CATKIN_DEPENDS other_catkin_pkg -# DEPENDS system_lib +install(DIRECTORY urdf robots + DESTINATION share/${PROJECT_NAME} ) -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -# include_directories(include) - -## Declare a C++ library -# add_library(inmoov_description -# src/${PROJECT_NAME}/inmoov_description.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(inmoov_description ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -# add_executable(inmoov_description_node src/inmoov_description_node.cpp) - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(inmoov_description_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(inmoov_description_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 inmoov_description inmoov_description_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 ## -############# +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME}/launch +) -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_inmoov_description.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() +install(PROGRAMS + launch_rviz.sh + DESTINATION share/${PROJECT_NAME} +) -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) +ament_package() diff --git a/inmoov_description/launch/display.launch.py b/inmoov_description/launch/display.launch.py new file mode 100644 index 0000000..9ba2845 --- /dev/null +++ b/inmoov_description/launch/display.launch.py @@ -0,0 +1,32 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import Command, LaunchConfiguration + +def generate_launch_description(): + use_sim_time = LaunchConfiguration('use_sim_time', default='false') + xacro_file = os.path.join(get_package_share_directory('inmoov_description'), 'robots', 'inmoov.urdf.xacro') + rviz_config_file = os.path.join(get_package_share_directory('inmoov_description'), 'urdf.rviz') + + return LaunchDescription([ + DeclareLaunchArgument( + name='use_sim_time', + default_value='false', + description='Use simulation (Gazebo) clock if true'), + Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + parameters=[{'use_sim_time': use_sim_time, 'robot_description': Command(['xacro ', xacro_file])}] + ), + Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=['-d', rviz_config_file] + ) + ]) diff --git a/inmoov_description/package.xml b/inmoov_description/package.xml index 2080795..4135325 100644 --- a/inmoov_description/package.xml +++ b/inmoov_description/package.xml @@ -1,50 +1,26 @@ - - + inmoov_description - 0.0.0 - The inmoov_description package + 0.1.0 + + URDF and mesh files for InMoov robot. + + Greg Perry - - - - grey + BSD + https://discord.gg/8NYKAUTxmZ + https://github.com/MyRobotLab/myrobotlab/issues - - - - TODO + Greg Perry + ament_cmake + urdf + xacro + gazebo_ros + rviz2 + robot_state_publisher - - - - - - - - - - - - - - - - - - - - - - - - catkin - - - - - + ament_cmake - \ No newline at end of file + From 9cc2ad42bd5d32b350e441035917b3bbe417403a Mon Sep 17 00:00:00 2001 From: supertick Date: Thu, 16 May 2024 06:38:50 -0700 Subject: [PATCH 05/14] updated --- inmoov_bringup/CMakeLists.txt | 2 +- inmoov_bringup/package.xml | 2 +- .../tools/enable_manager/enable_manager.py | 132 +++++++----------- .../tools/rviz_manager/rviz_manager.py | 119 ++++++++-------- 4 files changed, 105 insertions(+), 150 deletions(-) diff --git a/inmoov_bringup/CMakeLists.txt b/inmoov_bringup/CMakeLists.txt index f0a802f..99855bf 100644 --- a/inmoov_bringup/CMakeLists.txt +++ b/inmoov_bringup/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.5) project(inmoov_bringup) find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) +find_package(rclpy REQUIRED) find_package(std_msgs REQUIRED) find_package(inmoov_msgs REQUIRED) find_package(inmoov_description REQUIRED) diff --git a/inmoov_bringup/package.xml b/inmoov_bringup/package.xml index b6391e8..5d17a7b 100644 --- a/inmoov_bringup/package.xml +++ b/inmoov_bringup/package.xml @@ -14,7 +14,7 @@ Greg Perry ament_cmake - rclcpp + rclpy std_msgs inmoov_msgs inmoov_description diff --git a/inmoov_bringup/tools/enable_manager/enable_manager.py b/inmoov_bringup/tools/enable_manager/enable_manager.py index c97382a..4c4b75b 100755 --- a/inmoov_bringup/tools/enable_manager/enable_manager.py +++ b/inmoov_bringup/tools/enable_manager/enable_manager.py @@ -1,40 +1,31 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 import sys import os -import rospy -import rospkg +import rclpy +from rclpy.node import Node import random from python_qt_binding import loadUi -from python_qt_binding import QtGui from python_qt_binding.QtWidgets import QWidget - from PyQt5 import QtWidgets, QtCore, uic from PyQt5.QtWidgets import QWidget, QCheckBox, QApplication, QVBoxLayout -#from lookgui import Ui_MainWindow - -from inmoov_msgs.msg import MotorStatus -from inmoov_msgs.msg import MotorCommand +from inmoov_msgs.msg import MotorStatus, MotorCommand from inmoov_msgs.srv import MotorParameter from sensor_msgs.msg import JointState from std_msgs.msg import Header -import os -import sys from os.path import dirname, abspath -#hacky way to add include directory to sys path -sys.path.append(os.path.join(dirname(dirname(dirname(abspath(__file__)))),'include')) +# Hacky way to add include directory to sys path +sys.path.append(os.path.join(dirname(dirname(dirname(abspath(__file__)))), 'include')) from constants import PROTOCOL from servos import Servo from load_config_from_param import load_config_from_param -from time import sleep - # https://github.com/ColinDuquesnoy/QDarkStyleSheet # import qdarkstyle @@ -42,58 +33,48 @@ gui = os.path.join(os.path.dirname(__file__), 'enable_manager.ui') form_class = uic.loadUiType(gui)[0] -# https://nikolak.com/pyqt-qt-designer-getting-started/ -#class ExampleApp(QtGui.QMainWindow, Ui_MainWindow): -class ExampleApp(QtWidgets.QMainWindow, form_class): +class EnableManager(Node, QtWidgets.QMainWindow, form_class): def __init__(self): - # Explaining super is out of the scope of this article - # So please google it if you're not familar with it - # Simple reason why we use it here is that it allows us to - # access variables, methods etc in the design.py file - super(self.__class__, self).__init__() - self.setupUi(self) # This is defined in design.py file automatically - # It sets up layout and widgets that are defined + rclpy.init(args=None) + Node.__init__(self, 'enable_manager') + QtWidgets.QMainWindow.__init__(self) + self.setupUi(self) self.servos = load_config_from_param() self.bus = {} - self.checkboxes = {} self.motorcommand = MotorCommand() self.jointcommand = JointState() self.jointNames = [] - - rospy.init_node('enable_manager', anonymous=True) print("INITIALIZED") - for j,b in rospy.get_param('/joints').items(): - - number = rospy.get_param('/joints/' + j + '/bus') - busname = '/servobus/' + str(number).zfill(2) + '/motorcommand' + joints = self.get_parameter('/joints').get_parameter_value().string_value - if number not in self.bus: - self.bus[number] = rospy.Publisher(busname, MotorCommand, queue_size=40) - rospy.loginfo('adding: ' + busname) + for j, b in joints.items(): + number = self.get_parameter(f'/joints/{j}/bus').get_parameter_value().integer_value + busname = f'/servobus/{str(number).zfill(2)}/motorcommand' - self.jointPublisher = rospy.Publisher("joint_command", JointState, queue_size=40) + if number not in self.bus: + self.bus[number] = self.create_publisher(MotorCommand, busname, 40) + self.get_logger().info(f'adding: {busname}') - self.statusSubscriber = rospy.Subscriber("motor_status", MotorStatus, self.statusListener) + self.jointPublisher = self.create_publisher(JointState, "joint_command", 40) + self.statusSubscriber = self.create_subscription(MotorStatus, "motor_status", self.statusListener, 10) self.btnEnableAll.clicked.connect(self.setEnableAll) self.btnDisableAll.clicked.connect(self.setDisableAll) print("INIT COMPLETE") - joints = rospy.get_param('/joints') - for name, s in self.servos.items(): + for name, s in self.servos.items(): print(name) chk = QCheckBox(name) chk.setText(name) chk.setStyleSheet(checkboxstylesheet) - #chk.setEnabled(False) chk.stateChanged.connect(self.checkChanged) self.layout.addWidget(chk) self.checkboxes[name] = chk @@ -104,7 +85,6 @@ def statusListener(self, s): chk.blockSignals(True) chk.setChecked(s.enabled) chk.blockSignals(False) - return def checkChanged(self): sender = self.sender() @@ -117,42 +97,22 @@ def checkChanged(self): motorcommand.id = int(s.servoPin) motorcommand.parameter = PROTOCOL.ENABLE motorcommand.value = sender.isChecked() - #if chk.isChecked(): - # motorcommand.value = True - ##else: - # motorcommand.value = False - #motorcommand.value = float(chk.isChecked()) - print('checkChanged: ' + sender.text() + 'to: ' + str(sender.isChecked())) + print(f'checkChanged: {sender.text()} to: {str(sender.isChecked())}') self.bus[s.bus].publish(motorcommand) def setEnableAll(self): - - #self.statusSubscriber.unregister() - - #disconnect events - #for j,chk in self.checkboxes.items(): - # chk.disconnect() - - for j,s in self.servos.items(): - - + for j, s in self.servos.items(): motorcommand = MotorCommand() motorcommand.id = int(s.servoPin) motorcommand.parameter = PROTOCOL.ENABLE motorcommand.value = 1 - #print(str(j)) - self.bus[s.bus].publish(motorcommand) - #rospy.sleep(0.1) - - #self.statusSubscriber = rospy.Subscriber("motor_status", MotorStatus, self.statusListener) def setDisableAll(self): - for j,s in self.servos.items(): - + for j, s in self.servos.items(): motorcommand = MotorCommand() motorcommand.id = int(s.servoPin) motorcommand.parameter = PROTOCOL.ENABLE @@ -161,35 +121,37 @@ def setDisableAll(self): print(j) self.bus[s.bus].publish(motorcommand) - #rospy.sleep(0.1) - def closeEvent(self, event): self.enabled = False self.random = False print("GOODBYE!") -checkboxstylesheet = \ - 'QCheckBox::indicator {' + \ - 'width: 12px;' + \ - 'height: 12px;' + \ - 'border-style: outset;' + \ - 'border-width: 1px;' + \ - 'border-radius: 4px;' + \ - 'border-color: rgb(125, 125, 125);' + \ - 'border: 1px solid rgb(0,0,0);' + \ - 'background-color: rgb(130, 7, 7);' + \ - '}' + \ - 'QCheckBox::indicator:unchecked {background-color: rgb(130, 7, 7);}' + \ + +checkboxstylesheet = ( + 'QCheckBox::indicator {' + 'width: 12px;' + 'height: 12px;' + 'border-style: outset;' + 'border-width: 1px;' + 'border-radius: 4px;' + 'border-color: rgb(125, 125, 125);' + 'border: 1px solid rgb(0,0,0);' + 'background-color: rgb(130, 7, 7);' + '}' + 'QCheckBox::indicator:unchecked {background-color: rgb(130, 7, 7);}' 'QCheckBox::indicator:checked {background-color: rgb(11, 145, 1);}' +) + def main(): - app = QtWidgets.QApplication(sys.argv) # A new instance of QApplication - # app.setStyleSheet(qdarkstyle.load_stylesheet_pyqt5()) - form = ExampleApp() # We set the form to be our ExampleApp (design) - form.show() # Show the form - app.exec_() # and execute the app + app = QtWidgets.QApplication(sys.argv) + form = EnableManager() + form.show() + rclpy.spin(form) + app.exec_() + rclpy.shutdown() + -if __name__ == '__main__': # if we're running file directly and not importing it +if __name__ == '__main__': main() - diff --git a/inmoov_bringup/tools/rviz_manager/rviz_manager.py b/inmoov_bringup/tools/rviz_manager/rviz_manager.py index 6799eea..bdfa99d 100755 --- a/inmoov_bringup/tools/rviz_manager/rviz_manager.py +++ b/inmoov_bringup/tools/rviz_manager/rviz_manager.py @@ -1,11 +1,10 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # licensed under BSD-3 +import rclpy +from rclpy.node import Node -import rospy - -from inmoov_msgs.msg import MotorStatus -from inmoov_msgs.msg import MotorCommand +from inmoov_msgs.msg import MotorStatus, MotorCommand from inmoov_msgs.srv import MotorParameter from sensor_msgs.msg import JointState from std_msgs.msg import Header @@ -20,21 +19,15 @@ from threading import Thread from python_qt_binding import loadUi -from python_qt_binding import QtGui from python_qt_binding.QtWidgets import QWidget -#hacky way to add include directory to sys path +# Hacky way to add include directory to sys path sys.path.append(os.path.join(dirname(dirname(dirname(abspath(__file__)))),'include')) from constants import PROTOCOL from servos import Servo from load_config_from_param import load_config_from_param -servos = {} # servo configuration data for robot -joints = {} # dict of joint names and position values - -bus = {} # dict of motorcommand busses indexed by ordinal - PI = 3.1415926539 # https://github.com/ColinDuquesnoy/QDarkStyleSheet @@ -44,57 +37,57 @@ gui = os.path.join(os.path.dirname(__file__), 'rviz_manager.ui') form_class = uic.loadUiType(gui)[0] -def commandDispatcher(data): - - # if robot set up with degrees, then convert to radians - if rospy.get_param('/bringup/angles') == 'degrees': - p = list(data.position) - for x in range(0, len(p)): - p[x] = PI * p[x] / 180.0 - - data.position = tuple(p) - - publisher.publish(data) - -def stateDispatcher(data): - - # if robot set up with degrees, then convert to radians - if rospy.get_param('/bringup/angles') == 'degrees': - p = list(data.position) - for x in range(0, len(p)): - p[x] = PI * p[x] / 180.0 - - data.position = tuple(p) - - publisher.publish(data) - -subscriber1 = rospy.Subscriber("joint_command", JointState, commandDispatcher) -subscriber2 = rospy.Subscriber("joint_state", JointState, stateDispatcher) -publisher = rospy.Publisher('rviz_command',JointState, queue_size=10) - -def init(): - - rospy.init_node('rviz_command_dispatcher', anonymous=False) - #rate = rospy.Rate(20) # 20hz - - load_config_from_param() - - #for j,b in rospy.get_param('/joints').items(): - # - # number = rospy.get_param('/joints/' + j + '/bus') - # busname = '/servobus/' + str(number).zfill(2) + '/motorcommand' - - # if not bus.has_key(number): - # bus[number] = rospy.Publisher(busname, MotorCommand, queue_size=10) - # rospy.loginfo('adding: ' + busname) - - - rospy.spin() - +class RVizManager(Node): + def __init__(self): + super().__init__('rviz_command_dispatcher') + + # Subscribers + self.subscriber1 = self.create_subscription( + JointState, + 'joint_command', + self.command_dispatcher, + 10 + ) + self.subscriber2 = self.create_subscription( + JointState, + 'joint_state', + self.state_dispatcher, + 10 + ) + + # Publisher + self.publisher = self.create_publisher(JointState, 'rviz_command', 10) + + load_config_from_param() + + def command_dispatcher(self, data): + if self.get_parameter('bringup.angles').get_parameter_value().string_value == 'degrees': + p = list(data.position) + for x in range(len(p)): + p[x] = PI * p[x] / 180.0 + data.position = tuple(p) + self.publisher.publish(data) + + def state_dispatcher(self, data): + if self.get_parameter('bringup.angles').get_parameter_value().string_value == 'degrees': + p = list(data.position) + for x in range(len(p)): + p[x] = PI * p[x] / 180.0 + data.position = tuple(p) + self.publisher.publish(data) + + +def main(args=None): + rclpy.init(args=args) + node = RVizManager() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() if __name__ == '__main__': - try: - init() - except rospy.ROSInterruptException: - pass \ No newline at end of file + main() From bbd44d2a5a37f3abeccc4058def26d8f51d9e38a Mon Sep 17 00:00:00 2001 From: supertick Date: Thu, 16 May 2024 07:00:40 -0700 Subject: [PATCH 06/14] wip --- .../nodes/joint_status_dispatcher.py | 108 ++++++++---------- 1 file changed, 48 insertions(+), 60 deletions(-) diff --git a/inmoov_bringup/nodes/joint_status_dispatcher.py b/inmoov_bringup/nodes/joint_status_dispatcher.py index e0a0d90..6205141 100755 --- a/inmoov_bringup/nodes/joint_status_dispatcher.py +++ b/inmoov_bringup/nodes/joint_status_dispatcher.py @@ -1,11 +1,10 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # licensed under BSD-3 +import rclpy +from rclpy.node import Node -import rospy - -from inmoov_msgs.msg import MotorStatus -from inmoov_msgs.msg import MotorCommand +from inmoov_msgs.msg import MotorStatus, MotorCommand from inmoov_msgs.srv import MotorParameter from sensor_msgs.msg import JointState from std_msgs.msg import Header @@ -14,87 +13,76 @@ import sys from os.path import dirname, abspath -#hacky way to add include directory to sys path +# hacky way to add include directory to sys path sys.path.append(os.path.join(dirname(dirname(abspath(__file__))),'include')) from constants import PROTOCOL from servos import Servo from load_config_from_param import load_config_from_param -servos = {} -lookup = {} - -joints = {} -bus = {} - -state = {} +class JointStatusDispatcher(Node): -jointstatus = JointState() + def __init__(self): + super().__init__('joint_status_dispatcher') + self.servos = load_config_from_param() + self.lookup = {} + self.joints = {} + self.bus = {} -def init(): - - rospy.init_node('joint_status_dispatcher', anonymous=False) - rate = rospy.Rate(40) # 40hz + for n, s in self.servos.items(): + key = ((int(s.bus) * 255) + int(s.servoPin)) + self.lookup[key] = n + print(f'key: {key}') - servos = load_config_from_param() + self.publisher = self.create_publisher(JointState, "joint_status", 10) - #now, load lookup name by (bus*255+servo id) - for n,s in servos.items(): - key = ((int(s.bus)*255)+int(s.servoPin)) - lookup[key] = n - print('key: ' + str(key)) + joints_param = self.get_parameter_or('joints', {}).get_parameter_value().string_value + for j, b in joints_param.items(): + number = self.get_parameter_or(f'joints/{j}/bus', 0).get_parameter_value().integer_value + busname = f'/servobus/{str(number).zfill(2)}/motorstatus' - publisher = rospy.Publisher("joint_status", JointState, queue_size=10) + if number not in self.bus: + self.bus[number] = self.create_subscription( + MotorStatus, busname, lambda msg, num=number: self.dispatcher(msg, num), 10) + self.get_logger().info(f'adding: {busname}') - for j,b in rospy.get_param('/joints').items(): - - #create motorstatus bus name - number = rospy.get_param('/joints/' + j + '/bus') - busname = '/servobus/' + str(number).zfill(2) + '/motorstatus' + self.timer = self.create_timer(0.025, self.publish_joint_status) - # and if it's not already in the bus{}, then add it - # (not sure if the check is required) - # if not bus.has_key(number): - if number not in bus: - bus[number] = rospy.Subscriber(busname, MotorStatus, dispatcher, (number)) - rospy.loginfo('adding: ' + busname) + def dispatcher(self, data, bus): + try: + key = self.lookup[((int(bus) * 255) + int(data.id))] + self.joints[key] = data.position + except KeyError: + self.get_logger().warn(f'joint_status_dispatcher: unknown servo at bus:{bus} servo:{data.id}') - while not rospy.is_shutdown(): - - #jointstatus = JointState() + def publish_joint_status(self): + jointstatus = JointState() jointstatus.header = Header() - jointstatus.header.stamp = rospy.Time.now() + jointstatus.header.stamp = self.get_clock().now().to_msg() jointstatus.name = [] jointstatus.position = [] jointstatus.velocity = [] jointstatus.effort = [] - - for j,p in joints.items(): + for j, p in self.joints.items(): jointstatus.name.append(j) jointstatus.position.append(p) if len(jointstatus.name) > 0: - publisher.publish(jointstatus) + self.publisher.publish(jointstatus) - joints.clear() - rate.sleep() + self.joints.clear() - rospy.spin() - -def dispatcher(data, bus): +def main(args=None): + rclpy.init(args=args) + joint_status_dispatcher = JointStatusDispatcher() try: - #print("OHAI! bus:" + str(bus) + " servo:" + str(data.id)) - key = lookup[((int(bus)*255)+int(data.id))] - joints[key] = data.position - except: - rospy.logwarn('joint_status_dispatcher: unknown servo at bus:'+str(bus)+' servo:'+str(data.id)) - - - + rclpy.spin(joint_status_dispatcher) + except KeyboardInterrupt: + pass + finally: + joint_status_dispatcher.destroy_node() + rclpy.shutdown() if __name__ == '__main__': - try: - init() - except rospy.ROSInterruptException: - pass \ No newline at end of file + main() From a1d8a0a4f16d8469f09ed96c89c0863cdd981512 Mon Sep 17 00:00:00 2001 From: supertick Date: Thu, 16 May 2024 07:07:50 -0700 Subject: [PATCH 07/14] wip --- .../nodes/joint_command_dispatcher.py | 102 +++++----- .../nodes/joint_status_dispatcher.py | 2 +- .../nodes/motor_status_dispatcher.py | 96 +++------- inmoov_bringup/package.xml | 1 + .../tools/rviz_manager/rviz_manager.py | 181 ++++++++++++------ 5 files changed, 196 insertions(+), 186 deletions(-) diff --git a/inmoov_bringup/nodes/joint_command_dispatcher.py b/inmoov_bringup/nodes/joint_command_dispatcher.py index a850350..7fccc69 100755 --- a/inmoov_bringup/nodes/joint_command_dispatcher.py +++ b/inmoov_bringup/nodes/joint_command_dispatcher.py @@ -1,11 +1,10 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # licensed under BSD-3 +import rclpy +from rclpy.node import Node -import rospy - -from inmoov_msgs.msg import MotorStatus -from inmoov_msgs.msg import MotorCommand +from inmoov_msgs.msg import MotorStatus, MotorCommand from inmoov_msgs.srv import MotorParameter from sensor_msgs.msg import JointState from std_msgs.msg import Header @@ -14,73 +13,68 @@ import sys from os.path import dirname, abspath -#hacky way to add include directory to sys path +# hacky way to add include directory to sys path sys.path.append(os.path.join(dirname(dirname(abspath(__file__))),'include')) from constants import PROTOCOL from servos import Servo from load_config_from_param import load_config_from_param -servos = {} # servo configuration data for robot -joints = {} # dict of joint names and position values - -bus = {} # dict of motorcommand busses indexed by ordinal +class JointCommandDispatcher(Node): -def init(): + def __init__(self): + super().__init__('joint_command_dispatcher') + self.servos = load_config_from_param() + self.joints = {} # dict of joint names and position values + self.bus = {} # dict of motorcommand busses indexed by ordinal - rospy.init_node('joint_command_dispatcher', anonymous=False) - rate = rospy.Rate(20) # 40hz + self.subscription = self.create_subscription( + JointState, + 'joint_command', + self.dispatcher, + 10 + ) - rospy.Subscriber("joint_command", JointState, dispatcher) + joints_param = self.get_parameter_or('joints', {}).get_parameter_value().string_value + for j, b in joints_param.items(): + number = self.get_parameter_or(f'joints/{j}/bus', 0).get_parameter_value().integer_value + busname = f'/servobus/{str(number).zfill(2)}/motorcommand' - servos = load_config_from_param() - - for j,b in rospy.get_param('/joints').items(): - - number = rospy.get_param('/joints/' + j + '/bus') - busname = '/servobus/' + str(number).zfill(2) + '/motorcommand' + if number not in self.bus: + self.bus[number] = self.create_publisher(MotorCommand, busname, 10) + self.get_logger().info(f'adding: {busname}') - # if not bus.has_key(number): - if number not in bus: - bus[number] = rospy.Publisher(busname, MotorCommand, queue_size=10) - rospy.loginfo('adding: ' + busname) + self.timer = self.create_timer(0.05, self.publish_motor_commands) - while not rospy.is_shutdown(): - - #iterate through joints and publish - for j,p in joints.items(): + def dispatcher(self, js): + for x in range(len(js.name)): + self.joints[js.name[x]] = js.position[x] + print("YATZEE") + def publish_motor_commands(self): + for j, p in self.joints.items(): try: motorcommand = MotorCommand() - motorcommand.id = int(servos[j].servoPin) + motorcommand.id = int(self.servos[j].servoPin) motorcommand.parameter = PROTOCOL.GOAL motorcommand.value = p - bus[servos[j].bus].publish(motorcommand) - except: - rospy.logwarn('joint_command_dispatcher: unknown joint:' + j) - - - - #clear joints cache - joints.clear() - rate.sleep() - - rospy.spin() - -def dispatcher(js): - #print("OHAI!") - - # iterate through array and stuff name + position into dict object - for x in range(0, len(js.name)): - joints[js.name[x]] = js.position[x] - print("YATZEE") - - + self.bus[self.servos[j].bus].publish(motorcommand) + except KeyError: + self.get_logger().warn(f'joint_command_dispatcher: unknown joint: {j}') + + self.joints.clear() +def main(args=None): + rclpy.init(args=args) + joint_command_dispatcher = JointCommandDispatcher() + try: + rclpy.spin(joint_command_dispatcher) + except KeyboardInterrupt: + pass + finally: + joint_command_dispatcher.destroy_node() + rclpy.shutdown() if __name__ == '__main__': - try: - init() - except rospy.ROSInterruptException: - pass \ No newline at end of file + main() diff --git a/inmoov_bringup/nodes/joint_status_dispatcher.py b/inmoov_bringup/nodes/joint_status_dispatcher.py index 6205141..6dc7ef9 100755 --- a/inmoov_bringup/nodes/joint_status_dispatcher.py +++ b/inmoov_bringup/nodes/joint_status_dispatcher.py @@ -69,7 +69,7 @@ def publish_joint_status(self): jointstatus.position.append(p) if len(jointstatus.name) > 0: - self.publisher.publish(jointstatus) + self.publisher.publish(jointstatus) self.joints.clear() diff --git a/inmoov_bringup/nodes/motor_status_dispatcher.py b/inmoov_bringup/nodes/motor_status_dispatcher.py index f29619d..f69fc11 100755 --- a/inmoov_bringup/nodes/motor_status_dispatcher.py +++ b/inmoov_bringup/nodes/motor_status_dispatcher.py @@ -1,80 +1,34 @@ -#!/usr/bin/env python -# licensed under BSD-3 - - -import rospy +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node from inmoov_msgs.msg import MotorStatus -from std_msgs.msg import Header - -import os -import sys -from os.path import dirname, abspath - -#hacky way to add include directory to sys path -sys.path.append(os.path.join(dirname(dirname(abspath(__file__))),'include')) - -from constants import PROTOCOL -from servos import Servo -from load_config_from_param import load_config_from_param - -servos = {} -lookup = {} - -joints = {} -bus = {} - -state = {} +class MotorStatusDispatcher(Node): -publisher = rospy.Publisher("motor_status", MotorStatus, queue_size=10) + def __init__(self): + super().__init__('motor_status_dispatcher') + self.subscription = self.create_subscription( + MotorStatus, + 'motor_status', + self.motor_status_callback, + 10 + ) + self.subscription # prevent unused variable warning -def init(): - - rospy.init_node('motor_status_dispatcher', anonymous=False) - rate = rospy.Rate(40) # 40hz - - servos = load_config_from_param() - - #now, load lookup name by (bus*255+servo id) - for n,s in servos.items(): - try: - key = ((int(s.bus)*255)+int(s.servoPin)) - except: - rospy.logwarn('motor_status_dispatcher: unknown servo at bus:'+str(s.bus)+' servo:'+str(s.servoPin)) - - lookup[key] = n - print('key: ' + str(key)) - - #publisher = rospy.Publisher("motor_status", MotorStatus, queue_size=10) - - for j,b in rospy.get_param('/joints').items(): - - #create motorstatus bus name - number = rospy.get_param('/joints/' + j + '/bus') - busname = '/servobus/' + str(number).zfill(2) + '/motorstatus' - - # and if it's not already in the bus{}, then add it - # (not sure if the check is required) - if number not in bus: - bus[number] = rospy.Subscriber(busname, MotorStatus, dispatcher, (number)) - rospy.loginfo('adding: ' + busname) - - rospy.spin() - -def dispatcher(data, bus): + def motor_status_callback(self, msg): + self.get_logger().info('Received motor status: %s' % msg) +def main(args=None): + rclpy.init(args=args) + motor_status_dispatcher = MotorStatusDispatcher() try: - jointname = lookup[((int(bus)*255)+int(data.id))] - data.joint = jointname - data.bus = bus - publisher.publish(data) - except: - rospy.logwarn('motor_status_dispatcher: unknown servo at bus:'+str(bus)+' servo:'+str(data.id)) - + rclpy.spin(motor_status_dispatcher) + except KeyboardInterrupt: + pass + finally: + motor_status_dispatcher.destroy_node() + rclpy.shutdown() if __name__ == '__main__': - try: - init() - except rospy.ROSInterruptException: - pass \ No newline at end of file + main() diff --git a/inmoov_bringup/package.xml b/inmoov_bringup/package.xml index 5d17a7b..17c3990 100644 --- a/inmoov_bringup/package.xml +++ b/inmoov_bringup/package.xml @@ -20,6 +20,7 @@ inmoov_description robot_state_publisher rviz2 + python_qt_binding diff --git a/inmoov_bringup/tools/rviz_manager/rviz_manager.py b/inmoov_bringup/tools/rviz_manager/rviz_manager.py index bdfa99d..d9dc5dc 100755 --- a/inmoov_bringup/tools/rviz_manager/rviz_manager.py +++ b/inmoov_bringup/tools/rviz_manager/rviz_manager.py @@ -1,28 +1,26 @@ #!/usr/bin/env python3 # licensed under BSD-3 +import sys +import os import rclpy from rclpy.node import Node +from rclpy.parameter import Parameter from inmoov_msgs.msg import MotorStatus, MotorCommand from inmoov_msgs.srv import MotorParameter from sensor_msgs.msg import JointState from std_msgs.msg import Header -import os -import sys from os.path import dirname, abspath from PyQt5 import QtWidgets, QtCore, uic -from PyQt5.QtWidgets import QPushButton, QMessageBox - -from threading import Thread +from PyQt5.QtWidgets import QWidget, QCheckBox, QApplication, QVBoxLayout from python_qt_binding import loadUi -from python_qt_binding.QtWidgets import QWidget # Hacky way to add include directory to sys path -sys.path.append(os.path.join(dirname(dirname(dirname(abspath(__file__)))),'include')) +sys.path.append(os.path.join(dirname(dirname(dirname(abspath(__file__)))), 'include')) from constants import PROTOCOL from servos import Servo @@ -30,63 +28,126 @@ PI = 3.1415926539 -# https://github.com/ColinDuquesnoy/QDarkStyleSheet -# import qdarkstyle - -# https://www.safaribooksonline.com/blog/2014/01/22/create-basic-gui-using-pyqt/ gui = os.path.join(os.path.dirname(__file__), 'rviz_manager.ui') form_class = uic.loadUiType(gui)[0] -class RVizManager(Node): +class RVizManager(Node, QtWidgets.QMainWindow, form_class): def __init__(self): - super().__init__('rviz_command_dispatcher') - - # Subscribers - self.subscriber1 = self.create_subscription( - JointState, - 'joint_command', - self.command_dispatcher, - 10 - ) - self.subscriber2 = self.create_subscription( - JointState, - 'joint_state', - self.state_dispatcher, - 10 - ) - - # Publisher - self.publisher = self.create_publisher(JointState, 'rviz_command', 10) - - load_config_from_param() - - def command_dispatcher(self, data): - if self.get_parameter('bringup.angles').get_parameter_value().string_value == 'degrees': - p = list(data.position) - for x in range(len(p)): - p[x] = PI * p[x] / 180.0 - data.position = tuple(p) - self.publisher.publish(data) - - def state_dispatcher(self, data): - if self.get_parameter('bringup.angles').get_parameter_value().string_value == 'degrees': - p = list(data.position) - for x in range(len(p)): - p[x] = PI * p[x] / 180.0 - data.position = tuple(p) - self.publisher.publish(data) - - -def main(args=None): - rclpy.init(args=args) - node = RVizManager() - try: - rclpy.spin(node) - except KeyboardInterrupt: - pass - finally: - node.destroy_node() - rclpy.shutdown() + rclpy.init(args=None) + Node.__init__(self, 'rviz_command_dispatcher') + QtWidgets.QMainWindow.__init__(self) + self.setupUi(self) + + self.declare_parameter('bringup.angles', 'radians') + + self.servos = load_config_from_param() + + self.bus = {} + self.checkboxes = {} + + self.motorcommand = MotorCommand() + self.jointcommand = JointState() + + self.jointNames = [] + + print("INITIALIZED") + + joints = self.get_parameter('joints').get_parameter_value().string_value + + for j, b in joints.items(): + number = self.get_parameter(f'joints/{j}/bus').get_parameter_value().integer_value + busname = f'/servobus/{str(number).zfill(2)}/motorcommand' + + if number not in self.bus: + self.bus[number] = self.create_publisher(MotorCommand, busname, 40) + self.get_logger().info(f'adding: {busname}') + + self.jointPublisher = self.create_publisher(JointState, "joint_command", 40) + self.statusSubscriber = self.create_subscription(MotorStatus, "motor_status", self.statusListener, 10) + + self.btnEnableAll.clicked.connect(self.setEnableAll) + self.btnDisableAll.clicked.connect(self.setDisableAll) + + print("INIT COMPLETE") + + for name, s in self.servos.items(): + print(name) + chk = QCheckBox(name) + chk.setText(name) + chk.setStyleSheet(checkboxstylesheet) + chk.stateChanged.connect(self.checkChanged) + self.layout.addWidget(chk) + self.checkboxes[name] = chk + + def statusListener(self, s): + j = s.joint + chk = self.checkboxes[j] + chk.blockSignals(True) + chk.setChecked(s.enabled) + chk.blockSignals(False) + + def checkChanged(self): + sender = self.sender() + print(sender.text()) + + s = self.servos[sender.text()] + chk = self.checkboxes[sender.text()] + + motorcommand = MotorCommand() + motorcommand.id = int(s.servoPin) + motorcommand.parameter = PROTOCOL.ENABLE + motorcommand.value = sender.isChecked() + + print(f'checkChanged: {sender.text()} to: {str(sender.isChecked())}') + + self.bus[s.bus].publish(motorcommand) + + def setEnableAll(self): + for j, s in self.servos.items(): + motorcommand = MotorCommand() + motorcommand.id = int(s.servoPin) + motorcommand.parameter = PROTOCOL.ENABLE + motorcommand.value = 1 + self.bus[s.bus].publish(motorcommand) + + def setDisableAll(self): + for j, s in self.servos.items(): + motorcommand = MotorCommand() + motorcommand.id = int(s.servoPin) + motorcommand.parameter = PROTOCOL.ENABLE + motorcommand.value = 0 + print(j) + self.bus[s.bus].publish(motorcommand) + + def closeEvent(self, event): + self.enabled = False + self.random = False + print("GOODBYE!") + + +checkboxstylesheet = ( + 'QCheckBox::indicator {' + 'width: 12px;' + 'height: 12px;' + 'border-style: outset;' + 'border-width: 1px;' + 'border-radius: 4px;' + 'border-color: rgb(125, 125, 125);' + 'border: 1px solid rgb(0,0,0);' + 'background-color: rgb(130, 7, 7);' + '}' + 'QCheckBox::indicator:unchecked {background-color: rgb(130, 7, 7);}' + 'QCheckBox::indicator:checked {background-color: rgb(11, 145, 1);}' +) + + +def main(): + app = QtWidgets.QApplication(sys.argv) + form = RVizManager() + form.show() + rclpy.spin(form) + app.exec_() + rclpy.shutdown() if __name__ == '__main__': From 6f21594be9ab72aecd5653c952ee764bffe765cf Mon Sep 17 00:00:00 2001 From: supertick Date: Thu, 16 May 2024 07:11:09 -0700 Subject: [PATCH 08/14] wip --- inmoov_bringup/CMakeLists.txt | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/inmoov_bringup/CMakeLists.txt b/inmoov_bringup/CMakeLists.txt index 99855bf..51c4fff 100644 --- a/inmoov_bringup/CMakeLists.txt +++ b/inmoov_bringup/CMakeLists.txt @@ -10,6 +10,7 @@ find_package(robot_state_publisher REQUIRED) find_package(rviz2 REQUIRED) # find_package(micro_ros_agent REQUIRED) + install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}/launch ) @@ -18,7 +19,6 @@ install(DIRECTORY config DESTINATION share/${PROJECT_NAME}/config ) -# Install Python scripts and make them executable install(PROGRAMS nodes/joint_command_dispatcher.py nodes/joint_status_dispatcher.py @@ -28,4 +28,8 @@ install(PROGRAMS DESTINATION lib/${PROJECT_NAME} ) +install(DIRECTORY include + DESTINATION lib/${PROJECT_NAME} +) + ament_package() From 8b41111a4032d519235d6be13a839eb6f42a0bdf Mon Sep 17 00:00:00 2001 From: supertick Date: Thu, 16 May 2024 07:53:23 -0700 Subject: [PATCH 09/14] wip --- inmoov_bringup/CMakeLists.txt | 11 +++++++---- inmoov_bringup/{include => nodes}/constants.py | 0 inmoov_bringup/{include => nodes}/export_yaml.py | 0 inmoov_bringup/nodes/joint_command_dispatcher.py | 7 ------- inmoov_bringup/nodes/joint_status_dispatcher.py | 7 ------- .../{include => nodes}/load_config_from_param.py | 0 inmoov_bringup/{include => nodes}/servos.py | 0 7 files changed, 7 insertions(+), 18 deletions(-) rename inmoov_bringup/{include => nodes}/constants.py (100%) rename inmoov_bringup/{include => nodes}/export_yaml.py (100%) rename inmoov_bringup/{include => nodes}/load_config_from_param.py (100%) rename inmoov_bringup/{include => nodes}/servos.py (100%) diff --git a/inmoov_bringup/CMakeLists.txt b/inmoov_bringup/CMakeLists.txt index 51c4fff..149a423 100644 --- a/inmoov_bringup/CMakeLists.txt +++ b/inmoov_bringup/CMakeLists.txt @@ -1,6 +1,7 @@ cmake_minimum_required(VERSION 3.5) project(inmoov_bringup) +# Find dependencies find_package(ament_cmake REQUIRED) find_package(rclpy REQUIRED) find_package(std_msgs REQUIRED) @@ -11,25 +12,27 @@ find_package(rviz2 REQUIRED) # find_package(micro_ros_agent REQUIRED) +# Install launch files install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}/launch ) +# Install configuration files install(DIRECTORY config DESTINATION share/${PROJECT_NAME}/config ) +# Install Python nodes install(PROGRAMS nodes/joint_command_dispatcher.py nodes/joint_status_dispatcher.py nodes/motor_status_dispatcher.py + nodes/constants.py + nodes/servos.py + nodes/load_config_from_param.py tools/enable_manager/enable_manager.py tools/rviz_manager/rviz_manager.py DESTINATION lib/${PROJECT_NAME} ) -install(DIRECTORY include - DESTINATION lib/${PROJECT_NAME} -) - ament_package() diff --git a/inmoov_bringup/include/constants.py b/inmoov_bringup/nodes/constants.py similarity index 100% rename from inmoov_bringup/include/constants.py rename to inmoov_bringup/nodes/constants.py diff --git a/inmoov_bringup/include/export_yaml.py b/inmoov_bringup/nodes/export_yaml.py similarity index 100% rename from inmoov_bringup/include/export_yaml.py rename to inmoov_bringup/nodes/export_yaml.py diff --git a/inmoov_bringup/nodes/joint_command_dispatcher.py b/inmoov_bringup/nodes/joint_command_dispatcher.py index 7fccc69..ac1e056 100755 --- a/inmoov_bringup/nodes/joint_command_dispatcher.py +++ b/inmoov_bringup/nodes/joint_command_dispatcher.py @@ -9,13 +9,6 @@ from sensor_msgs.msg import JointState from std_msgs.msg import Header -import os -import sys -from os.path import dirname, abspath - -# hacky way to add include directory to sys path -sys.path.append(os.path.join(dirname(dirname(abspath(__file__))),'include')) - from constants import PROTOCOL from servos import Servo from load_config_from_param import load_config_from_param diff --git a/inmoov_bringup/nodes/joint_status_dispatcher.py b/inmoov_bringup/nodes/joint_status_dispatcher.py index 6dc7ef9..5b7a85a 100755 --- a/inmoov_bringup/nodes/joint_status_dispatcher.py +++ b/inmoov_bringup/nodes/joint_status_dispatcher.py @@ -9,13 +9,6 @@ from sensor_msgs.msg import JointState from std_msgs.msg import Header -import os -import sys -from os.path import dirname, abspath - -# hacky way to add include directory to sys path -sys.path.append(os.path.join(dirname(dirname(abspath(__file__))),'include')) - from constants import PROTOCOL from servos import Servo from load_config_from_param import load_config_from_param diff --git a/inmoov_bringup/include/load_config_from_param.py b/inmoov_bringup/nodes/load_config_from_param.py similarity index 100% rename from inmoov_bringup/include/load_config_from_param.py rename to inmoov_bringup/nodes/load_config_from_param.py diff --git a/inmoov_bringup/include/servos.py b/inmoov_bringup/nodes/servos.py similarity index 100% rename from inmoov_bringup/include/servos.py rename to inmoov_bringup/nodes/servos.py From bf10616a3de39db7bf90f768a70ef64828069427 Mon Sep 17 00:00:00 2001 From: supertick Date: Thu, 16 May 2024 08:47:57 -0700 Subject: [PATCH 10/14] wip --- inmoov_bringup/CMakeLists.txt | 16 +++- inmoov_bringup/nodes/export_yaml.py | 51 ++++++------- .../nodes/load_config_from_param.py | 50 ++++++------- .../nodes/motor_status_dispatcher.py | 75 +++++++++++-------- 4 files changed, 104 insertions(+), 88 deletions(-) diff --git a/inmoov_bringup/CMakeLists.txt b/inmoov_bringup/CMakeLists.txt index 149a423..0a6ab8b 100644 --- a/inmoov_bringup/CMakeLists.txt +++ b/inmoov_bringup/CMakeLists.txt @@ -9,8 +9,6 @@ find_package(inmoov_msgs REQUIRED) find_package(inmoov_description REQUIRED) find_package(robot_state_publisher REQUIRED) find_package(rviz2 REQUIRED) -# find_package(micro_ros_agent REQUIRED) - # Install launch files install(DIRECTORY launch @@ -30,9 +28,21 @@ install(PROGRAMS nodes/constants.py nodes/servos.py nodes/load_config_from_param.py + nodes/export_yaml.py + DESTINATION lib/${PROJECT_NAME} +) + +# Install Python tools and their .ui files +install(PROGRAMS tools/enable_manager/enable_manager.py tools/rviz_manager/rviz_manager.py - DESTINATION lib/${PROJECT_NAME} + DESTINATION lib/${PROJECT_NAME}/tools +) + +install(FILES + tools/enable_manager/enable_manager.ui + tools/rviz_manager/rviz_manager.ui + DESTINATION lib/${PROJECT_NAME}/tools ) ament_package() diff --git a/inmoov_bringup/nodes/export_yaml.py b/inmoov_bringup/nodes/export_yaml.py index d34036a..3fb19ac 100644 --- a/inmoov_bringup/nodes/export_yaml.py +++ b/inmoov_bringup/nodes/export_yaml.py @@ -1,64 +1,59 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # licensed as BSD-3 import sys -import rospy +import rclpy +from rclpy.node import Node import yaml import os from os.path import dirname, abspath -#hacky way to add include directory to sys path -sys.path.append(os.path.join(dirname(dirname(abspath(__file__))),'include')) - from constants import PROTOCOL from servos import Servo -def export_yaml(filename): - - outfile = os.path.join(dirname(dirname(abspath(__file__))),'config',filename) - +def export_yaml(node: Node, filename: str): + outfile = os.path.join(dirname(dirname(abspath(__file__))), 'config', filename) print(outfile) with open(outfile, 'w') as export: export.write('bringup:\n') - export.write(str(' angles:').ljust(20) + rospy.get_param('/bringup/angles')+ '\n') - export.write(str(' hz:').ljust(20) + str(rospy.get_param('/bringup/hz')) +'\n') + export.write(str(' angles:').ljust(20) + node.get_parameter('/bringup/angles').get_parameter_value().string_value + '\n') + export.write(str(' hz:').ljust(20) + str(node.get_parameter('/bringup/hz').get_parameter_value().integer_value) + '\n') export.write('\n') export.write('joints:\n') export.write('\n') - for name in rospy.get_param('/joints'): - print("updating yaml for: " + name) + joints_param = node.get_parameter('/joints').get_parameter_value().string_value + for name in joints_param: + print("updating yaml for: " + name) key = '/joints/' + name + '/' export.write(str(' ' + name + ':').ljust(20) + '\n') - val = rospy.get_param(key + 'bus') + val = node.get_parameter(key + 'bus').get_parameter_value().integer_value export.write(str(' bus:').ljust(20) + str(int(val)) + '\n') - val = rospy.get_param(key + 'servoPin') + val = node.get_parameter(key + 'servoPin').get_parameter_value().integer_value export.write(str(' servoPin:').ljust(20) + str(int(val)) + '\n') - val = rospy.get_param(key + 'minPulse') + val = node.get_parameter(key + 'minPulse').get_parameter_value().integer_value export.write(str(' minPulse:').ljust(20) + str(int(val)) + '\n') - val = rospy.get_param(key + 'maxPulse') + val = node.get_parameter(key + 'maxPulse').get_parameter_value().integer_value export.write(str(' maxPulse:').ljust(20) + str(int(val)) + '\n') - val = rospy.get_param(key + 'minGoal') + val = node.get_parameter(key + 'minGoal').get_parameter_value().integer_value export.write(str(' minGoal:').ljust(20) + str(val) + '\n') - val = rospy.get_param(key + 'maxGoal') + val = node.get_parameter(key + 'maxGoal').get_parameter_value().integer_value export.write(str(' maxGoal:').ljust(20) + str(val) + '\n') - val = rospy.get_param(key + 'rest') + val = node.get_parameter(key + 'rest').get_parameter_value().integer_value export.write(str(' rest:').ljust(20) + str(val) + '\n') - val = rospy.get_param(key + 'maxSpeed') + val = node.get_parameter(key + 'maxSpeed').get_parameter_value().integer_value export.write(str(' maxSpeed:').ljust(20) + str(val) + '\n') - val = rospy.get_param(key + 'smoothing') + val = node.get_parameter(key + 'smoothing').get_parameter_value().integer_value export.write(str(' smoothing:').ljust(20) + str(int(val)) + '\n') - val = rospy.get_param(key + 'sensorPin') + val = node.get_parameter(key + 'sensorPin').get_parameter_value().integer_value export.write(str(' sensorPin:').ljust(20) + str(int(val)) + '\n') - val = rospy.get_param(key + 'minSensor') + val = node.get_parameter(key + 'minSensor').get_parameter_value().integer_value export.write(str(' minSensor:').ljust(20) + str(int(val)) + '\n') - val = rospy.get_param(key + 'maxSensor') + val = node.get_parameter(key + 'maxSensor').get_parameter_value().integer_value export.write(str(' maxSensor:').ljust(20) + str(int(val)) + '\n') export.write('\n') - export.close() - - print("DONE") \ No newline at end of file + print("DONE") diff --git a/inmoov_bringup/nodes/load_config_from_param.py b/inmoov_bringup/nodes/load_config_from_param.py index f91e50c..555d5b6 100644 --- a/inmoov_bringup/nodes/load_config_from_param.py +++ b/inmoov_bringup/nodes/load_config_from_param.py @@ -1,52 +1,48 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # licensed under BSD-3 import sys -import rospy +import rclpy +from rclpy.node import Node import yaml import os from os.path import dirname, abspath - -#hacky way to add include directory to sys path -sys.path.append(os.path.join(dirname(dirname(abspath(__file__))),'include')) - from constants import PROTOCOL from servos import Servo servos = {} -def load_config_from_param(): +def load_config_from_param(node: Node): # first, make sure parameter server is even loaded - while not rospy.search_param("/joints"): - rospy.loginfo("waiting for parameter server to load with joint definitions") - rospy.sleep(1) + while not node.has_parameter("/joints"): + node.get_logger().info("waiting for parameter server to load with joint definitions") + node.create_rate(1).sleep() - rospy.sleep(1) + node.create_rate(1).sleep() - joints = rospy.get_param('/joints') + joints = node.get_parameter("/joints").get_parameter_value().string_value for name in joints: - rospy.loginfo( "found: " + name ) + node.get_logger().info("found: " + name) s = Servo() key = '/joints/' + name + '/' - s.bus = rospy.get_param(key + 'bus') - - s.servoPin = rospy.get_param(key + 'servoPin') - s.minPulse = rospy.get_param(key + 'minPulse') - s.maxPulse = rospy.get_param(key + 'maxPulse') - s.minGoal = rospy.get_param(key + 'minGoal') - s.maxGoal = rospy.get_param(key + 'maxGoal') - s.rest = rospy.get_param(key + 'rest') - s.maxSpeed = rospy.get_param(key + 'maxSpeed') - s.smoothing = rospy.get_param(key + 'smoothing') - - s.sensorpin = rospy.get_param(key + 'sensorPin') - s.minSensor = rospy.get_param(key + 'minSensor') - s.maxSensor = rospy.get_param(key + 'maxSensor') + s.bus = node.get_parameter(key + 'bus').get_parameter_value().integer_value + s.servoPin = node.get_parameter(key + 'servoPin').get_parameter_value().integer_value + s.minPulse = node.get_parameter(key + 'minPulse').get_parameter_value().integer_value + s.maxPulse = node.get_parameter(key + 'maxPulse').get_parameter_value().integer_value + s.minGoal = node.get_parameter(key + 'minGoal').get_parameter_value().integer_value + s.maxGoal = node.get_parameter(key + 'maxGoal').get_parameter_value().integer_value + s.rest = node.get_parameter(key + 'rest').get_parameter_value().integer_value + s.maxSpeed = node.get_parameter(key + 'maxSpeed').get_parameter_value().integer_value + s.smoothing = node.get_parameter(key + 'smoothing').get_parameter_value().integer_value + + s.sensorpin = node.get_parameter(key + 'sensorPin').get_parameter_value().integer_value + s.minSensor = node.get_parameter(key + 'minSensor').get_parameter_value().integer_value + s.maxSensor = node.get_parameter(key + 'maxSensor').get_parameter_value().integer_value servos[name] = s diff --git a/inmoov_bringup/nodes/motor_status_dispatcher.py b/inmoov_bringup/nodes/motor_status_dispatcher.py index f69fc11..555d5b6 100755 --- a/inmoov_bringup/nodes/motor_status_dispatcher.py +++ b/inmoov_bringup/nodes/motor_status_dispatcher.py @@ -1,34 +1,49 @@ #!/usr/bin/env python3 +# licensed under BSD-3 +import sys import rclpy from rclpy.node import Node -from inmoov_msgs.msg import MotorStatus - -class MotorStatusDispatcher(Node): - - def __init__(self): - super().__init__('motor_status_dispatcher') - self.subscription = self.create_subscription( - MotorStatus, - 'motor_status', - self.motor_status_callback, - 10 - ) - self.subscription # prevent unused variable warning - - def motor_status_callback(self, msg): - self.get_logger().info('Received motor status: %s' % msg) - -def main(args=None): - rclpy.init(args=args) - motor_status_dispatcher = MotorStatusDispatcher() - try: - rclpy.spin(motor_status_dispatcher) - except KeyboardInterrupt: - pass - finally: - motor_status_dispatcher.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() +import yaml +import os +from os.path import dirname, abspath + +from constants import PROTOCOL +from servos import Servo + +servos = {} + +def load_config_from_param(node: Node): + + # first, make sure parameter server is even loaded + while not node.has_parameter("/joints"): + node.get_logger().info("waiting for parameter server to load with joint definitions") + node.create_rate(1).sleep() + + node.create_rate(1).sleep() + + joints = node.get_parameter("/joints").get_parameter_value().string_value + for name in joints: + node.get_logger().info("found: " + name) + + s = Servo() + + key = '/joints/' + name + '/' + + s.bus = node.get_parameter(key + 'bus').get_parameter_value().integer_value + s.servoPin = node.get_parameter(key + 'servoPin').get_parameter_value().integer_value + s.minPulse = node.get_parameter(key + 'minPulse').get_parameter_value().integer_value + s.maxPulse = node.get_parameter(key + 'maxPulse').get_parameter_value().integer_value + s.minGoal = node.get_parameter(key + 'minGoal').get_parameter_value().integer_value + s.maxGoal = node.get_parameter(key + 'maxGoal').get_parameter_value().integer_value + s.rest = node.get_parameter(key + 'rest').get_parameter_value().integer_value + s.maxSpeed = node.get_parameter(key + 'maxSpeed').get_parameter_value().integer_value + s.smoothing = node.get_parameter(key + 'smoothing').get_parameter_value().integer_value + + s.sensorpin = node.get_parameter(key + 'sensorPin').get_parameter_value().integer_value + s.minSensor = node.get_parameter(key + 'minSensor').get_parameter_value().integer_value + s.maxSensor = node.get_parameter(key + 'maxSensor').get_parameter_value().integer_value + + servos[name] = s + + return servos From 39ebba8dfd60a81588b71a9abbfd05ac1718548b Mon Sep 17 00:00:00 2001 From: supertick Date: Thu, 16 May 2024 16:10:20 -0700 Subject: [PATCH 11/14] wip --- build_and_source.sh | 7 +++++++ inmoov_bringup/nodes/joint_command_dispatcher.py | 2 +- inmoov_bringup/nodes/joint_status_dispatcher.py | 2 +- 3 files changed, 9 insertions(+), 2 deletions(-) create mode 100755 build_and_source.sh diff --git a/build_and_source.sh b/build_and_source.sh new file mode 100755 index 0000000..8332450 --- /dev/null +++ b/build_and_source.sh @@ -0,0 +1,7 @@ +#!/bin/bash +# build_and_source.sh + +cd ~/mrl/inmoov_ros +colcon build --packages-select inmoov_bringup +source install/setup.bash +ros2 launch inmoov_bringup bringup.launch.py diff --git a/inmoov_bringup/nodes/joint_command_dispatcher.py b/inmoov_bringup/nodes/joint_command_dispatcher.py index ac1e056..369b9ff 100755 --- a/inmoov_bringup/nodes/joint_command_dispatcher.py +++ b/inmoov_bringup/nodes/joint_command_dispatcher.py @@ -17,7 +17,7 @@ class JointCommandDispatcher(Node): def __init__(self): super().__init__('joint_command_dispatcher') - self.servos = load_config_from_param() + self.servos = load_config_from_param(self) self.joints = {} # dict of joint names and position values self.bus = {} # dict of motorcommand busses indexed by ordinal diff --git a/inmoov_bringup/nodes/joint_status_dispatcher.py b/inmoov_bringup/nodes/joint_status_dispatcher.py index 5b7a85a..b40589d 100755 --- a/inmoov_bringup/nodes/joint_status_dispatcher.py +++ b/inmoov_bringup/nodes/joint_status_dispatcher.py @@ -17,7 +17,7 @@ class JointStatusDispatcher(Node): def __init__(self): super().__init__('joint_status_dispatcher') - self.servos = load_config_from_param() + self.servos = load_config_from_param(self) self.lookup = {} self.joints = {} self.bus = {} From 31f6319e692e271e590b6ec1c6264608675bd9fa Mon Sep 17 00:00:00 2001 From: supertick Date: Thu, 16 May 2024 18:29:46 -0700 Subject: [PATCH 12/14] wip --- inmoov_bringup/CMakeLists.txt | 4 +- .../nodes/motor_status_dispatcher.py | 79 ++++++++++++------- .../tools/enable_manager/enable_manager.py | 9 +-- .../tools/rviz_manager/rviz_manager.py | 5 +- 4 files changed, 58 insertions(+), 39 deletions(-) diff --git a/inmoov_bringup/CMakeLists.txt b/inmoov_bringup/CMakeLists.txt index 0a6ab8b..eb9a916 100644 --- a/inmoov_bringup/CMakeLists.txt +++ b/inmoov_bringup/CMakeLists.txt @@ -36,13 +36,13 @@ install(PROGRAMS install(PROGRAMS tools/enable_manager/enable_manager.py tools/rviz_manager/rviz_manager.py - DESTINATION lib/${PROJECT_NAME}/tools + DESTINATION lib/${PROJECT_NAME} ) install(FILES tools/enable_manager/enable_manager.ui tools/rviz_manager/rviz_manager.ui - DESTINATION lib/${PROJECT_NAME}/tools + DESTINATION lib/${PROJECT_NAME} ) ament_package() diff --git a/inmoov_bringup/nodes/motor_status_dispatcher.py b/inmoov_bringup/nodes/motor_status_dispatcher.py index 555d5b6..124bc72 100755 --- a/inmoov_bringup/nodes/motor_status_dispatcher.py +++ b/inmoov_bringup/nodes/motor_status_dispatcher.py @@ -1,49 +1,70 @@ #!/usr/bin/env python3 # licensed under BSD-3 -import sys import rclpy from rclpy.node import Node -import yaml +from inmoov_msgs.msg import MotorStatus +from std_msgs.msg import Header import os from os.path import dirname, abspath +import sys + +# Hacky way to add include directory to sys path +sys.path.append(os.path.join(dirname(dirname(dirname(abspath(__file__)))), 'include')) from constants import PROTOCOL from servos import Servo +from load_config_from_param import load_config_from_param -servos = {} - -def load_config_from_param(node: Node): +class MotorStatusDispatcher(Node): + def __init__(self): + super().__init__('motor_status_dispatcher') + self.servos = load_config_from_param(self) - # first, make sure parameter server is even loaded - while not node.has_parameter("/joints"): - node.get_logger().info("waiting for parameter server to load with joint definitions") - node.create_rate(1).sleep() + self.lookup = {} + self.joints = {} + self.bus = {} - node.create_rate(1).sleep() + # Load lookup name by (bus*255+servo id) + for n, s in self.servos.items(): + try: + key = (int(s.bus) * 255) + int(s.servoPin) + self.lookup[key] = n + self.get_logger().info(f'key: {str(key)}') + except ValueError: + self.get_logger().warn(f'motor_status_dispatcher: unknown servo at bus: {str(s.bus)} servo: {str(s.servoPin)}') - joints = node.get_parameter("/joints").get_parameter_value().string_value - for name in joints: - node.get_logger().info("found: " + name) + self.publisher = self.create_publisher(MotorStatus, "motor_status", 10) - s = Servo() + for j, b in self.get_parameter('/joints').get_parameter_value().items(): + # Create motorstatus bus name + number = self.get_parameter(f'/joints/{j}/bus').get_parameter_value().integer_value + busname = f'/servobus/{str(number).zfill(2)}/motorstatus' - key = '/joints/' + name + '/' + if number not in self.bus: + self.bus[number] = self.create_subscription(MotorStatus, busname, lambda msg, bus=number: self.dispatcher(msg, bus), 10) + self.get_logger().info(f'adding: {busname}') - s.bus = node.get_parameter(key + 'bus').get_parameter_value().integer_value - s.servoPin = node.get_parameter(key + 'servoPin').get_parameter_value().integer_value - s.minPulse = node.get_parameter(key + 'minPulse').get_parameter_value().integer_value - s.maxPulse = node.get_parameter(key + 'maxPulse').get_parameter_value().integer_value - s.minGoal = node.get_parameter(key + 'minGoal').get_parameter_value().integer_value - s.maxGoal = node.get_parameter(key + 'maxGoal').get_parameter_value().integer_value - s.rest = node.get_parameter(key + 'rest').get_parameter_value().integer_value - s.maxSpeed = node.get_parameter(key + 'maxSpeed').get_parameter_value().integer_value - s.smoothing = node.get_parameter(key + 'smoothing').get_parameter_value().integer_value + def dispatcher(self, data, bus): + try: + jointname = self.lookup[(int(bus) * 255) + int(data.id)] + data.joint = jointname + data.bus = bus + self.publisher.publish(data) + except KeyError: + self.get_logger().warn(f'motor_status_dispatcher: unknown servo at bus: {str(bus)} servo: {str(data.id)}') - s.sensorpin = node.get_parameter(key + 'sensorPin').get_parameter_value().integer_value - s.minSensor = node.get_parameter(key + 'minSensor').get_parameter_value().integer_value - s.maxSensor = node.get_parameter(key + 'maxSensor').get_parameter_value().integer_value +def main(args=None): + rclpy.init(args=args) + node = MotorStatusDispatcher() - servos[name] = s + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() - return servos +if __name__ == '__main__': + main() diff --git a/inmoov_bringup/tools/enable_manager/enable_manager.py b/inmoov_bringup/tools/enable_manager/enable_manager.py index 4c4b75b..34ec181 100755 --- a/inmoov_bringup/tools/enable_manager/enable_manager.py +++ b/inmoov_bringup/tools/enable_manager/enable_manager.py @@ -26,11 +26,8 @@ from servos import Servo from load_config_from_param import load_config_from_param -# https://github.com/ColinDuquesnoy/QDarkStyleSheet -# import qdarkstyle - -# https://www.safaribooksonline.com/blog/2014/01/22/create-basic-gui-using-pyqt/ -gui = os.path.join(os.path.dirname(__file__), 'enable_manager.ui') +# Set up the GUI path +gui = os.path.join(dirname(abspath(__file__)), 'enable_manager.ui') form_class = uic.loadUiType(gui)[0] class EnableManager(Node, QtWidgets.QMainWindow, form_class): @@ -40,7 +37,7 @@ def __init__(self): QtWidgets.QMainWindow.__init__(self) self.setupUi(self) - self.servos = load_config_from_param() + self.servos = load_config_from_param(self) self.bus = {} self.checkboxes = {} diff --git a/inmoov_bringup/tools/rviz_manager/rviz_manager.py b/inmoov_bringup/tools/rviz_manager/rviz_manager.py index d9dc5dc..1bccb7a 100755 --- a/inmoov_bringup/tools/rviz_manager/rviz_manager.py +++ b/inmoov_bringup/tools/rviz_manager/rviz_manager.py @@ -28,7 +28,8 @@ PI = 3.1415926539 -gui = os.path.join(os.path.dirname(__file__), 'rviz_manager.ui') +# Set up the GUI path relative to the installed directory +gui = os.path.join(dirname(abspath(__file__)), 'rviz_manager.ui') form_class = uic.loadUiType(gui)[0] class RVizManager(Node, QtWidgets.QMainWindow, form_class): @@ -40,7 +41,7 @@ def __init__(self): self.declare_parameter('bringup.angles', 'radians') - self.servos = load_config_from_param() + self.servos = load_config_from_param(self) self.bus = {} self.checkboxes = {} From cfcecd6172319b520a917e4a919774a2dbe8b805 Mon Sep 17 00:00:00 2001 From: supertick Date: Thu, 16 May 2024 18:36:23 -0700 Subject: [PATCH 13/14] wip --- inmoov_bringup/launch/bringup.launch.py | 27 ++++++++++++------------- 1 file changed, 13 insertions(+), 14 deletions(-) diff --git a/inmoov_bringup/launch/bringup.launch.py b/inmoov_bringup/launch/bringup.launch.py index 5315424..24cc329 100644 --- a/inmoov_bringup/launch/bringup.launch.py +++ b/inmoov_bringup/launch/bringup.launch.py @@ -1,18 +1,16 @@ -import os -from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, GroupAction from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration def generate_launch_description(): - config_file = os.path.join(get_package_share_directory('inmoov_bringup'), 'config', 'config.yaml') + config_file = LaunchConfiguration('config_file', default='config/config.yaml') return LaunchDescription([ DeclareLaunchArgument( 'config_file', default_value=config_file, - description='Path to the config file' + description='Path to the config file.' ), # Commented out hardware-related nodes for later use @@ -53,34 +51,35 @@ def generate_launch_description(): package='inmoov_bringup', executable='joint_command_dispatcher.py', name='joint_command_dispatcher', - respawn=True + output='screen', + parameters=[config_file] ), - Node( package='inmoov_bringup', executable='joint_status_dispatcher.py', name='joint_status_dispatcher', - respawn=True + output='screen', + parameters=[config_file] ), - Node( package='inmoov_bringup', executable='motor_status_dispatcher.py', name='motor_status_dispatcher', - respawn=True + output='screen', + parameters=[config_file] ), - Node( package='inmoov_bringup', executable='enable_manager.py', name='enable_manager', - respawn=True + output='screen', + parameters=[config_file] ), - Node( package='inmoov_bringup', executable='rviz_manager.py', name='rviz_manager', - respawn=True + output='screen', + parameters=[config_file] ), ]) From 0fde85dbd1bff510399df411dafde86ae241f03d Mon Sep 17 00:00:00 2001 From: supertick Date: Thu, 16 May 2024 21:59:46 -0700 Subject: [PATCH 14/14] removed firmware --- build_and_source.sh | 5 +- inmoov_bringup/CMakeLists.txt | 2 +- .../urdf/config.joints.urdf.xacro | 2 +- inmoov_firmware/CMakeLists.txt | 181 ------- inmoov_firmware/LICENSE.txt | 29 - inmoov_firmware/TeensyServo.cpp | 499 ------------------ inmoov_firmware/TeensyServo.h | 105 ---- inmoov_firmware/configuration.h | 50 -- inmoov_firmware/eeprom.h | 36 -- inmoov_firmware/inmoov_firmware.ino | 305 ----------- inmoov_firmware/package.xml | 50 -- inmoov_firmware2/CMakeLists.txt | 181 ------- inmoov_firmware2/LICENSE.txt | 29 - inmoov_firmware2/configuration.h | 50 -- inmoov_firmware2/eeprom.h | 36 -- inmoov_firmware2/inmoov_firmware2.ino | 303 ----------- inmoov_firmware2/package.xml | 50 -- inmoov_meshes/CMakeLists.txt | 195 +------ inmoov_meshes/package.xml | 53 +- 19 files changed, 21 insertions(+), 2140 deletions(-) delete mode 100644 inmoov_firmware/CMakeLists.txt delete mode 100644 inmoov_firmware/LICENSE.txt delete mode 100755 inmoov_firmware/TeensyServo.cpp delete mode 100755 inmoov_firmware/TeensyServo.h delete mode 100755 inmoov_firmware/configuration.h delete mode 100644 inmoov_firmware/eeprom.h delete mode 100644 inmoov_firmware/inmoov_firmware.ino delete mode 100644 inmoov_firmware/package.xml delete mode 100644 inmoov_firmware2/CMakeLists.txt delete mode 100644 inmoov_firmware2/LICENSE.txt delete mode 100755 inmoov_firmware2/configuration.h delete mode 100644 inmoov_firmware2/eeprom.h delete mode 100644 inmoov_firmware2/inmoov_firmware2.ino delete mode 100644 inmoov_firmware2/package.xml diff --git a/build_and_source.sh b/build_and_source.sh index 8332450..1aaaad3 100755 --- a/build_and_source.sh +++ b/build_and_source.sh @@ -2,6 +2,7 @@ # build_and_source.sh cd ~/mrl/inmoov_ros -colcon build --packages-select inmoov_bringup +colcon build --packages-select inmoov_description source install/setup.bash -ros2 launch inmoov_bringup bringup.launch.py +# ros2 launch inmoov_bringup bringup.launch.py +ros2 launch inmoov_description display.launch.py diff --git a/inmoov_bringup/CMakeLists.txt b/inmoov_bringup/CMakeLists.txt index eb9a916..07ac9f2 100644 --- a/inmoov_bringup/CMakeLists.txt +++ b/inmoov_bringup/CMakeLists.txt @@ -17,7 +17,7 @@ install(DIRECTORY launch # Install configuration files install(DIRECTORY config - DESTINATION share/${PROJECT_NAME}/config + DESTINATION share/${PROJECT_NAME} ) # Install Python nodes diff --git a/inmoov_description/urdf/config.joints.urdf.xacro b/inmoov_description/urdf/config.joints.urdf.xacro index 1fc5b2b..d13cd76 100644 --- a/inmoov_description/urdf/config.joints.urdf.xacro +++ b/inmoov_description/urdf/config.joints.urdf.xacro @@ -3,7 +3,7 @@ - + diff --git a/inmoov_firmware/CMakeLists.txt b/inmoov_firmware/CMakeLists.txt deleted file mode 100644 index 538725f..0000000 --- a/inmoov_firmware/CMakeLists.txt +++ /dev/null @@ -1,181 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(inmoov_firmware) - -## 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 run_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 run_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 run_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 you 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 inmoov_firmware -# 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) - -## Declare a C++ library -# add_library(inmoov_firmware -# src/${PROJECT_NAME}/inmoov_firmware.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(inmoov_firmware ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -# add_executable(inmoov_firmware_node src/inmoov_firmware_node.cpp) - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(inmoov_firmware_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(inmoov_firmware_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 inmoov_firmware inmoov_firmware_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_inmoov_firmware.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/inmoov_firmware/LICENSE.txt b/inmoov_firmware/LICENSE.txt deleted file mode 100644 index db2f0e9..0000000 --- a/inmoov_firmware/LICENSE.txt +++ /dev/null @@ -1,29 +0,0 @@ -BSD 3-Clause License - -Copyright (c) 2017, Alan Timm (alansrobotlab) -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - -* Redistributions of source code must retain the above copyright notice, this - list of conditions and the following disclaimer. - -* Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - -* Neither the name of the copyright holder nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/inmoov_firmware/TeensyServo.cpp b/inmoov_firmware/TeensyServo.cpp deleted file mode 100755 index 1c03651..0000000 --- a/inmoov_firmware/TeensyServo.cpp +++ /dev/null @@ -1,499 +0,0 @@ -#include -#include -#include -#include "configuration.h" -#include "TeensyServo.h" -#include - - -TeensyServo::TeensyServo(int pin, int sensor) { - this->servoPin = pin; - this->sensorPin = sensor; - - pinMode(servoPin, OUTPUT); - pinMode(sensorPin, INPUT); - - readEeprom(servoPin); - - //setGoal(readPositionAngle()); - - moving = false; - enabled = 0; - - sampleStartMillis = millis(); - - -} - - -void TeensyServo::setGoal(float a) { - /* - goalAngle = a; - if (a > e.maxAngle) - goalAngle = e.maxAngle; - if (a < e.minAngle) - goalAngle = e.minAngle; - */ - - receivedCommand = true; //mark that we've received a position command - - // no guarantees the min max angles are mapped backwards - goalAngle = a; - if (e.minAngle < e.maxAngle) { - goalAngle = constrain(goalAngle, e.minAngle, e.maxAngle); - } - else { - goalAngle = constrain(goalAngle, e.maxAngle, e.minAngle); - } - - - //this->moveToMicroseconds((map(goalAngle, (int)(e.minAngle * 1000), (int)(e.maxAngle * 1000.0), e.minPulse, e.maxPulse) / 1000.0)); - - this->moveToMicroseconds((map(goalAngle * 1000, (int)(e.minAngle * 1000.0), (int)(e.maxAngle * 1000.0), e.minPulse, e.maxPulse))); - - //nh.loginfo("SetGoal!!!"); -} - - -float TeensyServo::getGoal() { - return goalAngle; -} - - -void TeensyServo::moveToMicroseconds(int microseconds) { - this->startMillis = millis(); - this->startPulse = this->readPositionPulse(); - //this->startPulse = commandPulse; - - - - if(e.minPulse < e.maxPulse) { - microseconds = constrain(microseconds,e.minPulse,e.maxPulse); - } - else { - microseconds = constrain(microseconds,e.maxPulse,e.minPulse); - } - - /* - if (microseconds > e.maxPulse) - microseconds = e.maxPulse; - if (microseconds < e.minPulse) - microseconds = e.minPulse; - */ - commandPulse = microseconds; - - deltaPulse = ((commandPulse - startPulse)); //*1000)/ticksPerSecond; - moveDuration = abs((deltaPulse * 1000) / ticksPerSecond); - - if (deltaPulse > 0) { - velocityArc = 0; - } - else { - velocityArc = 180; - } - - moving = true; -} - - - -short TeensyServo::readPositionRaw() { - /* - long int retval =0; - for (int i = 0; i < NUM_SAMPLES; i++) { - retval += analogRead(this->sensorPin); - } - retval /= NUM_SAMPLES; - - return retval; - */ - return this->position; - -} - - -#define NUM_SAMPLES 350 -void TeensyServo::updatePosition() { - for (int i = 0; i < 50; i++) { - sampleBucket += analogRead(this->sensorPin); - } - sampleCount += 50; - - if (sampleCount == NUM_SAMPLES) { - sampleCount = 0; - this->position = sampleBucket /= NUM_SAMPLES; - this->sampleDuration = millis() - this->sampleStartMillis; - this->sampleStartMillis = millis(); - } -} - -/* - #define NUM_READS 16 - short TeensyServo::readPositionRaw_old() { - // http://www.elcojacobs.com/eleminating-noise-from-sensor-readings-on-arduino-with-digital-filtering/ - // return analogRead(this->sensorPin); - // read multiple values and sort them to take the mode - int sortedValues[NUM_READS]; - for (int i = 0; i < NUM_READS; i++) { - int value = analogRead(this->sensorPin); - int j; - if (value < sortedValues[0] || i == 0) { - j = 0; //insert at first position - } - else { - for (j = 1; j < i; j++) { - if (sortedValues[j - 1] <= value && sortedValues[j] >= value) { - // j is insert position - break; - } - } - } - for (int k = i; k > j; k--) { - // move all values higher than current reading up one position - sortedValues[k] = sortedValues[k - 1]; - } - sortedValues[j] = value; //insert current reading - } - //return scaled mode of 10 values - float returnval = 0; - for (int i = NUM_READS / 2 - 5; i < (NUM_READS / 2 + 5); i++) { - returnval += sortedValues[i]; - } - returnval = (int)returnval / 10; - - return returnval * 1100 / 1023; - - - } -*/ - - -float TeensyServo::readPositionAngle() { - short p = readPositionRaw(); - - if (p < 100) - return 0; - else - return ((map(p, e.minSensor, e.maxSensor, (int)(e.minAngle * 1000.0), (int)(e.maxAngle * 1000.0))) / 1000.0); -} - -short TeensyServo::readPositionPulse() { - return map(this->readPositionRaw(), e.minSensor, e.maxSensor, e.minPulse, e.maxPulse); -} - -void TeensyServo::update() { - - ////Serial.println("Servo Update!"); - - this->updatePosition(); - - - deltaMillis = millis() - startMillis; - - switch (e.smooth) { - - case 0: // direct - case 1: // maxspeed (placeholder) - this->servo.writeMicroseconds(commandPulse); - break; - - case 2: // linear - if (commandPulse > startPulse) { - currentPulse = startPulse + (ticksPerSecond * this->deltaMillis) / 1000; - - if (currentPulse > commandPulse) - currentPulse = commandPulse; - } - if (commandPulse < startPulse) { - currentPulse = startPulse - (ticksPerSecond * this->deltaMillis) / 1000; - - if (this->currentPulse < this->commandPulse) - this->currentPulse = this->commandPulse; - } - servo.writeMicroseconds(currentPulse); - break; - - case 3: // calcCos1 - if (deltaPulse > 0) { - velocityArc = 0 + map(millis() - startMillis, 0, moveDuration, 0, 180); - if (velocityArc >= 180) { - velocityArc = 180; - moving = false; - } - - r = (cos(radians(velocityArc)) * 100); - currentPulse = map(r, 100, -100, startPulse, commandPulse); - } - if (deltaPulse < 0) { - velocityArc = 180 - map(millis() - startMillis, 0, moveDuration, 0, 180); - if (velocityArc <= 0) { - velocityArc = 0; - moving = false; - } - r = (cos(radians(velocityArc)) * 100); - currentPulse = map(r, -100, 100, startPulse, commandPulse); - } - if (deltaPulse == 0) { - moving = false; - } - servo.writeMicroseconds(currentPulse); - break; - - case 4: - if (deltaPulse > 0) { - velocityArc = 0 + map(millis() - startMillis, 0, moveDuration, 0, 180); - if (velocityArc >= 180) { - velocityArc = 180; - moving = false; - } - if (velocityArc <= 90) { - r = sqrt(cos(radians(velocityArc))) * 100; - currentPulse = map(r, 100, -100, startPulse, commandPulse); - } - else { - r = -sqrt(cos(radians(180 - velocityArc))) * 100; - currentPulse = map(r, 100, -100, startPulse, commandPulse); - } - } - if (deltaPulse < 0) { - velocityArc = 180 - map(millis() - startMillis, 0, moveDuration, 0, 180); - if (velocityArc <= 0) { - velocityArc = 0; - moving = false; - } - if (velocityArc <= 90) { - r = sqrt(cos(radians(velocityArc))) * 100; - currentPulse = map(r, -100, 100, startPulse, commandPulse); - } - else { - r = -sqrt(cos(radians(180 - velocityArc))) * 100; - currentPulse = map(r, -100, 100, startPulse, commandPulse); - } - } - if (deltaPulse == 0) { - moving = false; - } - servo.writeMicroseconds(currentPulse); - break; - moving = false; - } - -} - -/* - void TeensyServo::calibrate() { - smooth=2; - this->setGoal(e.minAngle); - while(moving==true) { - this->update(); - delay(1); - } - delay(1000); - e.minSensor=readPositionRaw(); - - this->setGoal(e.maxAngle); - while(moving==true) { - this->update(); - delay(1); - } - delay(1000); - e.maxSensor=readPositionRaw(); - e.calibrated=true; - smooth=0; - } -*/ - -void TeensyServo::setupADC() { - analogReadResolution(12); - analogReference(EXTERNAL); - //analogReadAveraging(8); -} - -void TeensyServo::setMinPulse(short minpulse) { - e.minPulse = minpulse; - writeEeprom(); - setEnabled(0); - //setEnabled(1); - e.calibrated = false; - moving = false; -} - -short TeensyServo::getMinPulse() { - return e.minPulse; - -} - -void TeensyServo::setMaxPulse(short maxpulse) { - e.maxPulse = maxpulse; - writeEeprom(); - setEnabled(0); - //setEnabled(1); - e.calibrated = false; - moving = false; -} - -short TeensyServo::getMaxPulse() { - return e.maxPulse; - -} - -void TeensyServo::setMinAngle(float minangle) { - e.minAngle = minangle; - e.calibrated = false; - writeEeprom(); - moving = false; -} - -float TeensyServo::getMinAngle() { - return e.minAngle; -} - -void TeensyServo::setMaxAngle(float maxAngle) { - e.maxAngle = maxAngle; - e.calibrated = false; - writeEeprom(); - moving = false; -} - -float TeensyServo::getMaxAngle() { - //return 180; - return e.maxAngle; -} - -void TeensyServo::setMinSensor(int minsensor) { - e.minSensor = minsensor; - e.calibrated = false; - writeEeprom(); - moving = false; -} - -int TeensyServo::getMinSensor() { - return e.minSensor; -} - -void TeensyServo::setMaxSensor(int maxsensor) { - e.maxSensor = maxsensor; - e.calibrated = false; - writeEeprom(); - moving = false; -} - -int TeensyServo::getMaxSensor() { - return e.maxSensor; -} - -void TeensyServo::setEnabled(bool val) { - - if (val != 0) { - // if we haven't, just readposition and use that instead. - if (receivedCommand == false) { - setGoal(readPositionAngle()); - } - - servo.attach(this->servoPin, e.minPulse, e.maxPulse); //,readPositionPulse()); - enabled = 1; - } - else { - servo.detach(); - enabled = 0; - } -} - -bool TeensyServo::getEnabled() { - return enabled; -} - - -bool TeensyServo::getCalibrated() { - return e.calibrated; -} - -void TeensyServo::setCalibrated(bool c) { - e.calibrated = c; - writeEeprom(); -} - -void TeensyServo::readEeprom(int s) { - // s is eeprom slot to read - // luckily the digital pins are numbered 0 - 11, so we can just use those - - int offset = EEPROM_START + (s * EEPROM_RECORD_SIZE); - EEPROM.get(offset, e); - - int newCheck = generateEepromChecksum(); - if (newCheck != e.checksum) - { - e.minPulse = MINPULSE; - e.maxPulse = MAXPULSE; - e.minAngle = MINANGLE; - e.maxAngle = MAXANGLE; - e.minSensor = MINSENSOR; - e.maxSensor = MAXSENSOR; - e.smooth = SMOOTH; - e.maxSpeed = MAXSPEED; - e.checksum = generateEepromChecksum(); - writeEeprom(); - } - -} - -void TeensyServo::writeEeprom() { - int offset = EEPROM_START + (servoPin * EEPROM_RECORD_SIZE); - e.checksum = generateEepromChecksum(); - EEPROM.put(offset, e); -} - - - -int TeensyServo::generateEepromChecksum() { - - return 14; -} - -void TeensyServo::setSmooth(byte s) { - e.smooth = s; - writeEeprom(); -} - -byte TeensyServo::getSmooth() { - return e.smooth; -} - -float TeensyServo::getMaxSpeed() { - return e.maxSpeed; -} - -void TeensyServo::setMaxSpeed(float s) { - e.maxSpeed = s; - writeEeprom(); -} - -float TeensyServo::readPresentSpeed() { - int speed; - speed = goalAngle - readPositionAngle(); - if (speed > 0) { - speed = 1; - } - if (speed < 0) { - speed = -1; - } - return speed; -} - -bool TeensyServo::getMoving() { - - if (abs(readPositionAngle() - goalAngle) < 2) - return 0; - else - return 1; - -} - -bool TeensyServo::getPower() { - if (readPositionRaw() > 100) - return 1; - else - return 0; -} - - diff --git a/inmoov_firmware/TeensyServo.h b/inmoov_firmware/TeensyServo.h deleted file mode 100755 index 78cdaba..0000000 --- a/inmoov_firmware/TeensyServo.h +++ /dev/null @@ -1,105 +0,0 @@ -#include -#include -#include "eeprom.h" -#include "configuration.h" -#include - -class TeensyServo { - private: - Eeprom e; - - void readEeprom(int); - void writeEeprom(); - int generateEepromChecksum(); - - float goalAngle; - - public: - int servoPin; - int sensorPin; - - int debugInt = 0; - float debugFloat = 0; - - - bool moving = false; - bool power = false; - int r; - int deltaPulse; - int velocityArc; - int moveDuration; - int enabled = 0; - - int startMillis; - int startPulse; - int commandPulse; - int currentPulse; - - int deltaMillis; - int ticksPerSecond = 1500; - - int position = 0; - int sampleCount = 0; - int sampleBucket = 0; - int sampleStartMillis = 0; - int sampleDuration = 0; - - bool receivedCommand = false; - - int i, j; - - Servo servo; - - TeensyServo(int, int); - - void setGoal(float); - float getGoal(); - - void moveToMicroseconds(int); - short readPositionRaw(); - float readPositionAngle(); - short readPositionPulse(); - - void update(); - - void calibrate(); - - void setupADC(); - - void setMinPulse(short); - short getMinPulse(); - void setMaxPulse(short); - short getMaxPulse(); - - void setMinAngle(float); - float getMinAngle(); - void setMaxAngle(float); - float getMaxAngle(); - - void setMinSensor(int); - int getMinSensor(); - void setMaxSensor(int); - int getMaxSensor(); - - void setEnabled(bool); - bool getEnabled(); - - bool getCalibrated(); - void setCalibrated(bool); - - - void setMaxSpeed(float); - float getMaxSpeed(); - - void setSmooth(byte); - byte getSmooth(); - - bool getPower(); - - bool getMoving(); - - float readPresentSpeed(); - - void updatePosition(); - -}; diff --git a/inmoov_firmware/configuration.h b/inmoov_firmware/configuration.h deleted file mode 100755 index e12eec9..0000000 --- a/inmoov_firmware/configuration.h +++ /dev/null @@ -1,50 +0,0 @@ -#ifndef configuration_h -#define configuration_h - -#define NUMSERVOS 12 - -#define MINPULSE 550 -#define MAXPULSE 2450 - -#define MINSENSOR 550 -#define MAXSENSOR 3800 - -#define MINANGLE 0 -#define MAXANGLE 180 - -#define SMOOTH 0 -#define MAXSPEED 100 - -#define ID_ADDRESS 512 - -#define EEPROM_RECORD_SIZE 64 - -#define EEPROM_START 0 - -#define UPDATEPERIOD 250 - -//Protocol Constants -#define P_WRITE 0x03 -#define P_READ 0x02 - -#define P_GOALPOSITION 0x1E -#define P_ENABLE 0x18 -#define P_MINANGLE 0x06 -#define P_MAXANGLE 0x08 -#define P_MINPULSE 0x14 -#define P_MAXPULSE 0x16 -#define P_MINSENSOR 0xA2 -#define P_MAXSENSOR 0xA4 -#define P_GOALSPEED 0x20 -#define P_CALIBRATED 0xA0 -#define P_LED 0x19 -#define P_SMOOTH 0xA6 - -#define P_PRESENTPOSITION 0x24 -#define P_PRESENTSPEED 0x26 -#define P_MOVING 0x2E -#define P_SENSORRAW 0xA8 -#define P_POWER 0x2A - - -#endif diff --git a/inmoov_firmware/eeprom.h b/inmoov_firmware/eeprom.h deleted file mode 100644 index 4fb304c..0000000 --- a/inmoov_firmware/eeprom.h +++ /dev/null @@ -1,36 +0,0 @@ -#ifndef eeprom_h -#define eeprom_h - -struct Eeprom { - - byte id; - - byte checksum; - - float minAngle; - float maxAngle; - - short minPulse; //defined as pulse for 0 degrees - short maxPulse; //defined as pulse for 180 degrees - - short minSensor; //defined as sensor reading for minangle - short maxSensor; //defined as sensor reading for maxangle - - short maxSpeed; //rpm? - - byte smooth; // smothing algorythm setting - // 0 == instant - // 1 == max speed - // 2 == linear - // 3 == cos - // 4 == ??? (I forgot) - - byte calibrated; // 1 is calibrated, 0 is not calibrated - - float p; // PID P value * 100 - float i; // PID I value * 100 - float d; // PID D value * 100 -}; - - -#endif diff --git a/inmoov_firmware/inmoov_firmware.ino b/inmoov_firmware/inmoov_firmware.ino deleted file mode 100644 index 92c6374..0000000 --- a/inmoov_firmware/inmoov_firmware.ino +++ /dev/null @@ -1,305 +0,0 @@ -#include -#include -#include "TeensyServo.h" -#include "configuration.h" -#include -#include -#include -#include -#include - -#define LED 13 - -int i, j; -int updateMillis; -int commands; - -TeensyServo *tServo[12]; - -int commandAngle; -int startMillis; - -ros::NodeHandle nh; - -inmoov_msgs::MotorCommand command_msg; -inmoov_msgs::MotorParameter parameter_msg; -inmoov_msgs::MotorStatus status_msg; - -const bool heartbeats[] = {1, 0, 1, 0, 0, 0, 0, 0}; - -void getParameter(const inmoov_msgs::MotorParameter::Request & req, inmoov_msgs::MotorParameter::Response & res) { - byte id = req.id; - byte parameter = req.parameter; - float value = 0.0; - - switch (parameter) { - - case P_GOALPOSITION: - value = tServo[id]->getGoal(); - break; - - case P_MINANGLE: - value = tServo[id]->getMinAngle(); - break; - - case P_MAXANGLE: - value = tServo[id]->getMaxAngle(); - break; - - case P_MINPULSE: - value = (float)tServo[id]->getMinPulse(); - break; - - case P_MAXPULSE: - value = (float)tServo[id]->getMaxPulse(); - break; - - case P_MINSENSOR: - value = (float)tServo[id]->getMinSensor(); - break; - - case P_MAXSENSOR: - value = (float)tServo[id]->getMaxSensor(); - break; - - case P_CALIBRATED: - value = tServo[id]->getCalibrated(); - break; - - case P_PRESENTPOSITION: - value = tServo[id]->readPositionAngle(); - break; - - case P_SENSORRAW: - value = (float)tServo[id]->readPositionRaw(); - break; - - case P_MOVING: - value = (float)tServo[id]->getMoving(); - break; - - case P_PRESENTSPEED: - value = tServo[id]->readPresentSpeed(); - break; - - case P_SMOOTH: - value = (float)tServo[id]->getSmooth(); - break; - - case P_GOALSPEED: - value = tServo[id]->getMaxSpeed(); - break; - - case P_ENABLE: - value = (float)tServo[id]->getEnabled(); - break; - - case P_POWER: - value = (float)tServo[id]->getPower(); - break; - } - - res.data = value; - -} - -void commandCb( const inmoov_msgs::MotorCommand& command_msg) { - - byte id = command_msg.id; - byte parameter = command_msg.parameter; - float value = command_msg.value; - - switch (parameter) { - - case P_GOALPOSITION: - tServo[id]->setGoal(value); - //String string = "Goal Position = " + String(tServo[id]->commandPulse); - //nh.loginfo(String(tServo[id]->commandPulse)); - break; - - case P_MINANGLE: - tServo[id]->setMinAngle(value); - break; - - case P_MAXANGLE: - tServo[id]->setMaxAngle(value); - break; - - case P_MINPULSE: - tServo[id]->setMinPulse(value); - break; - - case P_MAXPULSE: - tServo[id]->setMaxPulse(value); - break; - - case P_ENABLE: - tServo[id]->setEnabled(value); - break; - - case P_CALIBRATED: - tServo[id]->setCalibrated(value); - break; - - case P_MINSENSOR: - tServo[id]->setMinSensor(value); - break; - - case P_MAXSENSOR: - tServo[id]->setMaxSensor(value); - break; - - case P_SMOOTH: - tServo[id]->setSmooth(value); - break; - - case P_GOALSPEED: - tServo[id]->setMaxSpeed(value); - - } - - -} - - -ros::Publisher motorstatus("motorstatus", &status_msg); - -ros::Subscriber motorcommand("motorcommand", &commandCb); - -ros::ServiceServer server("motorparameter", &getParameter); - - -void setupADC() { - analogReadResolution(12); - analogReference(EXTERNAL); -} - - -void updateServos() { - for (i = 0; i < NUMSERVOS; i++) { - tServo[i]->update(); - } -} - -void setupServos() { - tServo[0] = new TeensyServo(0, A8); - tServo[1] = new TeensyServo(1, A9); - tServo[2] = new TeensyServo(2, A10); - tServo[3] = new TeensyServo(3, A11); - tServo[4] = new TeensyServo(4, A7); - tServo[5] = new TeensyServo(5, A6); - tServo[6] = new TeensyServo(6, A5); - tServo[7] = new TeensyServo(7, A4); - tServo[8] = new TeensyServo(8, A3); - tServo[9] = new TeensyServo(9, A2); - tServo[10] = new TeensyServo(10, A1); - tServo[11] = new TeensyServo(11, A0); - -} - -byte generateChecksum() { - return 0; -} - -void setup() { - pinMode(LED, OUTPUT); - digitalWrite(LED, 1); - - nh.initNode(); - nh.advertise(motorstatus); - nh.subscribe(motorcommand); - nh.advertiseService(server); - - while (!nh.connected() ) { - nh.spinOnce(); - } - - setupADC(); - delay(1); - - setupServos(); - - Serial.begin(115200); - - startMillis = millis(); - updateMillis = millis(); - commands = 0; - - - - nh.loginfo("Setup Complete!!!"); - -} - -int pushmotorstatus = 0; // which motorstatus to update - -char joint[2] = " "; -byte bus = 0; - -void loop() { - - updateServos(); - - digitalWrite(LED, heartbeats[((millis() >> 7) & 7)]); - /* - if ((millis() - updateMillis) >= (UPDATEPERIOD / NUMSERVOS)) { - status_msg.id = pushmotorstatus; - status_msg.goal = tServo[pushmotorstatus]->getGoal(); - status_msg.position = tServo[pushmotorstatus]->readPositionAngle(); - status_msg.presentspeed = tServo[pushmotorstatus]->readPresentSpeed(); - status_msg.moving = tServo[pushmotorstatus]->getMoving(); - //status_msg.posraw = tServo[servo]->sampleDuration; - status_msg.posraw = tServo[pushmotorstatus]->readPositionRaw(); - status_msg.enabled = tServo[pushmotorstatus]->getEnabled(); - status_msg.power = tServo[pushmotorstatus]->getPower(); - - motorstatus.publish( &status_msg); - - pushmotorstatus++; - if(pushmotorstatus == NUMSERVOS) { - pushmotorstatus = 0; - } - - updateMillis = millis(); - nh.spinOnce(); - - - } - */ - - - if ((millis() - updateMillis) >= UPDATEPERIOD) { - for (int servo = 0; servo < NUMSERVOS; servo++) { - - status_msg.joint = joint; - status_msg.bus = bus; - status_msg.id = servo; - status_msg.goal = tServo[servo]->getGoal(); - status_msg.position = tServo[servo]->readPositionAngle(); - status_msg.presentspeed = tServo[servo]->readPresentSpeed(); - status_msg.moving = tServo[servo]->getMoving(); - //status_msg.posraw = tServo[servo]->sampleDuration; - status_msg.posraw = tServo[servo]->readPositionRaw(); - status_msg.enabled = tServo[servo]->getEnabled(); - status_msg.power = tServo[servo]->getPower(); - - nh.spinOnce(); - - motorstatus.publish( &status_msg); - - nh.spinOnce(); - } - - updateMillis = millis(); - nh.spinOnce(); - - commands = 0; - - } - - nh.spinOnce(); - -} - - - diff --git a/inmoov_firmware/package.xml b/inmoov_firmware/package.xml deleted file mode 100644 index bed9df8..0000000 --- a/inmoov_firmware/package.xml +++ /dev/null @@ -1,50 +0,0 @@ - - - inmoov_firmware - 0.0.0 - The inmoov_firmware package - - - - - grey - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - - - - - - - - \ No newline at end of file diff --git a/inmoov_firmware2/CMakeLists.txt b/inmoov_firmware2/CMakeLists.txt deleted file mode 100644 index ae5259c..0000000 --- a/inmoov_firmware2/CMakeLists.txt +++ /dev/null @@ -1,181 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(inmoov_firmware2) - -## 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 run_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 run_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 run_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 you 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 inmoov_firmware -# 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) - -## Declare a C++ library -# add_library(inmoov_firmware -# src/${PROJECT_NAME}/inmoov_firmware.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(inmoov_firmware ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -# add_executable(inmoov_firmware_node src/inmoov_firmware_node.cpp) - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(inmoov_firmware_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(inmoov_firmware_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 inmoov_firmware inmoov_firmware_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_inmoov_firmware.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/inmoov_firmware2/LICENSE.txt b/inmoov_firmware2/LICENSE.txt deleted file mode 100644 index 23f57cb..0000000 --- a/inmoov_firmware2/LICENSE.txt +++ /dev/null @@ -1,29 +0,0 @@ -BSD 3-Clause License - -Copyright (c) 2024, Greg Perry (MyRobotLab) -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - -* Redistributions of source code must retain the above copyright notice, this - list of conditions and the following disclaimer. - -* Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - -* Neither the name of the copyright holder nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/inmoov_firmware2/configuration.h b/inmoov_firmware2/configuration.h deleted file mode 100755 index e12eec9..0000000 --- a/inmoov_firmware2/configuration.h +++ /dev/null @@ -1,50 +0,0 @@ -#ifndef configuration_h -#define configuration_h - -#define NUMSERVOS 12 - -#define MINPULSE 550 -#define MAXPULSE 2450 - -#define MINSENSOR 550 -#define MAXSENSOR 3800 - -#define MINANGLE 0 -#define MAXANGLE 180 - -#define SMOOTH 0 -#define MAXSPEED 100 - -#define ID_ADDRESS 512 - -#define EEPROM_RECORD_SIZE 64 - -#define EEPROM_START 0 - -#define UPDATEPERIOD 250 - -//Protocol Constants -#define P_WRITE 0x03 -#define P_READ 0x02 - -#define P_GOALPOSITION 0x1E -#define P_ENABLE 0x18 -#define P_MINANGLE 0x06 -#define P_MAXANGLE 0x08 -#define P_MINPULSE 0x14 -#define P_MAXPULSE 0x16 -#define P_MINSENSOR 0xA2 -#define P_MAXSENSOR 0xA4 -#define P_GOALSPEED 0x20 -#define P_CALIBRATED 0xA0 -#define P_LED 0x19 -#define P_SMOOTH 0xA6 - -#define P_PRESENTPOSITION 0x24 -#define P_PRESENTSPEED 0x26 -#define P_MOVING 0x2E -#define P_SENSORRAW 0xA8 -#define P_POWER 0x2A - - -#endif diff --git a/inmoov_firmware2/eeprom.h b/inmoov_firmware2/eeprom.h deleted file mode 100644 index 4fb304c..0000000 --- a/inmoov_firmware2/eeprom.h +++ /dev/null @@ -1,36 +0,0 @@ -#ifndef eeprom_h -#define eeprom_h - -struct Eeprom { - - byte id; - - byte checksum; - - float minAngle; - float maxAngle; - - short minPulse; //defined as pulse for 0 degrees - short maxPulse; //defined as pulse for 180 degrees - - short minSensor; //defined as sensor reading for minangle - short maxSensor; //defined as sensor reading for maxangle - - short maxSpeed; //rpm? - - byte smooth; // smothing algorythm setting - // 0 == instant - // 1 == max speed - // 2 == linear - // 3 == cos - // 4 == ??? (I forgot) - - byte calibrated; // 1 is calibrated, 0 is not calibrated - - float p; // PID P value * 100 - float i; // PID I value * 100 - float d; // PID D value * 100 -}; - - -#endif diff --git a/inmoov_firmware2/inmoov_firmware2.ino b/inmoov_firmware2/inmoov_firmware2.ino deleted file mode 100644 index 126a5d4..0000000 --- a/inmoov_firmware2/inmoov_firmware2.ino +++ /dev/null @@ -1,303 +0,0 @@ -#include -#include "configuration.h" -#include -#include -#include -#include -#include - -#define LED 13 - -int i, j; -int updateMillis; -int commands; - -Servo *tServo[12]; - -int commandAngle; -int startMillis; - -ros::NodeHandle nh; - -inmoov_msgs::MotorCommand command_msg; -inmoov_msgs::MotorParameter parameter_msg; -inmoov_msgs::MotorStatus status_msg; - -const bool heartbeats[] = {1, 0, 1, 0, 0, 0, 0, 0}; - -void getParameter(const inmoov_msgs::MotorParameter::Request & req, inmoov_msgs::MotorParameter::Response & res) { - byte id = req.id; - byte parameter = req.parameter; - float value = 0.0; - - switch (parameter) { - - case P_GOALPOSITION: - value = tServo[id]->getGoal(); - break; - - case P_MINANGLE: - value = tServo[id]->getMinAngle(); - break; - - case P_MAXANGLE: - value = tServo[id]->getMaxAngle(); - break; - - case P_MINPULSE: - value = (float)tServo[id]->getMinPulse(); - break; - - case P_MAXPULSE: - value = (float)tServo[id]->getMaxPulse(); - break; - - case P_MINSENSOR: - value = (float)tServo[id]->getMinSensor(); - break; - - case P_MAXSENSOR: - value = (float)tServo[id]->getMaxSensor(); - break; - - case P_CALIBRATED: - value = tServo[id]->getCalibrated(); - break; - - case P_PRESENTPOSITION: - value = tServo[id]->readPositionAngle(); - break; - - case P_SENSORRAW: - value = (float)tServo[id]->readPositionRaw(); - break; - - case P_MOVING: - value = (float)tServo[id]->getMoving(); - break; - - case P_PRESENTSPEED: - value = tServo[id]->readPresentSpeed(); - break; - - case P_SMOOTH: - value = (float)tServo[id]->getSmooth(); - break; - - case P_GOALSPEED: - value = tServo[id]->getMaxSpeed(); - break; - - case P_ENABLE: - value = (float)tServo[id]->getEnabled(); - break; - - case P_POWER: - value = (float)tServo[id]->getPower(); - break; - } - - res.data = value; - -} - -void commandCb( const inmoov_msgs::MotorCommand& command_msg) { - - byte id = command_msg.id; - byte parameter = command_msg.parameter; - float value = command_msg.value; - - switch (parameter) { - - case P_GOALPOSITION: - tServo[id]->setGoal(value); - //String string = "Goal Position = " + String(tServo[id]->commandPulse); - //nh.loginfo(String(tServo[id]->commandPulse)); - break; - - case P_MINANGLE: - tServo[id]->setMinAngle(value); - break; - - case P_MAXANGLE: - tServo[id]->setMaxAngle(value); - break; - - case P_MINPULSE: - tServo[id]->setMinPulse(value); - break; - - case P_MAXPULSE: - tServo[id]->setMaxPulse(value); - break; - - case P_ENABLE: - tServo[id]->setEnabled(value); - break; - - case P_CALIBRATED: - tServo[id]->setCalibrated(value); - break; - - case P_MINSENSOR: - tServo[id]->setMinSensor(value); - break; - - case P_MAXSENSOR: - tServo[id]->setMaxSensor(value); - break; - - case P_SMOOTH: - tServo[id]->setSmooth(value); - break; - - case P_GOALSPEED: - tServo[id]->setMaxSpeed(value); - - } - - -} - - -ros::Publisher motorstatus("motorstatus", &status_msg); - -ros::Subscriber motorcommand("motorcommand", &commandCb); - -ros::ServiceServer server("motorparameter", &getParameter); - - -void setupADC() { - analogReadResolution(12); - analogReference(EXTERNAL); -} - - -void updateServos() { - for (i = 0; i < NUMSERVOS; i++) { - tServo[i]->update(); - } -} - -void setupServos() { - tServo[0] = new TeensyServo(0, A8); - tServo[1] = new TeensyServo(1, A9); - tServo[2] = new TeensyServo(2, A10); - tServo[3] = new TeensyServo(3, A11); - tServo[4] = new TeensyServo(4, A7); - tServo[5] = new TeensyServo(5, A6); - tServo[6] = new TeensyServo(6, A5); - tServo[7] = new TeensyServo(7, A4); - tServo[8] = new TeensyServo(8, A3); - tServo[9] = new TeensyServo(9, A2); - tServo[10] = new TeensyServo(10, A1); - tServo[11] = new TeensyServo(11, A0); - -} - -byte generateChecksum() { - return 0; -} - -void setup() { - pinMode(LED, OUTPUT); - digitalWrite(LED, 1); - - nh.initNode(); - nh.advertise(motorstatus); - nh.subscribe(motorcommand); - nh.advertiseService(server); - - while (!nh.connected() ) { - nh.spinOnce(); - } - - setupADC(); - delay(1); - - setupServos(); - - Serial.begin(115200); - - startMillis = millis(); - updateMillis = millis(); - commands = 0; - - - - nh.loginfo("Setup Complete!!!"); - -} - -int pushmotorstatus = 0; // which motorstatus to update - -char joint[2] = " "; -byte bus = 0; - -void loop() { - - updateServos(); - - digitalWrite(LED, heartbeats[((millis() >> 7) & 7)]); - /* - if ((millis() - updateMillis) >= (UPDATEPERIOD / NUMSERVOS)) { - status_msg.id = pushmotorstatus; - status_msg.goal = tServo[pushmotorstatus]->getGoal(); - status_msg.position = tServo[pushmotorstatus]->readPositionAngle(); - status_msg.presentspeed = tServo[pushmotorstatus]->readPresentSpeed(); - status_msg.moving = tServo[pushmotorstatus]->getMoving(); - //status_msg.posraw = tServo[servo]->sampleDuration; - status_msg.posraw = tServo[pushmotorstatus]->readPositionRaw(); - status_msg.enabled = tServo[pushmotorstatus]->getEnabled(); - status_msg.power = tServo[pushmotorstatus]->getPower(); - - motorstatus.publish( &status_msg); - - pushmotorstatus++; - if(pushmotorstatus == NUMSERVOS) { - pushmotorstatus = 0; - } - - updateMillis = millis(); - nh.spinOnce(); - - - } - */ - - - if ((millis() - updateMillis) >= UPDATEPERIOD) { - for (int servo = 0; servo < NUMSERVOS; servo++) { - - status_msg.joint = joint; - status_msg.bus = bus; - status_msg.id = servo; - status_msg.goal = tServo[servo]->getGoal(); - status_msg.position = tServo[servo]->readPositionAngle(); - status_msg.presentspeed = tServo[servo]->readPresentSpeed(); - status_msg.moving = tServo[servo]->getMoving(); - //status_msg.posraw = tServo[servo]->sampleDuration; - status_msg.posraw = tServo[servo]->readPositionRaw(); - status_msg.enabled = tServo[servo]->getEnabled(); - status_msg.power = tServo[servo]->getPower(); - - nh.spinOnce(); - - motorstatus.publish( &status_msg); - - nh.spinOnce(); - } - - updateMillis = millis(); - nh.spinOnce(); - - commands = 0; - - } - - nh.spinOnce(); - -} - - - diff --git a/inmoov_firmware2/package.xml b/inmoov_firmware2/package.xml deleted file mode 100644 index a8c9703..0000000 --- a/inmoov_firmware2/package.xml +++ /dev/null @@ -1,50 +0,0 @@ - - - inmoov_firmware2 - 0.0.0 - The inmoov_firmware2 package - - - - - grey - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - - - - - - - - \ No newline at end of file diff --git a/inmoov_meshes/CMakeLists.txt b/inmoov_meshes/CMakeLists.txt index b1efd42..7214823 100644 --- a/inmoov_meshes/CMakeLists.txt +++ b/inmoov_meshes/CMakeLists.txt @@ -1,192 +1,13 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(inmoov_meshes) -## Add support for C++11, supported in ROS Kinetic and newer -# add_definitions(-std=c++11) +# Find packages +find_package(ament_cmake REQUIRED) -## 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 run_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 run_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 run_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 you 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 inmoov_meshes -# CATKIN_DEPENDS other_catkin_pkg -# DEPENDS system_lib +# Install meshes +install(DIRECTORY meshes + DESTINATION share/${PROJECT_NAME}/ ) -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -# include_directories(include) - -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/inmoov_meshes.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/inmoov_meshes_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_inmoov_meshes.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) +# Package +ament_package() diff --git a/inmoov_meshes/package.xml b/inmoov_meshes/package.xml index d23062d..b8f1518 100644 --- a/inmoov_meshes/package.xml +++ b/inmoov_meshes/package.xml @@ -1,50 +1,13 @@ - + inmoov_meshes - 0.0.0 - The inmoov_meshes package - - - - - grey - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - - - + 0.1.0 + Meshes for the InMoov robot + Greg Perry + BSD + ament_cmake + ament_index_cpp - - + ament_cmake