diff --git a/CMakeLists.txt b/CMakeLists.txt deleted file mode 120000 index 581e61d..0000000 --- a/CMakeLists.txt +++ /dev/null @@ -1 +0,0 @@ -/opt/ros/kinetic/share/catkin/cmake/toplevel.cmake \ No newline at end of file diff --git a/i2cpwm_board/CMakeLists.txt b/i2cpwm_board/CMakeLists.txt new file mode 100644 index 0000000..15423aa --- /dev/null +++ b/i2cpwm_board/CMakeLists.txt @@ -0,0 +1,22 @@ +cmake_minimum_required(VERSION 3.8) +project(i2cpwm_board) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# Find dependencies +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(builtin_interfaces REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/Servo.msg" + "msg/ServoArray.msg" + "msg/ServoConfig.msg" + "srv/ServosConfig.srv" + DEPENDENCIES builtin_interfaces +) + +ament_export_dependencies(rosidl_default_runtime) +ament_package() diff --git a/i2cpwm_board/msg/Servo.msg b/i2cpwm_board/msg/Servo.msg new file mode 100644 index 0000000..eeca1ed --- /dev/null +++ b/i2cpwm_board/msg/Servo.msg @@ -0,0 +1,2 @@ +int32 servo +float32 value diff --git a/i2cpwm_board/msg/ServoArray.msg b/i2cpwm_board/msg/ServoArray.msg new file mode 100644 index 0000000..d2bdcd6 --- /dev/null +++ b/i2cpwm_board/msg/ServoArray.msg @@ -0,0 +1 @@ +Servo[] servos diff --git a/i2cpwm_board/msg/ServoConfig.msg b/i2cpwm_board/msg/ServoConfig.msg new file mode 100644 index 0000000..a71a9ec --- /dev/null +++ b/i2cpwm_board/msg/ServoConfig.msg @@ -0,0 +1,4 @@ +int32 servo +int32 center +int32 range +int32 direction diff --git a/i2cpwm_board/package.xml b/i2cpwm_board/package.xml new file mode 100644 index 0000000..448e0f5 --- /dev/null +++ b/i2cpwm_board/package.xml @@ -0,0 +1,22 @@ + + + + i2cpwm_board + 0.0.1 + ROS2 interface definitions for i2cpwm_board servo controller + user + MIT + + ament_cmake + rosidl_default_generators + + builtin_interfaces + + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + diff --git a/i2cpwm_board/srv/ServosConfig.srv b/i2cpwm_board/srv/ServosConfig.srv new file mode 100644 index 0000000..0abb636 --- /dev/null +++ b/i2cpwm_board/srv/ServosConfig.srv @@ -0,0 +1,3 @@ +ServoConfig[] servos +--- +bool success diff --git a/lcd_monitor/CMakeLists.txt b/lcd_monitor/CMakeLists.txt deleted file mode 100644 index 13d1926..0000000 --- a/lcd_monitor/CMakeLists.txt +++ /dev/null @@ -1,206 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(lcd_monitor) - -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - rospy - geometry_msgs - std_msgs -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES lcd_monitor -# CATKIN_DEPENDS rospy -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( -# include - ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/lcd_monitor.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/lcd_monitor_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 for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -# install(TARGETS ${PROJECT_NAME}_node -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -# install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_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_lcd_monitor.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/lcd_monitor/launch/lcd_monitor.launch b/lcd_monitor/launch/lcd_monitor.launch deleted file mode 100644 index 148234d..0000000 --- a/lcd_monitor/launch/lcd_monitor.launch +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - diff --git a/lcd_monitor/launch/lcd_monitor_launch.py b/lcd_monitor/launch/lcd_monitor_launch.py new file mode 100644 index 0000000..1bcceb8 --- /dev/null +++ b/lcd_monitor/launch/lcd_monitor_launch.py @@ -0,0 +1,11 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='lcd_monitor', + executable='sm_lcd_node', + name='lcd_monitor_node', + output='screen'), + ]) diff --git a/lcd_monitor/package.xml b/lcd_monitor/package.xml index d1a5bd1..488d045 100644 --- a/lcd_monitor/package.xml +++ b/lcd_monitor/package.xml @@ -1,68 +1,22 @@ - + + lcd_monitor 0.0.0 The lcd_monitor package - - - - mike - - - - - TODO + ament_python - - - - - + rclpy + std_msgs + geometry_msgs - - - - + ament_lint_auto + ament_lint_common - - - - - - - - - - - - - - - - - - - - - - catkin - geometry_msgs - std_msgs - rospy - rospy - geometry_msgs - std_msgs - rospy - geometry_msgs - std_msgs - - - - - + ament_python diff --git a/lcd_monitor/resource/lcd_monitor b/lcd_monitor/resource/lcd_monitor new file mode 100644 index 0000000..e69de29 diff --git a/lcd_monitor/setup.py b/lcd_monitor/setup.py index fb52c30..b78c9bc 100644 --- a/lcd_monitor/setup.py +++ b/lcd_monitor/setup.py @@ -1,11 +1,27 @@ -## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD - from setuptools import setup -from catkin_pkg.python_setup import generate_distutils_setup -# fetch values from package.xml -setup_args = generate_distutils_setup( - packages=['lcd_monitor'], - package_dir={'': 'src'}) +package_name = 'lcd_monitor' -setup(**setup_args) +setup( + name=package_name, + version='0.0.0', + packages=[package_name], + package_dir={'': 'src'}, + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='mike', + maintainer_email='mike@todo.todo', + description='The lcd_monitor package', + license='TODO', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'sm_lcd_node = lcd_monitor.sm_lcd_driver:main', + ], + }, +) diff --git a/lcd_monitor/src/lcd_monitor/I2C_LCD_driver.py b/lcd_monitor/src/lcd_monitor/I2C_LCD_driver.py index 56fd3bb..7953d9d 100644 --- a/lcd_monitor/src/lcd_monitor/I2C_LCD_driver.py +++ b/lcd_monitor/src/lcd_monitor/I2C_LCD_driver.py @@ -1,4 +1,4 @@ -#!/usr/bin/python +#!/usr/bin/env python3 # -*- coding: utf-8 -*- # Original code found at: diff --git a/lcd_monitor/src/lcd_monitor/sm_lcd_driver.py b/lcd_monitor/src/lcd_monitor/sm_lcd_driver.py index 88cf6c4..9771395 100644 --- a/lcd_monitor/src/lcd_monitor/sm_lcd_driver.py +++ b/lcd_monitor/src/lcd_monitor/sm_lcd_driver.py @@ -2,17 +2,19 @@ from lcd_monitor import I2C_LCD_driver import datetime import time -import rospy +import rclpy +from rclpy.node import Node from math import pi from geometry_msgs.msg import Vector3 from geometry_msgs.msg import Twist from std_msgs.msg import String -class SpotMicroLcd(): +class SpotMicroLcd(Node): ''' Class to encapsulate lcd driver for spot micro robot ''' def __init__(self): - '''Constructor''' + super().__init__('lcd_monitor_node') + self._mylcd = I2C_LCD_driver.lcd() self._state_str = 'None' @@ -26,11 +28,11 @@ def __init__(self): self._theta_cmd = 0.0 self._psi_cmd = 0.0 - rospy.init_node('lcd_monitor_node') + self.create_subscription(Twist, 'lcd_vel_cmd', self.update_speed_cmd, 1) + self.create_subscription(Vector3, 'lcd_angle_cmd', self.update_angle_cmd, 1) + self.create_subscription(String, 'lcd_state', self.update_state_string, 1) - rospy.Subscriber('lcd_vel_cmd',Twist,self.update_speed_cmd) - rospy.Subscriber('lcd_angle_cmd',Vector3,self.update_angle_cmd) - rospy.Subscriber('lcd_state',String,self.update_state_string) + self._timer = self.create_timer(1.0/3.0, self.run_lcd) def update_speed_cmd(self, msg): ''' Updates speed command attributes''' @@ -54,30 +56,24 @@ def update_state_string(self, msg): self._padded_state_str = "To Idle" else: self._padded_state_str = self._state_str - + self._padded_state_str = self._padded_state_str.ljust(16,' ') - def run(self): + def run_lcd(self): ''' Runs the lcd driver and prints data''' + self._mylcd.lcd_display_string('State: %s'%(self._padded_state_str),1) - # Define the loop rate in Hz - rate = rospy.Rate(3) - - while not rospy.is_shutdown(): - - self._mylcd.lcd_display_string('State: %s'%(self._padded_state_str),1) - - - if self._state_str == "Stand": - self._mylcd.lcd_display_string('x%3.0f y%3.0f z%3.0f'%(self._phi_cmd, self._theta_cmd, self._psi_cmd),2) - elif self._state_str == "Walk": - self._mylcd.lcd_display_string('x%3.0f y%3.0f z%3.0f'%(self._fwd_speed_cmd, self._side_speed_cmd, self._yaw_rate_cmd),2) - else: - self._mylcd.lcd_display_string(' ',2) - # Sleep till next loop - rate.sleep() + if self._state_str == "Stand": + self._mylcd.lcd_display_string('x%3.0f y%3.0f z%3.0f'%(self._phi_cmd, self._theta_cmd, self._psi_cmd),2) + elif self._state_str == "Walk": + self._mylcd.lcd_display_string('x%3.0f y%3.0f z%3.0f'%(self._fwd_speed_cmd, self._side_speed_cmd, self._yaw_rate_cmd),2) + else: + self._mylcd.lcd_display_string(' ',2) -def main(): +def main(args=None): + rclpy.init(args=args) sm_lcd_obj = SpotMicroLcd() - sm_lcd_obj.run() + rclpy.spin(sm_lcd_obj) + sm_lcd_obj.destroy_node() + rclpy.shutdown() diff --git a/servo_move_keyboard/CMakeLists.txt b/servo_move_keyboard/CMakeLists.txt index 7823aad..35ba7ad 100644 --- a/servo_move_keyboard/CMakeLists.txt +++ b/servo_move_keyboard/CMakeLists.txt @@ -1,205 +1,20 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.8) project(servo_move_keyboard) -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - rospy - i2cpwm_board -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) +find_package(ament_cmake REQUIRED) -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES servo_move_keyboard -# CATKIN_DEPENDS rospy -# DEPENDS system_lib +install(PROGRAMS + scripts/servoMoveKeyboard.py + scripts/servoConfigTest.py + DESTINATION lib/${PROJECT_NAME} ) -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( -# include - ${catkin_INCLUDE_DIRS} +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} ) -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/servo_move_keyboard.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/servo_move_keyboard_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 for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -# install(TARGETS ${PROJECT_NAME}_node -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -# install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_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_servo_move_keyboard.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) +ament_package() diff --git a/servo_move_keyboard/launch/keyboard_move.launch b/servo_move_keyboard/launch/keyboard_move.launch deleted file mode 100644 index d6ea0bb..0000000 --- a/servo_move_keyboard/launch/keyboard_move.launch +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - │walk: Start walk mode and keyboard motion control - - diff --git a/servo_move_keyboard/launch/keyboard_move_launch.py b/servo_move_keyboard/launch/keyboard_move_launch.py new file mode 100644 index 0000000..298b9d5 --- /dev/null +++ b/servo_move_keyboard/launch/keyboard_move_launch.py @@ -0,0 +1,28 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +def generate_launch_description(): + run_i2c_pwmboard = LaunchConfiguration('run_i2c_pwmboard') + + return LaunchDescription([ + DeclareLaunchArgument( + 'run_i2c_pwmboard', + default_value='false', + description='Also run i2c_pwmboard node'), + + Node( + package='servo_move_keyboard', + executable='servoMoveKeyboard.py', + name='servo_move_keyboard_node', + output='screen'), + + Node( + package='i2cpwm_board', + executable='i2cpwm_board_node', + name='i2cpwm_board_node', + output='screen', + condition=IfCondition(run_i2c_pwmboard)), + ]) diff --git a/servo_move_keyboard/package.xml b/servo_move_keyboard/package.xml index 860071b..7d4432c 100644 --- a/servo_move_keyboard/package.xml +++ b/servo_move_keyboard/package.xml @@ -1,64 +1,21 @@ - + + servo_move_keyboard 0.0.0 - Package to move servos via keyboard inputs - - - - + The servo_move_keyboard package mike - - - - - TODO + ament_cmake - - - - - + rclpy + i2cpwm_board - - - - + ament_lint_auto + ament_lint_common - - - - - - - - - - - - - - - - - - - - - - catkin - rospy - rospy - rospy - - i2cpwm_board - i2cpwm_board - - - - + ament_cmake diff --git a/servo_move_keyboard/scripts/servoConfigTest.py b/servo_move_keyboard/scripts/servoConfigTest.py index 58a47d7..16937d7 100755 --- a/servo_move_keyboard/scripts/servoConfigTest.py +++ b/servo_move_keyboard/scripts/servoConfigTest.py @@ -1,15 +1,15 @@ -#!/usr/bin/python +#!/usr/bin/env python3 """ -Class for testing control of 12 servos. It assumes ros-12cpwmboard has been +Class for testing control of 12 servos. It assumes ros-i2cpwmboard has been installed """ -import rospy -import sys, select, termios, tty # For terminal keyboard key press reading -from i2cpwm_board.msg import Servo, ServoArray, ServoConfig, ServoConfigArray +import rclpy +from rclpy.node import Node +import sys, select, termios, tty +from i2cpwm_board.msg import Servo, ServoArray, ServoConfig from i2cpwm_board.srv import ServosConfig -# Global variable for number of servos numServos = 1 msg = """ @@ -21,7 +21,7 @@ oneServo: Move one servo manually, all others will be commanded to their center position allServos: Move all servo's manually together -Keyboard commands for One Servo Control +Keyboard commands for One Servo Control --------------------------- q t y u f g j k @@ -45,7 +45,6 @@ CTRL-C to quit """ -# Dictionary with anonomous helper functions to execute key commands keyDict = { 'q': None, 't': lambda x: x.set_value(x._min), @@ -63,14 +62,6 @@ validCmds = ['quit','oneServo','allServos'] class ServoConvert(): - ''' - ServoConvert Class encapsulates a servo - Servo has a center value, and range, and is commanded by a value between 0 and 4095. - This coorsponds to the duty cycle in a 12 bit pwm cycle. Nominally, a servo is commanded with pulses of - 1 to 2 ms in a 20 ms cycle, with 1.5 ms being the value for center position. - These nominal values would coorespond to integer values of approximately 204, 306, and 409 - for 1 ms, 1.5 ms, and 2 ms, respectively - ''' def __init__(self, id=1, center_value=0, direction=1): self.value = center_value self._center = center_value @@ -80,119 +71,98 @@ def __init__(self, id=1, center_value=0, direction=1): self.id = id def set_value(self, value_in): - ''' - Set Servo value - Input: Value between 0 and 4095 - ''' if False: print('Servo value not in range [0,4095]') else: self.value = value_in - - def set_center(self,center_val): - ''' - Set Servo center value - Input: Value between 0 and 4095 - ''' + def set_center(self, center_val): if False: print('Servo value not in range [0,4095]') else: self._center = center_val - print('Servo %2i center set to %4i'%(self.id+1,center_val)) + print('Servo %2i center set to %4i' % (self.id+1, center_val)) - def set_max(self,max_val): - ''' - Set Servo max value - Input: Value between 0 and 4095 - ''' + def set_max(self, max_val): if False: print('Servo value not in range [0,4095]') else: self._max = max_val - print('Servo %2i max set to %4i'%(self.id+1,max_val)) + print('Servo %2i max set to %4i' % (self.id+1, max_val)) - def set_min(self,min_val): - ''' - Set Servo min value - Input: Value between 0 and 4095 - ''' + def set_min(self, min_val): if False: print('Servo value not in range [0,4095]') else: self._min = min_val - print('Servo %2i min set to %4i'%(self.id+1,min_val)) + print('Servo %2i min set to %4i' % (self.id+1, min_val)) -class SpotMicroServoControl(): +class SpotMicroServoControl(Node): def __init__(self): - # Create a ServoConfig messag for one servo - self._servo_config_msg = ServoConfigArray() - self._servo1_config = ServoConfig() + super().__init__('spot_micro_servo_control') + self._servo1_config = ServoConfig() self._servo1_config.center = 300 self._servo1_config.range = 400 self._servo1_config.servo = 1 self._servo1_config.direction = 1 - self._servo_config_msg.servos.append(self._servo1_config) - - rospy.loginfo("> Waiting for config_servos service...") + self.get_logger().info("> Waiting for config_servos service...") print('test1') - rospy.wait_for_service('config_servos') - rospy.loginfo("> Config_servos service found!") + + self.servo_config_client = self.create_client(ServosConfig, 'config_servos') + while not self.servo_config_client.wait_for_service(timeout_sec=1.0): + if not rclpy.ok(): + self.get_logger().error('Interrupted while waiting for service') + return + self.get_logger().info('Waiting for config_servos service...') + + self.get_logger().info("> Config_servos service found!") print('test2') - try: - servoConfigService = rospy.ServiceProxy('config_servos',ServosConfig) - resp = servoConfigService(self._servo_config_msg.servos) - print("Config servos done!!, returned value: %i"%resp.error) - except rospy.ServiceException, e: - print "Service call failed: %s"%e - - rospy.loginfo("Setting Up the Spot Micro Servo Control Node...") + request = ServosConfig.Request() + request.servos.append(self._servo1_config) - # Set up and title the ros node for this code - rospy.init_node('spot_micro_servo_control') + try: + future = self.servo_config_client.call_async(request) + rclpy.spin_until_future_complete(self, future) + response = future.result() + if response is not None: + print("Config servos done!!, returned success: %s" % str(response.success)) + else: + print("Service call failed: no response") + except Exception as e: + print("Service call failed: %s" % str(e)) - # Intialize empty servo dictionary - self.servos = {} + self.get_logger().info("Setting Up the Spot Micro Servo Control Node...") - # Create a servo dictionary with 12 ServoConvert objects - # keys: integers 0 through 12 - # values: ServoConvert objects + self.servos = {} for i in range(numServos): self.servos[i] = ServoConvert(id=i) - rospy.loginfo("> Servos corrrectly initialized") + self.get_logger().info("> Servos correctly initialized") - # Create empty ServoArray message with n number of Servos in its array - self._servo_msg = ServoArray() - for i in range(numServos): + self._servo_msg = ServoArray() + for i in range(numServos): self._servo_msg.servos.append(Servo()) - # Create the servo array publisher - self.ros_pub_servo_array = rospy.Publisher("/servos_proportional", ServoArray, queue_size=1) - rospy.loginfo("> Publisher corrrectly initialized") + self.ros_pub_servo_array = self.create_publisher(ServoArray, "/servos_proportional", 1) + self.get_logger().info("> Publisher correctly initialized") - rospy.loginfo("Initialization complete") + self.get_logger().info("Initialization complete") - # Setup terminal input reading, taken from teleop_twist_keyboard self.settings = termios.tcgetattr(sys.stdin) def send_servo_msg(self): - for servo_key, servo_obj in self.servos.iteritems(): + for servo_key, servo_obj in self.servos.items(): self._servo_msg.servos[servo_obj.id].servo = servo_obj.id+1 self._servo_msg.servos[servo_obj.id].value = servo_obj.value - #rospy.loginfo("Sending to %s command %d"%(servo_key, servo_obj.value)) self.ros_pub_servo_array.publish(self._servo_msg) def reset_all_servos_center(self): - ''' - Reset all servos to their center value - ''' for s in self.servos: self.servos[s].value = self.servos[s]._center - + def getKey(self): tty.setraw(sys.stdin.fileno()) select.select([sys.stdin], [], [], 0) @@ -201,97 +171,84 @@ def getKey(self): return key def run(self): - - # Set all servos to their center values self.reset_all_servos_center() self.send_servo_msg() - # Prompt user with keyboard command information - # Ability to control individual servo to find limits and center values - # and ability to control all servos together - - while not rospy.is_shutdown(): - print(msg) - userInput = raw_input("Command?: ") - - if userInput not in validCmds: - print('Valid command not entered, try again...') - else: - if userInput == 'quit': - print("Ending program...") - print('Final Servo Values') - print('--------------------') - for i in range(numServos): - print('Servo %2i: Min: %1.2f, Center: %1.2f, Max: %1.2f'%(i,self.servos[i]._min,self.servos[i]._center,self.servos[i]._max)) - break - - elif userInput == 'oneServo': - # Reset all servos to center value, and send command - self.reset_all_servos_center() - self.send_servo_msg() - - # First get servo number to command - nSrv = -1 - while (1): - userInput = input('Which servo to control? Enter a number 1 through 12: ') - - if userInput not in range(1,numServos+1): - print("Invalid servo number entered, try again") - else: - nSrv = userInput - 1 - break - - # Loop and act on user command - print('Enter command, q to go back to option select: ') - while (1): - - userInput = self.getKey() - - if userInput == 'q': - break - elif userInput not in keyDict: - print('Key not in valid key commands, try again') - else: - keyDict[userInput](self.servos[nSrv]) - print('Servo %2i cmd: %1.2f'%(nSrv,self.servos[nSrv].value)) - self.send_servo_msg() - - elif userInput == 'allServos': - # Reset all servos to center value, and send command - self.reset_all_servos_center() - self.send_servo_msg() - - print('Enter command, q to go back to option select: ') - while (1): - - userInput = self.getKey() - - if userInput == 'q': - break - elif userInput not in keyDict: - print('Key not in valid key commands, try again') - elif userInput in ('b','n','m'): - print('Saving values not supported in all servo control mode') - else: - for s in self.servos.values(): - keyDict[userInput](s) - print('All Servos Commanded') - self.send_servo_msg() - - - - - - - - # print self._last_time_cmd_rcv, self.is_controller_connected - # if not self.is_controller_connected: - # self.set_actuators_idle() - - # Set the control rate in Hz - rate = rospy.Rate(10) - rate.sleep() + try: + while rclpy.ok(): + print(msg) + userInput = input("Command?: ") + + if userInput not in validCmds: + print('Valid command not entered, try again...') + else: + if userInput == 'quit': + print("Ending program...") + print('Final Servo Values') + print('--------------------') + for i in range(numServos): + print('Servo %2i: Min: %1.2f, Center: %1.2f, Max: %1.2f' % (i, self.servos[i]._min, self.servos[i]._center, self.servos[i]._max)) + break + + elif userInput == 'oneServo': + self.reset_all_servos_center() + self.send_servo_msg() + + nSrv = -1 + while True: + userInput = input('Which servo to control? Enter a number 1 through 12: ') + try: + val = int(userInput) + if val not in range(1, numServos+1): + print("Invalid servo number entered, try again") + else: + nSrv = val - 1 + break + except ValueError: + print("Invalid servo number entered, try again") + + print('Enter command, q to go back to option select: ') + while True: + userInput = self.getKey() + + if userInput == 'q': + break + elif userInput not in keyDict: + print('Key not in valid key commands, try again') + else: + keyDict[userInput](self.servos[nSrv]) + print('Servo %2i cmd: %1.2f' % (nSrv, self.servos[nSrv].value)) + self.send_servo_msg() + + elif userInput == 'allServos': + self.reset_all_servos_center() + self.send_servo_msg() + + print('Enter command, q to go back to option select: ') + while True: + userInput = self.getKey() + + if userInput == 'q': + break + elif userInput not in keyDict: + print('Key not in valid key commands, try again') + elif userInput in ('b','n','m'): + print('Saving values not supported in all servo control mode') + else: + for s in self.servos.values(): + keyDict[userInput](s) + print('All Servos Commanded') + self.send_servo_msg() + + rate = self.create_rate(10) + rate.sleep() + except KeyboardInterrupt: + pass + finally: + termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings) if __name__ == "__main__": - smsc = SpotMicroServoControl() - smsc.run() \ No newline at end of file + rclpy.init() + smsc = SpotMicroServoControl() + smsc.run() + rclpy.shutdown() diff --git a/servo_move_keyboard/scripts/servoMoveKeyboard.py b/servo_move_keyboard/scripts/servoMoveKeyboard.py index c8ddd09..672cc19 100755 --- a/servo_move_keyboard/scripts/servoMoveKeyboard.py +++ b/servo_move_keyboard/scripts/servoMoveKeyboard.py @@ -1,15 +1,15 @@ -#!/usr/bin/python +#!/usr/bin/env python3 """ -Class for testing control of 12 servos. It assumes ros-12cpwmboard has been +Class for testing control of 12 servos. It assumes ros-i2cpwmboard has been installed """ -import rospy -import sys, select, termios, tty # For terminal keyboard key press reading +import rclpy +from rclpy.node import Node +import sys, select, termios, tty from i2cpwm_board.msg import Servo, ServoArray -# Global variable for number of servos -numServos = 12 +numServos = 12 msg = """ Servo Control Module for 12 Servos. @@ -20,9 +20,9 @@ oneServo: Move one servo manually, all others will be commanded to their center position allServos: Move all servo's manually together -Keyboard commands for One Servo Control +Keyboard commands for One Servo Control --------------------------- - q y + q y f g j k z x b n m @@ -44,7 +44,6 @@ CTRL-C to quit """ -# Dictionary with anonomous helper functions to execute key commands keyDict = { 'q': None, 'z': lambda x: x.set_value(x._min), @@ -62,14 +61,6 @@ validCmds = ['quit','oneServo','allServos'] class ServoConvert(): - ''' - ServoConvert Class encapsulates a servo - Servo has a center value, and range, and is commanded by a value between 0 and 4095. - This coorsponds to the duty cycle in a 12 bit pwm cycle. Nominally, a servo is commanded with pulses of - 1 to 2 ms in a 20 ms cycle, with 1.5 ms being the value for center position. - These nominal values would coorespond to integer values of approximately 204, 306, and 409 - for 1 ms, 1.5 ms, and 2 ms, respectively - ''' def __init__(self, id=1, center_value=306, direction=1): self.value = center_value self._center = center_value @@ -79,100 +70,67 @@ def __init__(self, id=1, center_value=306, direction=1): self.id = id def set_value(self, value_in): - ''' - Set Servo value - Input: Value between 0 and 4095 - ''' if value_in not in range(4096): print('Servo value not in range [0,4095]') else: self.value = value_in - - def set_center(self,center_val): - ''' - Set Servo center value - Input: Value between 0 and 4095 - ''' + def set_center(self, center_val): if center_val not in range(4096): print('Servo value not in range [0,4095]') else: self._center = center_val - print('Servo %2i center set to %4i'%(self.id+1,center_val)) + print('Servo %2i center set to %4i' % (self.id+1, center_val)) - def set_max(self,max_val): - ''' - Set Servo max value - Input: Value between 0 and 4095 - ''' + def set_max(self, max_val): if max_val not in range(4096): print('Servo value not in range [0,4095]') else: self._max = max_val - print('Servo %2i max set to %4i'%(self.id+1,max_val)) + print('Servo %2i max set to %4i' % (self.id+1, max_val)) - def set_min(self,min_val): - ''' - Set Servo min value - Input: Value between 0 and 4095 - ''' + def set_min(self, min_val): if min_val not in range(4096): print('Servo value not in range [0,4095]') else: self._min = min_val - print('Servo %2i min set to %4i'%(self.id+1,min_val)) + print('Servo %2i min set to %4i' % (self.id+1, min_val)) -class SpotMicroServoControl(): +class SpotMicroServoControl(Node): def __init__(self): - rospy.loginfo("Setting Up the Spot Micro Servo Control Node...") - - # Set up and title the ros node for this code - rospy.init_node('spot_micro_servo_control') + super().__init__('spot_micro_servo_control') - # Intialize empty servo dictionary self.servos = {} - - # Create a servo dictionary with 12 ServoConvert objects - # keys: integers 0 through 12 - # values: ServoConvert objects for i in range(numServos): self.servos[i] = ServoConvert(id=i) - rospy.loginfo("> Servos corrrectly initialized") + self.get_logger().info("> Servos correctly initialized") - # Create empty ServoArray message with n number of Servos in its array - self._servo_msg = ServoArray() - for i in range(numServos): + self._servo_msg = ServoArray() + for i in range(numServos): self._servo_msg.servos.append(Servo()) - # Create the servo array publisher - self.ros_pub_servo_array = rospy.Publisher("/servos_absolute", ServoArray, queue_size=1) - rospy.loginfo("> Publisher corrrectly initialized") + self.ros_pub_servo_array = self.create_publisher(ServoArray, "/servos_absolute", 1) + self.get_logger().info("> Publisher correctly initialized") - rospy.loginfo("Initialization complete") + self.get_logger().info("Initialization complete") - # Setup terminal input reading, taken from teleop_twist_keyboard self.settings = termios.tcgetattr(sys.stdin) def send_servo_msg(self): - for servo_key, servo_obj in self.servos.iteritems(): + for servo_key, servo_obj in self.servos.items(): self._servo_msg.servos[servo_obj.id].servo = servo_obj.id+1 self._servo_msg.servos[servo_obj.id].value = servo_obj.value - #rospy.loginfo("Sending to %s command %d"%(servo_key, servo_obj.value)) self.ros_pub_servo_array.publish(self._servo_msg) def reset_all_servos_center(self): - ''' - Reset all servos to their center value - ''' for s in self.servos: self.servos[s].value = self.servos[s]._center - + def reset_all_servos_off(self): - '''Set all servos to off/freewheel value (pwm of 0)''' for s in self.servos: self.servos[s].value = 0 - + def getKey(self): tty.setraw(sys.stdin.fileno()) select.select([sys.stdin], [], [], 0) @@ -181,97 +139,85 @@ def getKey(self): return key def run(self): - - # Set all servos to their center values self.reset_all_servos_center() self.send_servo_msg() - # Prompt user with keyboard command information - # Ability to control individual servo to find limits and center values - # and ability to control all servos together - - while not rospy.is_shutdown(): - print(msg) - userInput = raw_input("Command?: ") - - if userInput not in validCmds: - print('Valid command not entered, try again...') - else: - if userInput == 'quit': - print("Ending program...") - print('Final Servo Values') - print('--------------------') - for i in range(numServos): - print('Servo %2i: Min: %4i, Center: %4i, Max: %4i'%(i,self.servos[i]._min,self.servos[i]._center,self.servos[i]._max)) - break - - elif userInput == 'oneServo': - # Reset all servos to center value, and send command - self.reset_all_servos_off() - self.send_servo_msg() - - # First get servo number to command - nSrv = -1 - while (1): - userInput = input('Which servo to control? Enter a number 1 through 12: ') - - if userInput not in range(1,numServos+1): - print("Invalid servo number entered, try again") - else: - nSrv = userInput - 1 - break - - # Loop and act on user command - print('Enter command, q to go back to option select: ') - while (1): - - userInput = self.getKey() - - if userInput == 'q': - break - elif userInput not in keyDict: - print('Key not in valid key commands, try again') - else: - keyDict[userInput](self.servos[nSrv]) - print('Servo %2i cmd: %4i'%(nSrv,self.servos[nSrv].value)) - self.send_servo_msg() - - elif userInput == 'allServos': - # Reset all servos to center value, and send command - self.reset_all_servos_center() - self.send_servo_msg() - - print('Enter command, q to go back to option select: ') - while (1): - - userInput = self.getKey() - - if userInput == 'q': - break - elif userInput not in keyDict: - print('Key not in valid key commands, try again') - elif userInput in ('b','n','m'): - print('Saving values not supported in all servo control mode') - else: - for s in self.servos.values(): - keyDict[userInput](s) - print('All Servos Commanded') - self.send_servo_msg() - - - - - - - - # print self._last_time_cmd_rcv, self.is_controller_connected - # if not self.is_controller_connected: - # self.set_actuators_idle() - - # Set the control rate in Hz - rate = rospy.Rate(10) - rate.sleep() + try: + while rclpy.ok(): + print(msg) + userInput = input("Command?: ") + + if userInput not in validCmds: + print('Valid command not entered, try again...') + else: + if userInput == 'quit': + print("Ending program...") + print('Final Servo Values') + print('--------------------') + for i in range(numServos): + print('Servo %2i: Min: %4i, Center: %4i, Max: %4i' % (i, self.servos[i]._min, self.servos[i]._center, self.servos[i]._max)) + break + + elif userInput == 'oneServo': + self.reset_all_servos_off() + self.send_servo_msg() + + nSrv = -1 + while True: + userInput = input('Which servo to control? Enter a number 1 through 12: ') + + try: + val = int(userInput) + if val not in range(1, numServos+1): + print("Invalid servo number entered, try again") + else: + nSrv = val - 1 + break + except ValueError: + print("Invalid servo number entered, try again") + + print('Enter command, q to go back to option select: ') + while True: + userInput = self.getKey() + + if userInput == 'q': + break + elif userInput not in keyDict: + print('Key not in valid key commands, try again') + else: + keyDict[userInput](self.servos[nSrv]) + print('Servo %2i cmd: %4i' % (nSrv, self.servos[nSrv].value)) + self.send_servo_msg() + + elif userInput == 'allServos': + self.reset_all_servos_center() + self.send_servo_msg() + + print('Enter command, q to go back to option select: ') + while True: + userInput = self.getKey() + + if userInput == 'q': + break + elif userInput not in keyDict: + print('Key not in valid key commands, try again') + elif userInput in ('b','n','m'): + print('Saving values not supported in all servo control mode') + else: + for s in self.servos.values(): + keyDict[userInput](s) + print('All Servos Commanded') + self.send_servo_msg() + + rate = self.create_rate(10) + rate.sleep() + except KeyboardInterrupt: + pass + finally: + termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings) if __name__ == "__main__": - smsc = SpotMicroServoControl() + rclpy.init() + smsc = SpotMicroServoControl() smsc.run() + rclpy.shutdown() diff --git a/spot_micro_keyboard_command/CMakeLists.txt b/spot_micro_keyboard_command/CMakeLists.txt index 59156c0..7123679 100644 --- a/spot_micro_keyboard_command/CMakeLists.txt +++ b/spot_micro_keyboard_command/CMakeLists.txt @@ -1,206 +1,19 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.8) project(spot_micro_keyboard_command) -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - rospy - std_msgs - geometry_msgs -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) +find_package(ament_cmake REQUIRED) -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES spot_micro_keyboard_command -# CATKIN_DEPENDS rospy std_msgs -# DEPENDS system_lib +install(PROGRAMS + scripts/spotMicroKeyboardMove.py + DESTINATION lib/${PROJECT_NAME} ) -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( -# include - ${catkin_INCLUDE_DIRS} +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} ) -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/spot_micro_keyboard_command.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/spot_micro_keyboard_command_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 -# catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -# install(TARGETS ${PROJECT_NAME}_node -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -# install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_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_spot_micro_keyboard_command.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) +ament_package() diff --git a/spot_micro_keyboard_command/launch/keyboard_command.launch b/spot_micro_keyboard_command/launch/keyboard_command.launch deleted file mode 100644 index ccf0d3d..0000000 --- a/spot_micro_keyboard_command/launch/keyboard_command.launch +++ /dev/null @@ -1,27 +0,0 @@ - - - - - - - - - --> - - - - - - - - - - - - - - - - - - diff --git a/spot_micro_keyboard_command/launch/keyboard_command_launch.py b/spot_micro_keyboard_command/launch/keyboard_command_launch.py new file mode 100644 index 0000000..4bce239 --- /dev/null +++ b/spot_micro_keyboard_command/launch/keyboard_command_launch.py @@ -0,0 +1,43 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +def generate_launch_description(): + run_plot = LaunchConfiguration('run_plot') + run_rviz = LaunchConfiguration('run_rviz') + + return LaunchDescription([ + DeclareLaunchArgument( + 'run_plot', + default_value='false', + description='Run python plotting node'), + DeclareLaunchArgument( + 'run_rviz', + default_value='false', + description='Run rviz node'), + + Node( + package='spot_micro_keyboard_command', + executable='spotMicroKeyboardMove.py', + name='spot_micro_keyboard_command_node', + output='screen'), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join( + get_package_share_directory('spot_micro_plot'), + 'launch', + 'start_plotting_launch.py')), + condition=IfCondition(run_plot)), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join( + get_package_share_directory('spot_micro_rviz'), + 'launch', + 'show_model_launch.py')), + condition=IfCondition(run_rviz)), + ]) diff --git a/spot_micro_keyboard_command/package.xml b/spot_micro_keyboard_command/package.xml index 22c0e0a..dd30fff 100644 --- a/spot_micro_keyboard_command/package.xml +++ b/spot_micro_keyboard_command/package.xml @@ -1,68 +1,22 @@ - + + spot_micro_keyboard_command 0.0.0 The spot_micro_keyboard_command package - - - - mike - - - - - TODO + ament_cmake - - - - - + rclpy + std_msgs + geometry_msgs - - - - + ament_lint_auto + ament_lint_common - - - - - - - - - - - - - - - - - - - - - - catkin - rospy - std_msgs - geometry_msgs - rospy - std_msgs - geometry_msgs - rospy - std_msgs - geometry_msgs - - - - - + ament_cmake diff --git a/spot_micro_keyboard_command/scripts/spotMicroKeyboardMove.py b/spot_micro_keyboard_command/scripts/spotMicroKeyboardMove.py index 42abcdd..faa4bb9 100755 --- a/spot_micro_keyboard_command/scripts/spotMicroKeyboardMove.py +++ b/spot_micro_keyboard_command/scripts/spotMicroKeyboardMove.py @@ -1,11 +1,12 @@ -#!/usr/bin/python +#!/usr/bin/env python3 """ -Class for sending keyboard commands to spot micro walk node, control velocity, yaw rate, and walk event +Class for sending keyboard commands to spot micro walk node, control velocity, yaw rate, and walk event """ -import rospy -import sys, select, termios, tty # For terminal keyboard key press reading -from std_msgs.msg import Float32, Bool +import rclpy +from rclpy.node import Node +import sys, select, termios, tty +from std_msgs.msg import Float32, Bool from geometry_msgs.msg import Vector3 from geometry_msgs.msg import Twist from math import pi @@ -19,19 +20,19 @@ walk: Start walk mode and keyboard motion control stand: Stand robot up -Keyboard commands for body motion +Keyboard commands for body motion --------------------------- q w e u - a s d - + a s d + u: Quit body motion command mode and go back to rest mode w: Increment forward speed command / decrease pitch angle a: Increment left speed command / left roll angle s: Increment backward speed command / increase pitch angle d: Increment right speed command / right roll angle - q: Increment body yaw rate command / left yaw angle (negative left, positive right) - e: Increment body yaw rate command / right yaw angle (negative left, positive right) + q: Increment body yaw rate command / left yaw angle (negative left, positive right) + e: Increment body yaw rate command / right yaw angle (negative left, positive right) f: In walk mode, zero out all rate commands. anything else : Prompt again for command @@ -46,73 +47,65 @@ yaw_rate_inc = 3*pi/180 angle_inc = 2.5*pi/180 -class SpotMicroKeyboardControl(): +class SpotMicroKeyboardControl(Node): def __init__(self): + super().__init__('spot_micro_keyboard_control') self._angle_cmd_msg = Vector3() - self._angle_cmd_msg.x = 0 - self._angle_cmd_msg.y = 0 - self._angle_cmd_msg.z = 0 + self._angle_cmd_msg.x = 0.0 + self._angle_cmd_msg.y = 0.0 + self._angle_cmd_msg.z = 0.0 self._speed_cmd_msg = Vector3() - self._speed_cmd_msg.x = 0 - self._speed_cmd_msg.y = 0 - self._speed_cmd_msg.z = 0 + self._speed_cmd_msg.x = 0.0 + self._speed_cmd_msg.y = 0.0 + self._speed_cmd_msg.z = 0.0 self._vel_cmd_msg = Twist() - self._vel_cmd_msg.linear.x = 0 - self._vel_cmd_msg.linear.y = 0 - self._vel_cmd_msg.linear.z = 0 - self._vel_cmd_msg.angular.x = 0 - self._vel_cmd_msg.angular.y = 0 - self._vel_cmd_msg.angular.z = 0 - + self._vel_cmd_msg.linear.x = 0.0 + self._vel_cmd_msg.linear.y = 0.0 + self._vel_cmd_msg.linear.z = 0.0 + self._vel_cmd_msg.angular.x = 0.0 + self._vel_cmd_msg.angular.y = 0.0 + self._vel_cmd_msg.angular.z = 0.0 + self._walk_event_cmd_msg = Bool() - self._walk_event_cmd_msg.data = True # Mostly acts as an event driven action on receipt of a true message + self._walk_event_cmd_msg.data = True self._stand_event_cmd_msg = Bool() self._stand_event_cmd_msg.data = True - + self._idle_event_cmd_msg = Bool() self._idle_event_cmd_msg.data = True - rospy.loginfo("Setting Up the Spot Micro Keyboard Control Node...") + self.get_logger().info("Setting Up the Spot Micro Keyboard Control Node...") - # Set up and title the ros node for this code - rospy.init_node('spot_micro_keyboard_control') + self._ros_pub_angle_cmd = self.create_publisher(Vector3, '/angle_cmd', 1) + self._ros_pub_vel_cmd = self.create_publisher(Twist, '/cmd_vel', 1) + self._ros_pub_walk_cmd = self.create_publisher(Bool, '/walk_cmd', 1) + self._ros_pub_stand_cmd = self.create_publisher(Bool, '/stand_cmd', 1) + self._ros_pub_idle_cmd = self.create_publisher(Bool, '/idle_cmd', 1) - # Create publishers for commanding velocity, angle, and robot states - self._ros_pub_angle_cmd = rospy.Publisher('/angle_cmd',Vector3,queue_size=1) - self._ros_pub_vel_cmd = rospy.Publisher('/cmd_vel',Twist,queue_size=1) - self._ros_pub_walk_cmd = rospy.Publisher('/walk_cmd',Bool, queue_size=1) - self._ros_pub_stand_cmd = rospy.Publisher('/stand_cmd',Bool,queue_size=1) - self._ros_pub_idle_cmd = rospy.Publisher('/idle_cmd',Bool,queue_size=1) + self.get_logger().info("Keyboard control node publishers correctly initialized") - rospy.loginfo("Keyboard control node publishers corrrectly initialized") - - # Setup terminal input reading, taken from teleop_twist_keyboard self.settings = termios.tcgetattr(sys.stdin) - rospy.loginfo("Keyboard control node initialization complete") + self.get_logger().info("Keyboard control node initialization complete") def reset_all_motion_commands_to_zero(self): - '''Reset body motion cmd states to zero and publish zero value body motion commands''' - - self._vel_cmd_msg.linear.x = 0 - self._vel_cmd_msg.linear.y = 0 - self._vel_cmd_msg.linear.z = 0 - self._vel_cmd_msg.angular.x = 0 - self._vel_cmd_msg.angular.y = 0 - self._vel_cmd_msg.angular.z = 0 + self._vel_cmd_msg.linear.x = 0.0 + self._vel_cmd_msg.linear.y = 0.0 + self._vel_cmd_msg.linear.z = 0.0 + self._vel_cmd_msg.angular.x = 0.0 + self._vel_cmd_msg.angular.y = 0.0 + self._vel_cmd_msg.angular.z = 0.0 self._ros_pub_vel_cmd.publish(self._vel_cmd_msg) def reset_all_angle_commands_to_zero(self): - '''Reset angle cmd states to zero and publish them''' - - self._angle_cmd_msg.x = 0 - self._angle_cmd_msg.y = 0 - self._angle_cmd_msg.z = 0 + self._angle_cmd_msg.x = 0.0 + self._angle_cmd_msg.y = 0.0 + self._angle_cmd_msg.z = 0.0 self._ros_pub_angle_cmd.publish(self._angle_cmd_msg) @@ -124,138 +117,130 @@ def getKey(self): return key def run(self): - # Publish all body motion commands to 0 self.reset_all_motion_commands_to_zero() - rospy.loginfo('Main keyboard control loop started.') - - # Main loop - while not rospy.is_shutdown(): - # Prompt user with keyboard command information, and wait for input keystroke - print(msg) - userInput = raw_input("Command?: ") - - if userInput not in valid_cmds: - rospy.logwarn('Invalid keyboard command entered: %s', userInput) - else: - if userInput == 'quit': - rospy.loginfo("Exiting keyboard control node...") - break - - elif userInput == 'stand': - #Publish stand command event - self._ros_pub_stand_cmd.publish(self._stand_event_cmd_msg) - rospy.loginfo('Stand command issued from keyboard.') - - elif userInput == 'idle': - #Publish idle command event - self._ros_pub_idle_cmd.publish(self._idle_event_cmd_msg) - rospy.loginfo('Idle command issued from keyboard.') - - elif userInput == 'angle_cmd': - # Reset all angle commands - self.reset_all_angle_commands_to_zero() - rospy.loginfo('Entering keyboard angle command mode.') - - # Enter loop to act on user command - print('Enter command, u to go back to command select: ') - - while (1): - print('Cmd Values: phi: %1.3f deg, theta: %1.3f deg, psi: %1.3f deg '\ - %(self._angle_cmd_msg.x*180/pi, self._angle_cmd_msg.y*180/pi, self._angle_cmd_msg.z*180/pi)) - - userInput = self.getKey() - - if userInput == 'u': - # Break out of angle command mode - break - - elif userInput not in ('w','a','s','d','q','e','u'): - rospy.logwarn('Invalid keyboard command issued in angle command mode: %s', userInput) - else: - if userInput == 'w': - self._angle_cmd_msg.y = self._angle_cmd_msg.y - angle_inc - self._ros_pub_angle_cmd.publish(self._angle_cmd_msg) - - elif userInput == 's': - self._angle_cmd_msg.y = self._angle_cmd_msg.y + angle_inc - self._ros_pub_angle_cmd.publish(self._angle_cmd_msg) - - elif userInput == 'q': - self._angle_cmd_msg.z = self._angle_cmd_msg.z + angle_inc - self._ros_pub_angle_cmd.publish(self._angle_cmd_msg) - - elif userInput == 'e': - self._angle_cmd_msg.z = self._angle_cmd_msg.z - angle_inc - self._ros_pub_angle_cmd.publish(self._angle_cmd_msg) - - elif userInput == 'a': - self._angle_cmd_msg.x = self._angle_cmd_msg.x - angle_inc - self._ros_pub_angle_cmd.publish(self._angle_cmd_msg) - - elif userInput == 'd': - self._angle_cmd_msg.x = self._angle_cmd_msg.x + angle_inc - self._ros_pub_angle_cmd.publish(self._angle_cmd_msg) - - elif userInput == 'walk': - # Reset all body motion commands to zero - self.reset_all_motion_commands_to_zero() - - # Publish walk event - self._ros_pub_walk_cmd.publish(self._walk_event_cmd_msg) - rospy.loginfo('Idle command issued from keyboard.') - - # Prompt user with info and enter loop to act on user command - print('Enter command, u to go back to stand mode: ') - - while (1): - print('Cmd Values: x speed: %1.3f m/s, y speed: %1.3f m/s, yaw rate: %1.3f deg/s '\ - %(self._vel_cmd_msg.linear.x,self._vel_cmd_msg.linear.y,self._vel_cmd_msg.angular.z*180/pi)) - - userInput = self.getKey() - - if userInput == 'u': - # Send stand event message, this will take robot back to standing mode - self._ros_pub_stand_cmd.publish(self._stand_event_cmd_msg) - rospy.loginfo('Stand command issued from keyboard.') - break - - elif userInput not in ('w','a','s','d','q','e','u','f'): - print('Key not in valid key commands, try again') - rospy.logwarn('Invalid keyboard command issued in walk mode: %s', userInput) - else: - if userInput == 'w': - self._vel_cmd_msg.linear.x = self._vel_cmd_msg.linear.x + speed_inc - self._ros_pub_vel_cmd.publish(self._vel_cmd_msg) - - elif userInput == 's': - self._vel_cmd_msg.linear.x = self._vel_cmd_msg.linear.x - speed_inc - self._ros_pub_vel_cmd.publish(self._vel_cmd_msg) - - elif userInput == 'a': - self._vel_cmd_msg.linear.y = self._vel_cmd_msg.linear.y - speed_inc - self._ros_pub_vel_cmd.publish(self._vel_cmd_msg) - - elif userInput == 'd': - self._vel_cmd_msg.linear.y = self._vel_cmd_msg.linear.y + speed_inc - self._ros_pub_vel_cmd.publish(self._vel_cmd_msg) - - elif userInput == 'q': - self._vel_cmd_msg.angular.z = self._vel_cmd_msg.angular.z - yaw_rate_inc - self._ros_pub_vel_cmd.publish(self._vel_cmd_msg) - - elif userInput == 'e': - self._vel_cmd_msg.angular.z = self._vel_cmd_msg.angular.z + yaw_rate_inc - self._ros_pub_vel_cmd.publish(self._vel_cmd_msg) - - elif userInput == 'f': - self._vel_cmd_msg.linear.x = 0 - self._vel_cmd_msg.linear.y = 0 - self._vel_cmd_msg.angular.z = 0 - - self._ros_pub_vel_cmd.publish(self._vel_cmd_msg) - rospy.loginfo('Command issued to zero all rate commands.') - - + self.get_logger().info('Main keyboard control loop started.') + + try: + while rclpy.ok(): + print(msg) + userInput = input("Command?: ") + + if userInput not in valid_cmds: + self.get_logger().warn('Invalid keyboard command entered: %s' % userInput) + else: + if userInput == 'quit': + self.get_logger().info("Exiting keyboard control node...") + break + + elif userInput == 'stand': + self._ros_pub_stand_cmd.publish(self._stand_event_cmd_msg) + self.get_logger().info('Stand command issued from keyboard.') + + elif userInput == 'idle': + self._ros_pub_idle_cmd.publish(self._idle_event_cmd_msg) + self.get_logger().info('Idle command issued from keyboard.') + + elif userInput == 'angle_cmd': + self.reset_all_angle_commands_to_zero() + self.get_logger().info('Entering keyboard angle command mode.') + print('Enter command, u to go back to command select: ') + + while rclpy.ok(): + print('Cmd Values: phi: %1.3f deg, theta: %1.3f deg, psi: %1.3f deg ' \ + % (self._angle_cmd_msg.x*180/pi, self._angle_cmd_msg.y*180/pi, self._angle_cmd_msg.z*180/pi)) + + userInput = self.getKey() + + if userInput == 'u': + break + + elif userInput not in ('w','a','s','d','q','e','u'): + self.get_logger().warn('Invalid keyboard command issued in angle command mode: %s' % userInput) + else: + if userInput == 'w': + self._angle_cmd_msg.y = self._angle_cmd_msg.y - angle_inc + self._ros_pub_angle_cmd.publish(self._angle_cmd_msg) + + elif userInput == 's': + self._angle_cmd_msg.y = self._angle_cmd_msg.y + angle_inc + self._ros_pub_angle_cmd.publish(self._angle_cmd_msg) + + elif userInput == 'q': + self._angle_cmd_msg.z = self._angle_cmd_msg.z + angle_inc + self._ros_pub_angle_cmd.publish(self._angle_cmd_msg) + + elif userInput == 'e': + self._angle_cmd_msg.z = self._angle_cmd_msg.z - angle_inc + self._ros_pub_angle_cmd.publish(self._angle_cmd_msg) + + elif userInput == 'a': + self._angle_cmd_msg.x = self._angle_cmd_msg.x - angle_inc + self._ros_pub_angle_cmd.publish(self._angle_cmd_msg) + + elif userInput == 'd': + self._angle_cmd_msg.x = self._angle_cmd_msg.x + angle_inc + self._ros_pub_angle_cmd.publish(self._angle_cmd_msg) + + elif userInput == 'walk': + self.reset_all_motion_commands_to_zero() + self._ros_pub_walk_cmd.publish(self._walk_event_cmd_msg) + self.get_logger().info('Walk command issued from keyboard.') + + print('Enter command, u to go back to stand mode: ') + + while rclpy.ok(): + print('Cmd Values: x speed: %1.3f m/s, y speed: %1.3f m/s, yaw rate: %1.3f deg/s ' \ + % (self._vel_cmd_msg.linear.x, self._vel_cmd_msg.linear.y, self._vel_cmd_msg.angular.z*180/pi)) + + userInput = self.getKey() + + if userInput == 'u': + self._ros_pub_stand_cmd.publish(self._stand_event_cmd_msg) + self.get_logger().info('Stand command issued from keyboard.') + break + + elif userInput not in ('w','a','s','d','q','e','u','f'): + print('Key not in valid key commands, try again') + self.get_logger().warn('Invalid keyboard command issued in walk mode: %s' % userInput) + else: + if userInput == 'w': + self._vel_cmd_msg.linear.x = self._vel_cmd_msg.linear.x + speed_inc + self._ros_pub_vel_cmd.publish(self._vel_cmd_msg) + + elif userInput == 's': + self._vel_cmd_msg.linear.x = self._vel_cmd_msg.linear.x - speed_inc + self._ros_pub_vel_cmd.publish(self._vel_cmd_msg) + + elif userInput == 'a': + self._vel_cmd_msg.linear.y = self._vel_cmd_msg.linear.y - speed_inc + self._ros_pub_vel_cmd.publish(self._vel_cmd_msg) + + elif userInput == 'd': + self._vel_cmd_msg.linear.y = self._vel_cmd_msg.linear.y + speed_inc + self._ros_pub_vel_cmd.publish(self._vel_cmd_msg) + + elif userInput == 'q': + self._vel_cmd_msg.angular.z = self._vel_cmd_msg.angular.z - yaw_rate_inc + self._ros_pub_vel_cmd.publish(self._vel_cmd_msg) + + elif userInput == 'e': + self._vel_cmd_msg.angular.z = self._vel_cmd_msg.angular.z + yaw_rate_inc + self._ros_pub_vel_cmd.publish(self._vel_cmd_msg) + + elif userInput == 'f': + self._vel_cmd_msg.linear.x = 0.0 + self._vel_cmd_msg.linear.y = 0.0 + self._vel_cmd_msg.angular.z = 0.0 + + self._ros_pub_vel_cmd.publish(self._vel_cmd_msg) + self.get_logger().info('Command issued to zero all rate commands.') + except KeyboardInterrupt: + pass + finally: + termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings) + if __name__ == "__main__": - smkc = SpotMicroKeyboardControl() + rclpy.init() + smkc = SpotMicroKeyboardControl() smkc.run() + rclpy.shutdown() diff --git a/spot_micro_launch/CMakeLists.txt b/spot_micro_launch/CMakeLists.txt index a6f1916..ef87286 100644 --- a/spot_micro_launch/CMakeLists.txt +++ b/spot_micro_launch/CMakeLists.txt @@ -1,212 +1,14 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.8) project(spot_micro_launch) -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - hector_geotiff - hector_geotiff_plugins - hector_map_server - hector_mapping - hector_trajectory_server - rviz - tf - tf2 - topic_tools -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES spot_micro_launch -# CATKIN_DEPENDS hector_geotiff hector_geotiff_plugins hector_map_server hector_mapping hector_trajectory_server rviz tf tf2 topic_tools -# DEPENDS system_lib -) - -########### -## Build ## -########### +find_package(ament_cmake REQUIRED) -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( -# include - ${catkin_INCLUDE_DIRS} +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} ) -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/spot_micro_launch.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/spot_micro_launch_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 -# catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -# install(TARGETS ${PROJECT_NAME}_node -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -# install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_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_spot_micro_launch.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) +ament_package() diff --git a/spot_micro_launch/launch/keyboard_control_and_rviz.launch b/spot_micro_launch/launch/keyboard_control_and_rviz.launch deleted file mode 100644 index d06c36e..0000000 --- a/spot_micro_launch/launch/keyboard_control_and_rviz.launch +++ /dev/null @@ -1,22 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/spot_micro_launch/launch/keyboard_control_and_rviz_launch.py b/spot_micro_launch/launch/keyboard_control_and_rviz_launch.py new file mode 100644 index 0000000..7a6efeb --- /dev/null +++ b/spot_micro_launch/launch/keyboard_control_and_rviz_launch.py @@ -0,0 +1,37 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition, UnlessCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + +def generate_launch_description(): + rviz_slam = LaunchConfiguration('rviz_slam') + + return LaunchDescription([ + DeclareLaunchArgument( + 'rviz_slam', + default_value='false', + description='Run RViz with SLAM visualization'), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join( + get_package_share_directory('spot_micro_keyboard_command'), + 'launch', + 'keyboard_command_launch.py'))), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join( + get_package_share_directory('spot_micro_rviz'), + 'launch', + 'slam_launch.py')), + condition=IfCondition(rviz_slam)), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join( + get_package_share_directory('spot_micro_rviz'), + 'launch', + 'show_model_launch.py')), + condition=UnlessCondition(rviz_slam)), + ]) diff --git a/spot_micro_launch/launch/motion_control_and_hector_slam.launch b/spot_micro_launch/launch/motion_control_and_hector_slam.launch deleted file mode 100644 index 9669fc9..0000000 --- a/spot_micro_launch/launch/motion_control_and_hector_slam.launch +++ /dev/null @@ -1,78 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/spot_micro_launch/launch/motion_control_and_hector_slam_launch.py b/spot_micro_launch/launch/motion_control_and_hector_slam_launch.py new file mode 100644 index 0000000..fbb97bc --- /dev/null +++ b/spot_micro_launch/launch/motion_control_and_hector_slam_launch.py @@ -0,0 +1,75 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition, UnlessCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +def generate_launch_description(): + run_post_proc = LaunchConfiguration('run_post_proc') + geotiff_map_file_path = LaunchConfiguration('geotiff_map_file_path') + + return LaunchDescription([ + DeclareLaunchArgument( + 'run_post_proc', + default_value='false', + description='Run only hector_mapping in post processing mode'), + DeclareLaunchArgument( + 'geotiff_map_file_path', + default_value='', + description='Path for geotiff map output'), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join( + get_package_share_directory('spot_micro_motion_cmd'), + 'launch', + 'motion_cmd_launch.py')), + condition=UnlessCondition(run_post_proc)), + + # RPLIDAR node - parameters may need adjustment based on your ROS2 rplidar_ros version + Node( + package='rplidar_ros', + executable='rplidar_node', + name='rplidarNode', + output='screen', + parameters=[{ + 'serial_port': '/dev/ttyUSB0', + 'serial_baudrate': 115200, + 'frame_id': 'lidar_link', + 'inverted': False, + 'angle_compensate': True, + }], + condition=UnlessCondition(run_post_proc)), + + # Hector mapping - may require external ROS2 port of hector_slam + Node( + package='hector_mapping', + executable='hector_mapping', + name='hector_mapping', + output='screen', + parameters=[{ + 'map_frame': 'map', + 'base_frame': 'base_footprint', + 'odom_frame': 'odom', + 'use_tf_scan_transformation': True, + 'use_tf_pose_start_estimate': False, + 'pub_map_odom_transform': True, + 'map_resolution': 0.050, + 'map_size': 2048, + 'map_start_x': 0.5, + 'map_start_y': 0.5, + 'map_multi_res_levels': 2, + 'update_factor_free': 0.4, + 'update_factor_occupied': 0.9, + 'map_update_distance_thresh': 0.4, + 'map_update_angle_thresh': 0.06, + 'laser_z_min_value': -1.0, + 'laser_z_max_value': 1.0, + 'advertise_map_service': True, + 'scan_subscriber_queue_size': 5, + 'scan_topic': 'scan', + 'tf_map_scanmatch_transform_frame_name': 'scanmatcher_frame', + }]), + ]) diff --git a/spot_micro_launch/package.xml b/spot_micro_launch/package.xml index 1e26096..6185b07 100644 --- a/spot_micro_launch/package.xml +++ b/spot_micro_launch/package.xml @@ -1,89 +1,18 @@ - + + spot_micro_launch 0.0.0 The spot_micro_launch package - - - - mike - - - - - TODO + ament_cmake - - - - - - - - - - + ament_lint_auto + ament_lint_common - - - - - - - - - - - - - - - - - - - - - - catkin - hector_geotiff - hector_geotiff_plugins - hector_map_server - hector_mapping - hector_trajectory_server - rviz - tf - tf2 - topic_tools - rplidar_ros - hector_geotiff - hector_geotiff_plugins - hector_map_server - hector_mapping - hector_trajectory_server - rviz - tf - tf2 - topic_tools - rplidar_ros - hector_geotiff - hector_geotiff_plugins - hector_map_server - hector_mapping - hector_trajectory_server - rviz - tf - tf2 - topic_tools - rplidar_ros - - - - - + ament_cmake diff --git a/spot_micro_motion_cmd/CMakeLists.txt b/spot_micro_motion_cmd/CMakeLists.txt index 8441136..9068efb 100644 --- a/spot_micro_motion_cmd/CMakeLists.txt +++ b/spot_micro_motion_cmd/CMakeLists.txt @@ -1,226 +1,82 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.8) project(spot_micro_motion_cmd) -## Compile as C++11, supported in ROS Kinetic and newer -add_compile_options(-std=c++14) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - roscpp - std_msgs - geometry_msgs - i2cpwm_board - tf2 - tf2_ros - tf2_geometry_msgs -) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(i2cpwm_board REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(Eigen3 REQUIRED) add_subdirectory(libs/spot_micro_kinematics_cpp) -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - INCLUDE_DIRS include/spot_micro_motion_cmd -# LIBRARIES spot_micro_motion_cmd - CATKIN_DEPENDS roscpp std_msgs geometry_msgs tf2 tf2_ros -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations include_directories( include/spot_micro_motion_cmd src/smfsm src/rate_limited_first_order_filter - ${catkin_INCLUDE_DIRS} ) -## Declare a C++ library -add_library(smfsm - src/smfsm/spot_micro_state.cpp - src/smfsm/spot_micro_idle.cpp - src/smfsm/spot_micro_stand.cpp - src/smfsm/spot_micro_transition_stand.cpp - src/smfsm/spot_micro_transition_idle.cpp - src/smfsm/spot_micro_walk.cpp +add_library(smfsm + src/smfsm/spot_micro_state.cpp + src/smfsm/spot_micro_idle.cpp + src/smfsm/spot_micro_stand.cpp + src/smfsm/spot_micro_transition_stand.cpp + src/smfsm/spot_micro_transition_idle.cpp + src/smfsm/spot_micro_walk.cpp ) target_link_libraries(smfsm - spot_micro_kinematics + spot_micro_kinematics ) -## 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/spot_micro_motion_cmd.cpp src/spot_micro_motion_cmd_node.cpp src/utils.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} - smfsm - spot_micro_kinematics +ament_target_dependencies(smfsm + rclcpp + std_msgs + geometry_msgs + i2cpwm_board + tf2 + tf2_ros + tf2_geometry_msgs + tf2_eigen ) -############# -## 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 -# catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -# install(TARGETS ${PROJECT_NAME}_node -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -# install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_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} -# ) +add_executable(${PROJECT_NAME}_node + src/spot_micro_motion_cmd.cpp + src/spot_micro_motion_cmd_node.cpp + src/utils.cpp +) -############# -## Testing ## -############# +target_link_libraries(${PROJECT_NAME}_node + smfsm + spot_micro_kinematics +) -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_spot_micro_motion_cmd.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() +ament_target_dependencies(${PROJECT_NAME}_node + rclcpp + std_msgs + geometry_msgs + i2cpwm_board + tf2 + tf2_ros + tf2_geometry_msgs + tf2_eigen +) -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) +install(TARGETS ${PROJECT_NAME}_node + DESTINATION lib/${PROJECT_NAME} +) +install(DIRECTORY config launch + DESTINATION share/${PROJECT_NAME} +) +ament_package() diff --git a/spot_micro_motion_cmd/config/spot_micro_motion_cmd.yaml b/spot_micro_motion_cmd/config/spot_micro_motion_cmd.yaml index 6294492..70407e1 100644 --- a/spot_micro_motion_cmd/config/spot_micro_motion_cmd.yaml +++ b/spot_micro_motion_cmd/config/spot_micro_motion_cmd.yaml @@ -1,106 +1,55 @@ -# Configuration parameters - -# Robot wireframe size parameters -# Lengths representening a wireframe model of the robot. All lengths joint to joint -hip_link_length: 0.055 # Straight line distance of the hip link (horizontal leg link) -upper_leg_link_length: 0.1075 # Straight line distance of the upper leg link, joint to joint -lower_leg_link_length: 0.130 # Straight line distance of the lower leg link, joint to joint -body_width: 0.078 # Horizontal width between hip joints -body_length: 0.186 # Length between shoulder joints - -# Stance parameters -default_stand_height: 0.155 # Height of robot body center when standing -stand_front_x_offset: 0.015 # Fwb/back offset of front two legs from default stance -stand_back_x_offset: -0.000 # Fwd/back offset of back two legs from default stance -#stand_front_x_offset: -0.010 # Offset better tuned for trot gait -#stand_back_x_offset: -0.010 # Offset better tuned for trot gait -lie_down_height: 0.083 # Height of body center when sitting -lie_down_foot_x_offset: 0.065 # Fwd/back offset of all(?) feet from default stance when sitting - -# Servo parameters -num_servos: 12 -servo_max_angle_deg: 82.5 -RF_3: {num: 1, center: 306,range: 372,direction: 1, center_angle_deg: 88.2} -RF_2: {num: 2, center: 306,range: 389,direction: 1, center_angle_deg: -27.6} -RF_1: {num: 3, center: 306,range: 396,direction: -1, center_angle_deg: -5.4} -RB_3: {num: 4, center: 306,range: 396,direction: 1, center_angle_deg: 85.8} -RB_2: {num: 5, center: 306,range: 396,direction: 1, center_angle_deg: -35.4} -RB_1: {num: 6, center: 306,range: 411,direction: 1, center_angle_deg: -4.4} -LB_3: {num: 7, center: 306,range: 390,direction: 1, center_angle_deg: -73.9} -LB_2: {num: 8, center: 306,range: 392,direction: 1, center_angle_deg: 38.7} -LB_1: {num: 9, center: 306,range: 374,direction: -1, center_angle_deg: -0.4} -LF_3: {num: 10, center: 306,range: 387,direction: 1, center_angle_deg: -82.8} -LF_2: {num: 11, center: 306,range: 397,direction: 1, center_angle_deg: 38.6} -LF_1: {num: 12, center: 306,range: 389,direction: 1, center_angle_deg: -7.6} - -# Control Parameters -transit_tau: 0.3 # Time constant in seconds for first order filters during transition state -transit_rl: 0.06 # Body motion rate limit in m/s for transition state -transit_angle_rl: 0.35 # Body motion angular rate limit in rad/s for transition state -max_fwd_velocity: 0.4 -max_side_velocity: 0.4 -max_yaw_rate: 0.35 - -# Gait parameters -z_clearance: 0.050 # Max height of foot during swing phase. Increase to have feet go - # higher during swing. Feet travel in a triangular motion during swing -alpha: 0.5 # Controls whether feet are centered within fwd/back motion -beta: 0.5 # Controls whether feet are centered within side to side motion -foot_height_time_constant: 0.02 - -################## -# GAIT SELECTION # -################## -# Uncomment one set of gait parameters for the desired -# gait to use, and comment out the other set. - -############################################ -############## 8 Phase Gait ################ -############################################ -# Only one leg is ever in swing phase at one time -# 4 swing phases + 4 body motion phases to balance body over -# three legs that will remain in contact with ground -num_phases: 8 -rb_contact_phases: [1, 0, 1, 1, 1, 1, 1, 1] -rf_contact_phases: [1, 1, 1, 0, 1, 1, 1, 1] -lf_contact_phases: [1, 1, 1, 1, 1, 1, 1, 0] -lb_contact_phases: [1, 1, 1, 1, 1, 0, 1, 1] -overlap_time: 0.0 -swing_time: 0.36 -body_shift_phases: [1, 2, 3, 4, 5, 6, 7, 8] -fwd_body_balance_shift: 0.035 # Amount the body center will shift forward when walking -back_body_balance_shift: 0.005 # Amount the body center will shift backward when walking -side_body_balance_shift: 0.015 # Amount the body center will shift side to side when walking - -################################################# -############## 4 Phase Trot Gait ################ -################################################# -# 4 Phase gait where the two diagonally oposite legs move in -# tandem together during swing phase -# 2 swing phases + 2 phases where all four feet are on the ground -# num_phases: 4 -# rb_contact_phases: [1, 0, 1, 1] -# rf_contact_phases: [1, 1, 1, 0] -# lf_contact_phases: [1, 0, 1, 1] -# lb_contact_phases: [1, 1, 1, 0] -# overlap_time: 0.18 -# swing_time: 0.18 -# body_shift_phases: [0,0,0,0] -# fwd_body_balance_shift: 0 -# back_body_balance_shift: 0 -# side_body_balance_shift: 0 - -# TF Parameter -lidar_x_pos: 0.045 # X offset of lidar coordinate frame center from robot body center -lidar_y_pos: 0.0 # Y offset of lidar coordinate frame center from robot body center -lidar_z_pos: 0.085 # Z offset of lidar coordinate frame center from robot body center -lidar_yaw_angle: 180 # Angle in degrees of yaw angle of the mounted lidar. Pitch and roll are assumed to be 0 - -# Node parameters -dt: 0.02 # Loop time of main loop, seconds (0.02 s is 50 hz) -publish_odom: true # Determines whether odometry is published or not - -# Debug mode -debug_mode: false # Prints out additional information -plot_mode: false # Publishes data for the python plotting node to work - +spot_micro_motion_cmd: + ros__parameters: + hip_link_length: 0.055 + upper_leg_link_length: 0.1075 + lower_leg_link_length: 0.130 + body_width: 0.078 + body_length: 0.186 + default_stand_height: 0.155 + stand_front_x_offset: 0.015 + stand_back_x_offset: -0.000 + lie_down_height: 0.083 + lie_down_foot_x_offset: 0.065 + num_servos: 12 + servo_max_angle_deg: 82.5 + RF_3: {num: 1, center: 306, range: 372, direction: 1, center_angle_deg: 88.2} + RF_2: {num: 2, center: 306, range: 389, direction: 1, center_angle_deg: -27.6} + RF_1: {num: 3, center: 306, range: 396, direction: -1, center_angle_deg: -5.4} + RB_3: {num: 4, center: 306, range: 396, direction: 1, center_angle_deg: 85.8} + RB_2: {num: 5, center: 306, range: 396, direction: 1, center_angle_deg: -35.4} + RB_1: {num: 6, center: 306, range: 411, direction: 1, center_angle_deg: -4.4} + LB_3: {num: 7, center: 306, range: 390, direction: 1, center_angle_deg: -73.9} + LB_2: {num: 8, center: 306, range: 392, direction: 1, center_angle_deg: 38.7} + LB_1: {num: 9, center: 306, range: 374, direction: -1, center_angle_deg: -0.4} + LF_3: {num: 10, center: 306, range: 387, direction: 1, center_angle_deg: -82.8} + LF_2: {num: 11, center: 306, range: 397, direction: 1, center_angle_deg: 38.6} + LF_1: {num: 12, center: 306, range: 389, direction: 1, center_angle_deg: -7.6} + transit_tau: 0.3 + transit_rl: 0.06 + transit_angle_rl: 0.35 + max_fwd_velocity: 0.4 + max_side_velocity: 0.4 + max_yaw_rate: 0.35 + z_clearance: 0.050 + alpha: 0.5 + beta: 0.5 + foot_height_time_constant: 0.02 + num_phases: 8 + rb_contact_phases: [1, 0, 1, 1, 1, 1, 1, 1] + rf_contact_phases: [1, 1, 1, 0, 1, 1, 1, 1] + lf_contact_phases: [1, 1, 1, 1, 1, 1, 1, 0] + lb_contact_phases: [1, 1, 1, 1, 1, 0, 1, 1] + overlap_time: 0.0 + swing_time: 0.36 + body_shift_phases: [1, 2, 3, 4, 5, 6, 7, 8] + fwd_body_balance_shift: 0.035 + back_body_balance_shift: 0.005 + side_body_balance_shift: 0.015 + lidar_x_pos: 0.045 + lidar_y_pos: 0.0 + lidar_z_pos: 0.085 + lidar_yaw_angle: 180.0 + dt: 0.02 + publish_odom: true + debug_mode: false + plot_mode: false diff --git a/spot_micro_motion_cmd/include/spot_micro_motion_cmd/spot_micro_motion_cmd.h b/spot_micro_motion_cmd/include/spot_micro_motion_cmd/spot_micro_motion_cmd.h index 0723a9b..533af1a 100644 --- a/spot_micro_motion_cmd/include/spot_micro_motion_cmd/spot_micro_motion_cmd.h +++ b/spot_micro_motion_cmd/include/spot_micro_motion_cmd/spot_micro_motion_cmd.h @@ -1,24 +1,23 @@ -#pragma once //designed to include the current source file only once in a single compilation. -#ifndef SPOT_MICRO_MOTION_CMD //usd for conditional compiling. +#pragma once +#ifndef SPOT_MICRO_MOTION_CMD #define SPOT_MICRO_MOTION_CMD -#include +#include #include #include -#include "std_msgs/Bool.h" -#include "std_msgs/String.h" -#include "geometry_msgs/Vector3.h" -#include "geometry_msgs/Twist.h" -#include "std_msgs/Float32MultiArray.h" -#include "i2cpwm_board/Servo.h" -#include "i2cpwm_board/ServoArray.h" +#include +#include +#include +#include +#include +#include +#include +#include #include "command.h" #include "spot_micro_kinematics/spot_micro_kinematics.h" #include "spot_micro_state.h" -// Define a configuration struct -// To hold configuration parameters from parameter server/config file struct SpotMicroNodeConfig { smk::SpotMicroConfig smc; float default_stand_height; @@ -42,19 +41,19 @@ struct SpotMicroNodeConfig { float alpha; float beta; int num_phases; - std::vector rb_contact_phases; - std::vector rf_contact_phases; - std::vector lf_contact_phases; - std::vector lb_contact_phases; + std::vector rb_contact_phases; + std::vector rf_contact_phases; + std::vector lf_contact_phases; + std::vector lb_contact_phases; float overlap_time; float swing_time; int overlap_ticks; int swing_ticks; int stance_ticks; - std::vector phase_ticks; + std::vector phase_ticks; int phase_length; float foot_height_time_constant; - std::vector body_shift_phases; + std::vector body_shift_phases; float fwd_body_balance_shift; float side_body_balance_shift; float back_body_balance_shift; @@ -65,169 +64,77 @@ struct SpotMicroNodeConfig { float lidar_yaw_angle; }; - -/* defining the class */ -class SpotMicroMotionCmd +class SpotMicroMotionCmd : public rclcpp::Node { public: - // Constructor - SpotMicroMotionCmd(ros::NodeHandle &nh, ros::NodeHandle &pnh); - - // Destructor - ~SpotMicroMotionCmd(); - - // Main loop runner, called periodically at the loop rate - void runOnce(); + SpotMicroMotionCmd(); + ~SpotMicroMotionCmd(); - // Publish a servo configuration message + void runOnce(); bool publishServoConfiguration(); - - // Set servo proprotional message data void setServoCommandMessageData(); - - // Publishes a servo proportional command message - void publishServoProportionalCommand(); - - // Publishes a servo absolute command message with all servos set to a command - // value of 0. This effectively disables the servos (stops them from holding - // position, should just freewheel) + void publishServoProportionalCommand(); void publishZeroServoAbsoluteCommand(); - - // Returns the loaded parameters SpotMicroNodeConfig getNodeConfig(); - - // Returns leg positions representing a neutral stance smk::LegsFootPos getNeutralStance(); - - // Returns leg positions representing a lieing down stance smk::LegsFootPos getLieDownStance(); - - // Manually override and command idle mode, used for shutdown void commandIdle(); - - // Returns current state name std::string getCurrentStateName(); private: - // Declare SpotMicroState a friend so it can access and modify private - // members of this class friend class SpotMicroState; - // Pointer to state object std::unique_ptr state_; - - // Command object for encapsulating external commands - Command cmd_; // Command object, encapsulate commands - - // Spot Micro Kinematics object. Holds kinematic state of robot, and holds - // kinematics operations for setting position/orientation of the robot - smk::SpotMicroKinematics sm_; - - // Spot Micro Node Config object + Command cmd_; + smk::SpotMicroKinematics sm_; SpotMicroNodeConfig smnc_; - - // Holds the body state to be commanded: feet position, body position and - // angles - smk::BodyState body_state_cmd_; - - // Odometry of the robot position and orientation based on integrated rate - // commands. Only x and y position, and yaw angle, will be integrated from - // rate commands + smk::BodyState body_state_cmd_; smk::BodyState robot_odometry_; + std::map servo_cmds_rad_ = { + {"RF_3", 0.0f}, {"RF_2", 0.0f}, {"RF_1", 0.0f}, + {"RB_3", 0.0f}, {"RB_2", 0.0f}, {"RB_1", 0.0f}, + {"LB_3", 0.0f}, {"LB_2", 0.0f}, {"LB_1", 0.0f}, + {"LF_3", 0.0f}, {"LF_2", 0.0f}, {"LF_1", 0.0f} + }; - // Map to hold servo command values in radians - std::map servo_cmds_rad_ = { {"RF_3", 0.0f}, {"RF_2", 0.0f}, {"RF_1", 0.0f}, - {"RB_3", 0.0f}, {"RB_2", 0.0f}, {"RB_1", 0.0f}, - {"LB_3", 0.0f}, {"LB_2", 0.0f}, {"LB_1", 0.0f}, - {"LF_3", 0.0f}, {"LF_2", 0.0f}, {"LF_1", 0.0f} }; - - // Reads parameters from parameter server to initialize spot micro node config - // struct void readInConfigParameters(); - // Servo array message for servo proportional command - i2cpwm_board::ServoArray servo_array_; - - // Servo array message for servo absolute command - i2cpwm_board::ServoArray servo_array_absolute_; - - - // ROS publisher and subscriber handles - ros::NodeHandle nh_; // Defining the ros NodeHandle variable for registrating the same with the master - ros::NodeHandle pnh_; // Private version of node handle - ros::Subscriber stand_sub_; // ros subscriber handle for stand_cmd topic - ros::Subscriber idle_sub_; // ros subscriber handle for idle_cmd topic - ros::Subscriber walk_sub_; - ros::Subscriber vel_cmd_sub_; - ros::Subscriber body_angle_cmd_sub_; - ros::Publisher servos_absolute_pub_; - ros::Publisher servos_proportional_pub_; - ros::Publisher body_state_pub_; - ros::Publisher lcd_vel_cmd_pub_; - ros::Publisher lcd_angle_cmd_pub_; - ros::Publisher lcd_state_pub_; - ros::ServiceClient servos_config_client_; - tf2_ros::TransformBroadcaster transform_br_; - tf2_ros::StaticTransformBroadcaster static_transform_br_; - - // Message for encapsulating robot body state - std_msgs::Float32MultiArray body_state_msg_; - - // Messages to hold robot state information for displaying on LCD monitor - // and for any other monitoring purposes. - std_msgs::String lcd_state_string_msg_; - geometry_msgs::Twist lcd_vel_cmd_msg_; - geometry_msgs::Vector3 lcd_angle_cmd_msg_; - - // Callback method for stand command - void standCommandCallback(const std_msgs::Bool::ConstPtr& msg); - - // Callback method for idle command - void idleCommandCallback(const std_msgs::Bool::ConstPtr& msg); - - // Callback method for walk command - void walkCommandCallback(const std_msgs::Bool::ConstPtr& msg); - - // Callback method for angle command - void angleCommandCallback(const geometry_msgs::Vector3ConstPtr& msg); - - // Callback method for velocity command - // Currently, the only supported commands from this message are - // x and y axis linear velocity, and z axis angular rate - void velCommandCallback(const geometry_msgs::TwistConstPtr& msg); - - // Resets all events if they were true + i2cpwm_board::msg::ServoArray servo_array_; + i2cpwm_board::msg::ServoArray servo_array_absolute_; + + rclcpp::Subscription::SharedPtr stand_sub_; + rclcpp::Subscription::SharedPtr idle_sub_; + rclcpp::Subscription::SharedPtr walk_sub_; + rclcpp::Subscription::SharedPtr vel_cmd_sub_; + rclcpp::Subscription::SharedPtr body_angle_cmd_sub_; + rclcpp::Publisher::SharedPtr servos_absolute_pub_; + rclcpp::Publisher::SharedPtr servos_proportional_pub_; + rclcpp::Publisher::SharedPtr body_state_pub_; + rclcpp::Publisher::SharedPtr lcd_vel_cmd_pub_; + rclcpp::Publisher::SharedPtr lcd_angle_cmd_pub_; + rclcpp::Publisher::SharedPtr lcd_state_pub_; + rclcpp::Client::SharedPtr servos_config_client_; + std::unique_ptr transform_br_; + std::unique_ptr static_transform_br_; + + std_msgs::msg::Float32MultiArray body_state_msg_; + std_msgs::msg::String lcd_state_string_msg_; + geometry_msgs::msg::Twist lcd_vel_cmd_msg_; + geometry_msgs::msg::Vector3 lcd_angle_cmd_msg_; + + void standCommandCallback(const std_msgs::msg::Bool::SharedPtr msg); + void idleCommandCallback(const std_msgs::msg::Bool::SharedPtr msg); + void walkCommandCallback(const std_msgs::msg::Bool::SharedPtr msg); + void angleCommandCallback(const geometry_msgs::msg::Vector3::SharedPtr msg); + void velCommandCallback(const geometry_msgs::msg::Twist::SharedPtr msg); void resetEventCommands(); - - // State Machine Related Methods - // Handle input commands, delegate to state machine void handleInputCommands(); - - // Changes state of the state machine void changeState(std::unique_ptr sms); - - // Publishes body state as a float array for plotting by another node void publishBodyState(); - - // Publish LCD monitor messages void publishLcdMonitorData(); - - // Broadcast static tf2 coordinate frame transformation to /tf_static - // Should only be called once at initalization, as it's only for static - // transformations of the robot model that do not change over time void publishStaticTransforms(); - - // Broadcast dynamic tf2 coordinate frame transformations to /tf - // Will broadcast dynamic robot and leg joint transformations void publishDynamicTransforms(); - - // Integrate robot odometry. The robot doesn't actually have any - // sensed odometry, but an open loop estimate derived from velocity - // commands should still be useful void integrateOdometry(); - - // Calculates the robot odometry coordinate frame Eigen::Affine3d getOdometryTransform(); - }; -#endif +#endif diff --git a/spot_micro_motion_cmd/include/spot_micro_motion_cmd/utils.h b/spot_micro_motion_cmd/include/spot_micro_motion_cmd/utils.h index 87fb6f3..e0d6f50 100644 --- a/spot_micro_motion_cmd/include/spot_micro_motion_cmd/utils.h +++ b/spot_micro_motion_cmd/include/spot_micro_motion_cmd/utils.h @@ -1,29 +1,29 @@ -#pragma once //designed to include the current source file only once in a single compilation. +#pragma once #include +#include #include "tf2/LinearMath/Quaternion.h" -#include "tf2_eigen/tf2_eigen.h" -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" - -// Utility functions for spot mico motion command node +#include "tf2_eigen/tf2_eigen.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +// Utility functions for spot micro motion command node // Convert a Eigen Matrix4f to an Affine3d Eigen::Affine3d matrix4fToAffine3d(const Eigen::Matrix4f& in); - -// Create a ROS tf2 TransformStamped from a Eigen Affine3d, -// parent frame id and child frame id. Stamped with current time, +// Create a ROS tf2 TransformStamped from a Eigen Affine3d, +// parent frame id and child frame id. Stamped with provided time, // so should be broadcast ASAP -geometry_msgs::TransformStamped eigAndFramesToTrans( - const Eigen::Affine3d& transform, - std::string parent_frame_id, std::string child_frame_id); - +geometry_msgs::msg::TransformStamped eigAndFramesToTrans( + const Eigen::Affine3d& transform, + std::string parent_frame_id, std::string child_frame_id, + const rclcpp::Time& stamp); -// Create a transform from a translation, rotation, and parent and -// child frame IDs. Will stamp the transform with ros::Time::now(), +// Create a transform from a translation, rotation, and parent and +// child frame IDs. Will stamp the transform with the provided time, // so the returned transform should be broadcast asap - geometry_msgs::TransformStamped createTransform( - std::string parent_frame_id, std::string child_frame_id, - double x, double y, double z, - double roll, double pitch, double yaw); \ No newline at end of file +geometry_msgs::msg::TransformStamped createTransform( + std::string parent_frame_id, std::string child_frame_id, + double x, double y, double z, + double roll, double pitch, double yaw, + const rclcpp::Time& stamp); diff --git a/spot_micro_motion_cmd/launch/motion_cmd.launch b/spot_micro_motion_cmd/launch/motion_cmd.launch deleted file mode 100644 index 8bf608f..0000000 --- a/spot_micro_motion_cmd/launch/motion_cmd.launch +++ /dev/null @@ -1,40 +0,0 @@ - - - - - - - - - - - - - --> - - - - - - - │walk: Start walk mode and keyboard motion control - - - - - - - - - - - - - - - - - - - - diff --git a/spot_micro_motion_cmd/launch/motion_cmd_launch.py b/spot_micro_motion_cmd/launch/motion_cmd_launch.py new file mode 100644 index 0000000..1e88768 --- /dev/null +++ b/spot_micro_motion_cmd/launch/motion_cmd_launch.py @@ -0,0 +1,54 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition, UnlessCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +def generate_launch_description(): + run_standalone = LaunchConfiguration('run_standalone') + debug_mode = LaunchConfiguration('debug_mode') + run_lcd = LaunchConfiguration('run_lcd') + + config = os.path.join( + get_package_share_directory('spot_micro_motion_cmd'), + 'config', + 'spot_micro_motion_cmd.yaml') + + return LaunchDescription([ + DeclareLaunchArgument( + 'run_standalone', + default_value='false', + description='Run without i2c_pwmboard'), + DeclareLaunchArgument( + 'debug_mode', + default_value='false', + description='Enable debug mode'), + DeclareLaunchArgument( + 'run_lcd', + default_value='false', + description='Run lcd monitor node'), + + Node( + package='i2cpwm_board', + executable='i2cpwm_board_node', + name='i2cpwm_board_node', + output='screen', + condition=UnlessCondition(run_standalone)), + + Node( + package='spot_micro_motion_cmd', + executable='spot_micro_motion_cmd_node', + name='spot_micro_motion_cmd_node', + output='screen', + parameters=[config]), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join( + get_package_share_directory('lcd_monitor'), + 'launch', + 'lcd_monitor_launch.py')), + condition=IfCondition(run_lcd)), + ]) diff --git a/spot_micro_motion_cmd/package.xml b/spot_micro_motion_cmd/package.xml index 50e7de5..3756925 100644 --- a/spot_micro_motion_cmd/package.xml +++ b/spot_micro_motion_cmd/package.xml @@ -1,80 +1,27 @@ - + + spot_micro_motion_cmd 0.0.0 The spot_micro_motion_cmd package - - - - mike - - - - - TODO + ament_cmake - - - - - + rclcpp + std_msgs + geometry_msgs + i2cpwm_board + tf2 + tf2_ros + tf2_geometry_msgs + tf2_eigen - - - - + ament_lint_auto + ament_lint_common - - - - - - - - - - - - - - - - - - - - - - catkin - i2cpwm_board - roscpp - std_msgs - geometry_msgs - tf2 - tf2_ros - tf2_geometry_msgs - i2cpwm_board - roscpp - std_msgs - geometry_msgs - tf2 - tf2_ros - tf2_geometry_msgs - i2cpwm_board - roscpp - std_msgs - geometry_msgs - tf2 - tf2_ros - tf2_geometry_msgs - - - - - + ament_cmake diff --git a/spot_micro_motion_cmd/src/smfsm/command.h b/spot_micro_motion_cmd/src/smfsm/command.h index a1c259f..9af147a 100644 --- a/spot_micro_motion_cmd/src/smfsm/command.h +++ b/spot_micro_motion_cmd/src/smfsm/command.h @@ -1,12 +1,8 @@ #pragma once -#include "std_msgs/Float32.h" -#include "std_msgs/Bool.h" - - class Command { public: - + float x_vel_cmd_mps_; float y_vel_cmd_mps_; float yaw_rate_cmd_rps_; @@ -17,7 +13,7 @@ class Command { bool idle_cmd_; bool walk_cmd_; bool stand_cmd_; - + // Constructor Command() : x_vel_cmd_mps_(0.0) @@ -32,7 +28,6 @@ class Command { { } bool getStandCmd() const { - // stand cmd status getter. return stand_cmd_; } @@ -42,12 +37,12 @@ class Command { bool getWalkCmd() const { return walk_cmd_; - } - + } + float getXSpeedCmd() const { return x_vel_cmd_mps_; } - + float getYSpeedCmd() const { return y_vel_cmd_mps_; } @@ -76,9 +71,8 @@ class Command { theta_cmd_ = 0; psi_cmd_ = 0; } - + void resetEventCmds() { - // Reset all event commands to false idle_cmd_ = false; walk_cmd_ = false; stand_cmd_ = false; diff --git a/spot_micro_motion_cmd/src/smfsm/spot_micro_state.h b/spot_micro_motion_cmd/src/smfsm/spot_micro_state.h index e8ad075..6cea2e1 100644 --- a/spot_micro_motion_cmd/src/smfsm/spot_micro_state.h +++ b/spot_micro_motion_cmd/src/smfsm/spot_micro_state.h @@ -1,4 +1,6 @@ #pragma once +#include +#include #include "spot_micro_kinematics/spot_micro_kinematics.h" #include "rate_limited_first_order_filter.h" diff --git a/spot_micro_motion_cmd/src/spot_micro_motion_cmd.cpp b/spot_micro_motion_cmd/src/spot_micro_motion_cmd.cpp index e4bf6b3..168b5b4 100644 --- a/spot_micro_motion_cmd/src/spot_micro_motion_cmd.cpp +++ b/spot_micro_motion_cmd/src/spot_micro_motion_cmd.cpp @@ -1,116 +1,78 @@ #include "spot_micro_motion_cmd.h" #include -#include "std_msgs/Float32.h" -#include "std_msgs/Bool.h" -#include "std_msgs/String.h" -#include "std_msgs/Float32MultiArray.h" -#include "geometry_msgs/Vector3.h" -#include "geometry_msgs/Twist.h" -#include "tf2/LinearMath/Quaternion.h" -#include "tf2_eigen/tf2_eigen.h" -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include #include "spot_micro_motion_cmd.h" #include "spot_micro_kinematics/spot_micro_kinematics.h" -#include "i2cpwm_board/Servo.h" -#include "i2cpwm_board/ServoArray.h" -#include "i2cpwm_board/ServoConfig.h" -#include "i2cpwm_board/ServosConfig.h" +#include "i2cpwm_board/msg/servo.hpp" +#include "i2cpwm_board/msg/servo_array.hpp" +#include "i2cpwm_board/msg/servo_config.hpp" +#include "i2cpwm_board/srv/servos_config.hpp" #include "spot_micro_idle.h" #include "utils.h" - using namespace smk; using namespace Eigen; -using namespace geometry_msgs; +using namespace geometry_msgs::msg; typedef std::vector> VectorStringPairs; -// Constructor -SpotMicroMotionCmd::SpotMicroMotionCmd(ros::NodeHandle &nh, ros::NodeHandle &pnh) { +SpotMicroMotionCmd::SpotMicroMotionCmd() : Node("spot_micro_motion_cmd") { - nh_ = nh; - pnh_ = pnh; if (smnc_.debug_mode) { std::cout<<"from Constructor \n"; } - // Initialize Command cmd_ = Command(); - - // Initialize state to Idle state state_ = std::make_unique(); - - // Read in config parameters into smnc_ readInConfigParameters(); - - // Initialize spot micro kinematics object of this class sm_ = smk::SpotMicroKinematics(0.0f, 0.0f, 0.0f, smnc_.smc); - // Set an initial body height and stance cmd for idle mode body_state_cmd_.euler_angs = {.phi = 0.0f, .theta = 0.0f, .psi = 0.0f}; body_state_cmd_.xyz_pos = {.x = 0.0f, .y = smnc_.lie_down_height, .z = 0.0f}; body_state_cmd_.leg_feet_pos = getLieDownStance(); - // Set the spot micro kinematics object to this initial command sm_.setBodyState(body_state_cmd_); - // Set initial odometry state to zero robot_odometry_.euler_angs = {.phi = 0.0f, .theta = 0.0f, .psi = 0.0f}; robot_odometry_.xyz_pos = {.x = 0.0f, .y = 0.0f, .z = 0.0f}; - // Initialize servo array message with 12 servo objects for (int i = 1; i <= smnc_.num_servos; i++) { - i2cpwm_board::Servo temp_servo; + i2cpwm_board::msg::Servo temp_servo; temp_servo.servo = i; - temp_servo.value = 0; + temp_servo.value = 0.0f; servo_array_.servos.push_back(temp_servo); } - // Initialize servo array absolute message with 12 servo object with a value of - // zero, just copy servo_array_msg since it's already correct servo_array_absolute_.servos = servo_array_.servos; - // Initialize publishers and subscribers - // stand cmd event subscriber - stand_sub_ = nh.subscribe("/stand_cmd", 1, &SpotMicroMotionCmd::standCommandCallback, this); - - // idle cmd event subscriber - idle_sub_ = nh.subscribe("/idle_cmd", 1, &SpotMicroMotionCmd::idleCommandCallback, this); - - // walk cmd event subscriber - walk_sub_ = nh.subscribe("/walk_cmd", 1, &SpotMicroMotionCmd::walkCommandCallback, this); - - // body angle command subscriber - body_angle_cmd_sub_ = nh.subscribe("/angle_cmd", 1, &SpotMicroMotionCmd::angleCommandCallback, this); - - // velocity command subscriber - vel_cmd_sub_ = nh.subscribe("/cmd_vel", 1, &SpotMicroMotionCmd::velCommandCallback, this); - - // servos_absolute publisher - servos_absolute_pub_ = nh.advertise("servos_absolute", 1); - - // Servos proportional publisher - servos_proportional_pub_ = nh.advertise("servos_proportional",1); - - // Servos configuration publisher - servos_config_client_ = nh.serviceClient("config_servos"); - - // Body state publisher for plotting - body_state_pub_ = nh.advertise("body_state",1); + stand_sub_ = this->create_subscription( + "/stand_cmd", 1, std::bind(&SpotMicroMotionCmd::standCommandCallback, this, std::placeholders::_1)); + idle_sub_ = this->create_subscription( + "/idle_cmd", 1, std::bind(&SpotMicroMotionCmd::idleCommandCallback, this, std::placeholders::_1)); + walk_sub_ = this->create_subscription( + "/walk_cmd", 1, std::bind(&SpotMicroMotionCmd::walkCommandCallback, this, std::placeholders::_1)); + body_angle_cmd_sub_ = this->create_subscription( + "/angle_cmd", 1, std::bind(&SpotMicroMotionCmd::angleCommandCallback, this, std::placeholders::_1)); + vel_cmd_sub_ = this->create_subscription( + "/cmd_vel", 1, std::bind(&SpotMicroMotionCmd::velCommandCallback, this, std::placeholders::_1)); + + servos_absolute_pub_ = this->create_publisher("servos_absolute", 1); + servos_proportional_pub_ = this->create_publisher("servos_proportional", 1); + servos_config_client_ = this->create_client("config_servos"); + body_state_pub_ = this->create_publisher("body_state", 1); + lcd_state_pub_ = this->create_publisher("lcd_state", 1); + lcd_vel_cmd_pub_ = this->create_publisher("lcd_vel_cmd", 1); + lcd_angle_cmd_pub_ = this->create_publisher("lcd_angle_cmd", 1); - // State string publisher for lcd monitor - lcd_state_pub_ = nh.advertise("lcd_state",1); - - // Velocity command state publisher for lcd monitor - lcd_vel_cmd_pub_ = nh.advertise("lcd_vel_cmd",1); - - // Angle command state publisher for lcd monitor - lcd_angle_cmd_pub_ = nh.advertise("lcd_angle_cmd",1); - - - - // Initialize lcd monitor messages lcd_state_string_msg_.data = "Idle"; lcd_vel_cmd_msg_.linear.x = 0.0f; @@ -119,179 +81,152 @@ SpotMicroMotionCmd::SpotMicroMotionCmd(ros::NodeHandle &nh, ros::NodeHandle &pnh lcd_vel_cmd_msg_.angular.x = 0.0f; lcd_vel_cmd_msg_.angular.y = 0.0f; lcd_vel_cmd_msg_.angular.z = 0.0f; - + lcd_angle_cmd_msg_.x = 0.0f; lcd_angle_cmd_msg_.y = 0.0f; lcd_angle_cmd_msg_.z = 0.0f; - - // Only do if plot mode - // Initialize body state message for plot debug only - // Initialize 18 values to hold xyz positions of the four legs (12) + - // the body x,y,z positions (3), and the body angles (3) for a total of 18 if (smnc_.plot_mode) { for (int i = 0; i < 18; i++) { - body_state_msg_.data.push_back(0.0f); + body_state_msg_.data.push_back(0.0f); } } - // Publish static transforms + transform_br_ = std::make_unique(*this); + static_transform_br_ = std::make_unique(*this); + publishStaticTransforms(); } - -// Destructor method SpotMicroMotionCmd::~SpotMicroMotionCmd() { - if (smnc_.debug_mode) { std::cout<<"from Destructor \n"; } - // Free up the memory assigned from heap } - void SpotMicroMotionCmd::runOnce() { if (smnc_.debug_mode) { std::cout<<"from Runonce \n"; } - // Call method to handle input commands handleInputCommands(); - - // Consume all event commands. - // This resets all event commands if they were true. Doing this enforces a rising edge detection resetEventCommands(); - // Only publish body state message in debug mode if (smnc_.plot_mode) { publishBodyState(); } - // Publish lcd monitor data publishLcdMonitorData(); - - // Broadcast dynamic transforms publishDynamicTransforms(); if (smnc_.publish_odom) { - // Integrate robot odometry integrateOdometry(); } } +bool SpotMicroMotionCmd::publishServoConfiguration() { + auto request = std::make_shared(); -bool SpotMicroMotionCmd::publishServoConfiguration() { - // Create a temporary servo config - i2cpwm_board::ServoConfig temp_servo_config; - i2cpwm_board::ServosConfig temp_servo_config_array; - - // Loop through servo configuration dictionary in smnc_, append servo to - for (std::map>::iterator - iter = smnc_.servo_config.begin(); + for (auto iter = smnc_.servo_config.begin(); iter != smnc_.servo_config.end(); ++iter) { std::map servo_config_params = iter->second; - temp_servo_config.center = servo_config_params["center"]; - temp_servo_config.range = servo_config_params["range"]; - temp_servo_config.servo = servo_config_params["num"]; - temp_servo_config.direction = servo_config_params["direction"]; + i2cpwm_board::msg::ServoConfig temp_servo_config; + temp_servo_config.center = static_cast(servo_config_params["center"]); + temp_servo_config.range = static_cast(servo_config_params["range"]); + temp_servo_config.servo = static_cast(servo_config_params["num"]); + temp_servo_config.direction = static_cast(servo_config_params["direction"]); - // Append to temp_servo_config_array - temp_servo_config_array.request.servos.push_back(temp_servo_config); + request->servos.push_back(temp_servo_config); } - // call the client service, return true if succesfull, false if not - if (!servos_config_client_.call(temp_servo_config_array)) { - if (!smnc_.debug_mode) { - // Only error out if not in debug mode - ROS_ERROR("Failed to call service servo_config"); + while (!servos_config_client_->wait_for_service(std::chrono::seconds(1))) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for service"); return false; } + RCLCPP_INFO(this->get_logger(), "Waiting for servo config service..."); } - return true; -} + auto future = servos_config_client_->async_send_request(request); + if (rclcpp::spin_until_future_complete(shared_from_this(), future) == rclcpp::FutureReturnCode::SUCCESS) { + return future.get()->success; + } + if (!smnc_.debug_mode) { + RCLCPP_ERROR(this->get_logger(), "Failed to call service servo_config"); + } + return false; +} void SpotMicroMotionCmd::setServoCommandMessageData() { - // Set the state of the spot micro kinematics object by setting the foot - // positions, body position, and body orientation. Retrieve joint angles and - // set the servo cmd message data sm_.setBodyState(body_state_cmd_); LegsJointAngles joint_angs = sm_.getLegsJointAngles(); - // Set the angles for the servo command message servo_cmds_rad_["RF_1"] = joint_angs.right_front.ang1; servo_cmds_rad_["RF_2"] = joint_angs.right_front.ang2; servo_cmds_rad_["RF_3"] = joint_angs.right_front.ang3; - + servo_cmds_rad_["RB_1"] = joint_angs.right_back.ang1; servo_cmds_rad_["RB_2"] = joint_angs.right_back.ang2; servo_cmds_rad_["RB_3"] = joint_angs.right_back.ang3; - + servo_cmds_rad_["LF_1"] = joint_angs.left_front.ang1; servo_cmds_rad_["LF_2"] = joint_angs.left_front.ang2; servo_cmds_rad_["LF_3"] = joint_angs.left_front.ang3; - + servo_cmds_rad_["LB_1"] = joint_angs.left_back.ang1; servo_cmds_rad_["LB_2"] = joint_angs.left_back.ang2; servo_cmds_rad_["LB_3"] = joint_angs.left_back.ang3; } - void SpotMicroMotionCmd::publishServoProportionalCommand() { - for (std::map>::iterator - iter = smnc_.servo_config.begin(); + for (auto iter = smnc_.servo_config.begin(); iter != smnc_.servo_config.end(); ++iter) { - + std::string servo_name = iter->first; std::map servo_config_params = iter->second; - - int servo_num = servo_config_params["num"]; + + int servo_num = static_cast(servo_config_params["num"]); float cmd_ang_rad = servo_cmds_rad_[servo_name]; float center_ang_rad = servo_config_params["center_angle_deg"]*M_PI/180.0f; float servo_proportional_cmd = (cmd_ang_rad - center_ang_rad) / (smnc_.servo_max_angle_deg*M_PI/180.0f); - + if (servo_proportional_cmd > 1.0f) { servo_proportional_cmd = 1.0f; - ROS_WARN("Proportional Command above +1.0 was computed, clipped to 1.0"); - ROS_WARN("Joint %s, Angle: %1.2f", servo_name.c_str(), cmd_ang_rad*180.0/M_PI); - + RCLCPP_WARN(this->get_logger(), "Proportional Command above +1.0 was computed, clipped to 1.0"); + RCLCPP_WARN(this->get_logger(), "Joint %s, Angle: %1.2f", servo_name.c_str(), cmd_ang_rad*180.0/M_PI); + } else if (servo_proportional_cmd < -1.0f) { servo_proportional_cmd = -1.0f; - ROS_WARN("Proportional Command below -1.0 was computed, clipped to -1.0"); - ROS_WARN("Joint %s, Angle: %1.2f", servo_name.c_str(), cmd_ang_rad*180.0/M_PI); + RCLCPP_WARN(this->get_logger(), "Proportional Command below -1.0 was computed, clipped to -1.0"); + RCLCPP_WARN(this->get_logger(), "Joint %s, Angle: %1.2f", servo_name.c_str(), cmd_ang_rad*180.0/M_PI); } - + servo_array_.servos[servo_num-1].servo = servo_num; - servo_array_.servos[servo_num-1].value = servo_proportional_cmd; - } + servo_array_.servos[servo_num-1].value = servo_proportional_cmd; + } - // Publish message - servos_proportional_pub_.publish(servo_array_); + servos_proportional_pub_->publish(servo_array_); } - void SpotMicroMotionCmd::publishZeroServoAbsoluteCommand() { - // Publish the servo absolute message - servos_absolute_pub_.publish(servo_array_absolute_); + servos_absolute_pub_->publish(servo_array_absolute_); } - SpotMicroNodeConfig SpotMicroMotionCmd::getNodeConfig() { return smnc_; } - LegsFootPos SpotMicroMotionCmd::getNeutralStance() { - float len = smnc_.smc.body_length; // body length - float width = smnc_.smc.body_width; // body width - float l1 = smnc_.smc.hip_link_length; // liength of the hip link - float f_offset = smnc_.stand_front_x_offset; // x offset for front feet in neutral stance - float b_offset = smnc_.stand_back_x_offset; // y offset for back feet in neutral stance + float len = smnc_.smc.body_length; + float width = smnc_.smc.body_width; + float l1 = smnc_.smc.hip_link_length; + float f_offset = smnc_.stand_front_x_offset; + float b_offset = smnc_.stand_back_x_offset; LegsFootPos neutral_stance; neutral_stance.right_back = {.x = -len/2 + b_offset, .y = 0.0f, .z = width/2 + l1}; @@ -302,11 +237,10 @@ LegsFootPos SpotMicroMotionCmd::getNeutralStance() { return neutral_stance; } - LegsFootPos SpotMicroMotionCmd::getLieDownStance() { - float len = smnc_.smc.body_length; // body length - float width = smnc_.smc.body_width; // body width - float l1 = smnc_.smc.hip_link_length; // length of the hip link + float len = smnc_.smc.body_length; + float width = smnc_.smc.body_width; + float l1 = smnc_.smc.hip_link_length; float x_off = smnc_.lie_down_feet_x_offset; LegsFootPos lie_down_stance; @@ -318,172 +252,183 @@ LegsFootPos SpotMicroMotionCmd::getLieDownStance() { return lie_down_stance; } - void SpotMicroMotionCmd::commandIdle() { cmd_.idle_cmd_ = true; } - std::string SpotMicroMotionCmd::getCurrentStateName() { return state_->getCurrentStateName(); } - void SpotMicroMotionCmd::readInConfigParameters() { - // Read in and save parameters - // Use private node handle for getting params so just the relative - // parameter name can be used (as opposed to the global name, e.g.: - // /spot_micro_motion_cmd/param1 - pnh_.getParam("hip_link_length", smnc_.smc.hip_link_length); - pnh_.getParam("upper_leg_link_length", smnc_.smc.upper_leg_link_length); - pnh_.getParam("lower_leg_link_length", smnc_.smc.lower_leg_link_length); - pnh_.getParam("body_width", smnc_.smc.body_width); - pnh_.getParam("body_length", smnc_.smc.body_length); - pnh_.getParam("default_stand_height", smnc_.default_stand_height); - pnh_.getParam("stand_front_x_offset", smnc_.stand_front_x_offset); - pnh_.getParam("stand_back_x_offset", smnc_.stand_back_x_offset); - pnh_.getParam("lie_down_height", smnc_.lie_down_height); - pnh_.getParam("lie_down_foot_x_offset", smnc_.lie_down_feet_x_offset); - pnh_.getParam("num_servos", smnc_.num_servos); - pnh_.getParam("servo_max_angle_deg", smnc_.servo_max_angle_deg); - pnh_.getParam("transit_tau", smnc_.transit_tau); - pnh_.getParam("transit_rl", smnc_.transit_rl); - pnh_.getParam("transit_angle_rl", smnc_.transit_angle_rl); - pnh_.getParam("dt", smnc_.dt); - pnh_.getParam("debug_mode", smnc_.debug_mode); - pnh_.getParam("plot_mode", smnc_.plot_mode); - pnh_.getParam("max_fwd_velocity", smnc_.max_fwd_velocity); - pnh_.getParam("max_side_velocity", smnc_.max_side_velocity); - pnh_.getParam("max_yaw_rate", smnc_.max_yaw_rate); - pnh_.getParam("z_clearance", smnc_.z_clearance); - pnh_.getParam("alpha", smnc_.alpha); - pnh_.getParam("beta", smnc_.beta); - pnh_.getParam("num_phases", smnc_.num_phases); - pnh_.getParam("rb_contact_phases", smnc_.rb_contact_phases); - pnh_.getParam("rf_contact_phases", smnc_.rf_contact_phases); - pnh_.getParam("lf_contact_phases", smnc_.lf_contact_phases); - pnh_.getParam("lb_contact_phases", smnc_.lb_contact_phases); - pnh_.getParam("overlap_time", smnc_.overlap_time); - pnh_.getParam("swing_time", smnc_.swing_time); - pnh_.getParam("foot_height_time_constant", smnc_.foot_height_time_constant); - pnh_.getParam("body_shift_phases", smnc_.body_shift_phases); - pnh_.getParam("fwd_body_balance_shift", smnc_.fwd_body_balance_shift); - pnh_.getParam("back_body_balance_shift", smnc_.back_body_balance_shift); - pnh_.getParam("side_body_balance_shift", smnc_.side_body_balance_shift); - pnh_.getParam("publish_odom", smnc_.publish_odom); - pnh_.getParam("lidar_x_pos", smnc_.lidar_x_pos); - pnh_.getParam("lidar_y_pos", smnc_.lidar_y_pos); - pnh_.getParam("lidar_z_pos", smnc_.lidar_z_pos); - pnh_.getParam("lidar_yaw_angle", smnc_.lidar_yaw_angle); - - // Derived parameters, round result of division of floats - smnc_.overlap_ticks = round(smnc_.overlap_time / smnc_.dt); - smnc_.swing_ticks = round(smnc_.swing_time / smnc_.dt); - - // 8 Phase gait specific - if (smnc_.num_phases == 8) { + this->declare_parameter("hip_link_length", 0.0); + this->declare_parameter("upper_leg_link_length", 0.0); + this->declare_parameter("lower_leg_link_length", 0.0); + this->declare_parameter("body_width", 0.0); + this->declare_parameter("body_length", 0.0); + this->declare_parameter("default_stand_height", 0.0); + this->declare_parameter("stand_front_x_offset", 0.0); + this->declare_parameter("stand_back_x_offset", 0.0); + this->declare_parameter("lie_down_height", 0.0); + this->declare_parameter("lie_down_foot_x_offset", 0.0); + this->declare_parameter("num_servos", 12); + this->declare_parameter("servo_max_angle_deg", 0.0); + this->declare_parameter("transit_tau", 0.0); + this->declare_parameter("transit_rl", 0.0); + this->declare_parameter("transit_angle_rl", 0.0); + this->declare_parameter("dt", 0.02); + this->declare_parameter("debug_mode", false); + this->declare_parameter("plot_mode", false); + this->declare_parameter("max_fwd_velocity", 0.0); + this->declare_parameter("max_side_velocity", 0.0); + this->declare_parameter("max_yaw_rate", 0.0); + this->declare_parameter("z_clearance", 0.0); + this->declare_parameter("alpha", 0.0); + this->declare_parameter("beta", 0.0); + this->declare_parameter("num_phases", 4); + this->declare_parameter>("rb_contact_phases", {}); + this->declare_parameter>("rf_contact_phases", {}); + this->declare_parameter>("lf_contact_phases", {}); + this->declare_parameter>("lb_contact_phases", {}); + this->declare_parameter("overlap_time", 0.0); + this->declare_parameter("swing_time", 0.0); + this->declare_parameter("foot_height_time_constant", 0.0); + this->declare_parameter>("body_shift_phases", {}); + this->declare_parameter("fwd_body_balance_shift", 0.0); + this->declare_parameter("back_body_balance_shift", 0.0); + this->declare_parameter("side_body_balance_shift", 0.0); + this->declare_parameter("publish_odom", false); + this->declare_parameter("lidar_x_pos", 0.0); + this->declare_parameter("lidar_y_pos", 0.0); + this->declare_parameter("lidar_z_pos", 0.0); + this->declare_parameter("lidar_yaw_angle", 0.0); + + smnc_.smc.hip_link_length = static_cast(this->get_parameter("hip_link_length").as_double()); + smnc_.smc.upper_leg_link_length = static_cast(this->get_parameter("upper_leg_link_length").as_double()); + smnc_.smc.lower_leg_link_length = static_cast(this->get_parameter("lower_leg_link_length").as_double()); + smnc_.smc.body_width = static_cast(this->get_parameter("body_width").as_double()); + smnc_.smc.body_length = static_cast(this->get_parameter("body_length").as_double()); + smnc_.default_stand_height = static_cast(this->get_parameter("default_stand_height").as_double()); + smnc_.stand_front_x_offset = static_cast(this->get_parameter("stand_front_x_offset").as_double()); + smnc_.stand_back_x_offset = static_cast(this->get_parameter("stand_back_x_offset").as_double()); + smnc_.lie_down_height = static_cast(this->get_parameter("lie_down_height").as_double()); + smnc_.lie_down_feet_x_offset = static_cast(this->get_parameter("lie_down_foot_x_offset").as_double()); + smnc_.num_servos = this->get_parameter("num_servos").as_int(); + smnc_.servo_max_angle_deg = static_cast(this->get_parameter("servo_max_angle_deg").as_double()); + smnc_.transit_tau = static_cast(this->get_parameter("transit_tau").as_double()); + smnc_.transit_rl = static_cast(this->get_parameter("transit_rl").as_double()); + smnc_.transit_angle_rl = static_cast(this->get_parameter("transit_angle_rl").as_double()); + smnc_.dt = static_cast(this->get_parameter("dt").as_double()); + smnc_.debug_mode = this->get_parameter("debug_mode").as_bool(); + smnc_.plot_mode = this->get_parameter("plot_mode").as_bool(); + smnc_.max_fwd_velocity = static_cast(this->get_parameter("max_fwd_velocity").as_double()); + smnc_.max_side_velocity = static_cast(this->get_parameter("max_side_velocity").as_double()); + smnc_.max_yaw_rate = static_cast(this->get_parameter("max_yaw_rate").as_double()); + smnc_.z_clearance = static_cast(this->get_parameter("z_clearance").as_double()); + smnc_.alpha = static_cast(this->get_parameter("alpha").as_double()); + smnc_.beta = static_cast(this->get_parameter("beta").as_double()); + smnc_.num_phases = static_cast(this->get_parameter("num_phases").as_int()); + smnc_.rb_contact_phases = this->get_parameter("rb_contact_phases").as_integer_array(); + smnc_.rf_contact_phases = this->get_parameter("rf_contact_phases").as_integer_array(); + smnc_.lf_contact_phases = this->get_parameter("lf_contact_phases").as_integer_array(); + smnc_.lb_contact_phases = this->get_parameter("lb_contact_phases").as_integer_array(); + smnc_.overlap_time = static_cast(this->get_parameter("overlap_time").as_double()); + smnc_.swing_time = static_cast(this->get_parameter("swing_time").as_double()); + smnc_.foot_height_time_constant = static_cast(this->get_parameter("foot_height_time_constant").as_double()); + smnc_.body_shift_phases = this->get_parameter("body_shift_phases").as_integer_array(); + smnc_.fwd_body_balance_shift = static_cast(this->get_parameter("fwd_body_balance_shift").as_double()); + smnc_.back_body_balance_shift = static_cast(this->get_parameter("back_body_balance_shift").as_double()); + smnc_.side_body_balance_shift = static_cast(this->get_parameter("side_body_balance_shift").as_double()); + smnc_.publish_odom = this->get_parameter("publish_odom").as_bool(); + smnc_.lidar_x_pos = static_cast(this->get_parameter("lidar_x_pos").as_double()); + smnc_.lidar_y_pos = static_cast(this->get_parameter("lidar_y_pos").as_double()); + smnc_.lidar_z_pos = static_cast(this->get_parameter("lidar_z_pos").as_double()); + smnc_.lidar_yaw_angle = static_cast(this->get_parameter("lidar_yaw_angle").as_double()); + + smnc_.overlap_ticks = static_cast(round(smnc_.overlap_time / smnc_.dt)); + smnc_.swing_ticks = static_cast(round(smnc_.swing_time / smnc_.dt)); + + if (smnc_.num_phases == 8) { smnc_.stance_ticks = 7 * smnc_.swing_ticks; - smnc_.overlap_ticks = round(smnc_.overlap_time / smnc_.dt); - smnc_.phase_ticks = std::vector + smnc_.overlap_ticks = static_cast(round(smnc_.overlap_time / smnc_.dt)); + smnc_.phase_ticks = std::vector {smnc_.swing_ticks, smnc_.swing_ticks, smnc_.swing_ticks, smnc_.swing_ticks, - smnc_.swing_ticks, smnc_.swing_ticks, smnc_.swing_ticks, smnc_.swing_ticks}; + smnc_.swing_ticks, smnc_.swing_ticks, smnc_.swing_ticks, smnc_.swing_ticks}; smnc_.phase_length = smnc_.num_phases * smnc_.swing_ticks; - } else { - // 4 phase gait specific + } else { smnc_.stance_ticks = 2 * smnc_.overlap_ticks + smnc_.swing_ticks; - smnc_.overlap_ticks = round(smnc_.overlap_time / smnc_.dt); - smnc_.phase_ticks = std::vector + smnc_.overlap_ticks = static_cast(round(smnc_.overlap_time / smnc_.dt)); + smnc_.phase_ticks = std::vector {smnc_.overlap_ticks, smnc_.swing_ticks, smnc_.overlap_ticks, smnc_.swing_ticks}; smnc_.phase_length = 2 * smnc_.swing_ticks + 2 * smnc_.overlap_ticks; } - // Temporary map for populating map in smnc_ - std::map temp_map; - - // Iterate over servo names, as defined in the map servo_cmds_rad, to populate - // the servo config map in smnc_ - for(std::map::iterator - iter = servo_cmds_rad_.begin(); - iter != servo_cmds_rad_.end(); - ++iter) { - - std::string servo_name = iter->first; // Get key, string of the servo name - - pnh_.getParam(servo_name, temp_map); // Read the parameter. Parameter name must match servo name - smnc_.servo_config[servo_name] = temp_map; // Assing in servo config to map in the struct + for (auto iter = servo_cmds_rad_.begin(); + iter != servo_cmds_rad_.end(); + ++iter) { + + std::string servo_name = iter->first; + this->declare_parameter(servo_name + ".num", 0); + this->declare_parameter(servo_name + ".center", 0); + this->declare_parameter(servo_name + ".range", 0); + this->declare_parameter(servo_name + ".direction", 0); + this->declare_parameter(servo_name + ".center_angle_deg", 0.0); + + std::map temp_map; + temp_map["num"] = static_cast(this->get_parameter(servo_name + ".num").as_int()); + temp_map["center"] = static_cast(this->get_parameter(servo_name + ".center").as_int()); + temp_map["range"] = static_cast(this->get_parameter(servo_name + ".range").as_int()); + temp_map["direction"] = static_cast(this->get_parameter(servo_name + ".direction").as_int()); + temp_map["center_angle_deg"] = static_cast(this->get_parameter(servo_name + ".center_angle_deg").as_double()); + smnc_.servo_config[servo_name] = temp_map; } - } - void SpotMicroMotionCmd::standCommandCallback( - const std_msgs::Bool::ConstPtr& msg) { + const std_msgs::msg::Bool::SharedPtr msg) { if (msg->data == true) {cmd_.stand_cmd_ = true;} } - void SpotMicroMotionCmd::idleCommandCallback( - const std_msgs::Bool::ConstPtr& msg) { + const std_msgs::msg::Bool::SharedPtr msg) { if (msg->data == true) {cmd_.idle_cmd_ = true;} } - void SpotMicroMotionCmd::walkCommandCallback( - const std_msgs::Bool::ConstPtr& msg) { + const std_msgs::msg::Bool::SharedPtr msg) { if (msg->data == true) {cmd_.walk_cmd_ = true;} } - void SpotMicroMotionCmd::angleCommandCallback( - const geometry_msgs::Vector3ConstPtr& msg) { + const geometry_msgs::msg::Vector3::SharedPtr msg) { cmd_.phi_cmd_ = msg->x; cmd_.theta_cmd_ = msg->y; cmd_.psi_cmd_ = msg->z; } - void SpotMicroMotionCmd::velCommandCallback( - const geometry_msgs::TwistConstPtr& msg) { + const geometry_msgs::msg::Twist::SharedPtr msg) { cmd_.x_vel_cmd_mps_ = msg->linear.x; cmd_.y_vel_cmd_mps_ = msg->linear.y; cmd_.yaw_rate_cmd_rps_ = msg->angular.z; } - void SpotMicroMotionCmd::resetEventCommands() { - // Reset all event commands, setting all command states false if they were true - cmd_.resetEventCmds(); + cmd_.resetEventCmds(); } - void SpotMicroMotionCmd::handleInputCommands() { - // Delegate input handling to state state_->handleInputCommands(sm_.getBodyState(), smnc_, cmd_, this, &body_state_cmd_); } - void SpotMicroMotionCmd::changeState(std::unique_ptr sms) { - // Change the active state state_ = std::move(sms); - - // Call init method of new state state_->init(sm_.getBodyState(), smnc_, cmd_, this); - - // Reset all command values cmd_.resetAllCommands(); } - void SpotMicroMotionCmd::publishBodyState() { - // Order of the float array: - // 3 floats xyz for rightback leg foot pos - // 3 floats xyz for rightfront leg foot pos - // 3 floats xyz for leftfront leg foot pos - // 3 floats xyz for leftback leg foot pos - // 3 floats for xyz body position - // 3 floats for phi, theta, psi body angles - body_state_msg_.data[0] = body_state_cmd_.leg_feet_pos.right_back.x; body_state_msg_.data[1] = body_state_cmd_.leg_feet_pos.right_back.y; body_state_msg_.data[2] = body_state_cmd_.leg_feet_pos.right_back.z; @@ -508,237 +453,178 @@ void SpotMicroMotionCmd::publishBodyState() { body_state_msg_.data[16] = body_state_cmd_.euler_angs.theta; body_state_msg_.data[17] = body_state_cmd_.euler_angs.psi; - body_state_pub_.publish(body_state_msg_); + body_state_pub_->publish(body_state_msg_); } - void SpotMicroMotionCmd::publishLcdMonitorData() { lcd_state_string_msg_.data = getCurrentStateName(); lcd_vel_cmd_msg_.linear.x = cmd_.getXSpeedCmd(); lcd_vel_cmd_msg_.linear.y = cmd_.getYSpeedCmd(); lcd_vel_cmd_msg_.angular.z = cmd_.getYawRateCmd(); - + lcd_angle_cmd_msg_.x = cmd_.getPhiCmd(); lcd_angle_cmd_msg_.y = cmd_.getThetaCmd(); - lcd_angle_cmd_msg_.z = cmd_.getPsiCmd(); + lcd_angle_cmd_msg_.z = cmd_.getPsiCmd(); - lcd_state_pub_.publish(lcd_state_string_msg_); - lcd_vel_cmd_pub_.publish(lcd_vel_cmd_msg_); - lcd_angle_cmd_pub_.publish(lcd_angle_cmd_msg_); + lcd_state_pub_->publish(lcd_state_string_msg_); + lcd_vel_cmd_pub_->publish(lcd_vel_cmd_msg_); + lcd_angle_cmd_pub_->publish(lcd_angle_cmd_msg_); } - void SpotMicroMotionCmd::publishStaticTransforms() { TransformStamped tr_stamped; - - // base_link to front_link transform + auto now = this->now(); + tr_stamped = createTransform("base_link", "front_link", 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0); - static_transform_br_.sendTransform(tr_stamped); + 0.0, 0.0, 0.0, now); + static_transform_br_->sendTransform(tr_stamped); - // base_link to rear_link transform tr_stamped = createTransform("base_link", "rear_link", 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0); - static_transform_br_.sendTransform(tr_stamped); + 0.0, 0.0, 0.0, now); + static_transform_br_->sendTransform(tr_stamped); - // base_link to lidar_link transform float x_offset = smnc_.lidar_x_pos; float y_offset = smnc_.lidar_y_pos; float z_offset = smnc_.lidar_z_pos; - float yaw_angle = smnc_.lidar_yaw_angle*M_PI/180.0; // Converted to radians + float yaw_angle = smnc_.lidar_yaw_angle*M_PI/180.0; tr_stamped = createTransform("base_link", "lidar_link", x_offset, y_offset, z_offset, - 0.0, 0.0, yaw_angle); - static_transform_br_.sendTransform(tr_stamped); + 0.0, 0.0, yaw_angle, now); + static_transform_br_->sendTransform(tr_stamped); - // legs to leg cover transforms - const VectorStringPairs leg_cover_pairs { + const VectorStringPairs leg_cover_pairs { { "front_left_leg_link", "front_left_leg_link_cover" }, { "front_right_leg_link", "front_right_leg_link_cover"}, { "rear_right_leg_link", "rear_right_leg_link_cover" }, { "rear_left_leg_link", "rear_left_leg_link_cover" }}; - - // Loop over all leg to leg cover name pairs, publish a 0 dist/rot transform + for (auto it = leg_cover_pairs.begin(); it != leg_cover_pairs.end(); it++) { tr_stamped = createTransform(it->first, it->second, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0); - static_transform_br_.sendTransform(tr_stamped); + 0.0, 0.0, 0.0, now); + static_transform_br_->sendTransform(tr_stamped); } - // foot to toe link transforms - const VectorStringPairs foot_toe_pairs { + const VectorStringPairs foot_toe_pairs { { "front_left_foot_link", "front_left_toe_link" }, { "front_right_foot_link", "front_right_toe_link"}, { "rear_right_foot_link", "rear_right_toe_link" }, { "rear_left_foot_link", "rear_left_toe_link" }}; - - // Loop over all name pairs, publish the same transform + for (auto it = foot_toe_pairs.begin(); it != foot_toe_pairs.end(); it++) { tr_stamped = createTransform(it->first, it->second, - 0.0, 0.0, -0.13, // TODO: Change to a parameter - 0.0, 0.0, 0.0); - static_transform_br_.sendTransform(tr_stamped); + 0.0, 0.0, -0.13, + 0.0, 0.0, 0.0, now); + static_transform_br_->sendTransform(tr_stamped); } } - void SpotMicroMotionCmd::publishDynamicTransforms() { - // Get joint angles LegsJointAngles joint_angs = sm_.getLegsJointAngles(); - // Declare utility variables TransformStamped transform_stamped; Affine3d temp_trans; + auto now = this->now(); - ///////////////// - // ODOMETRY ///// - ///////////////// if (smnc_.publish_odom) { - transform_stamped = eigAndFramesToTrans(getOdometryTransform(), "odom", "base_footprint"); - transform_br_.sendTransform(transform_stamped); + transform_stamped = eigAndFramesToTrans(getOdometryTransform(), "odom", "base_footprint", now); + transform_br_->sendTransform(transform_stamped); } - - ///////////////// - // BODY CENTER // - ///////////////// - + temp_trans = matrix4fToAffine3d(sm_.getBodyHt()); - // Rotate body center transform to desired coordinate system - // Original, kinematics, coordinate frame: x forward, y up, z right - // Desired orientation: x forward, y left, z up - // Rotate the robot frame +90 deg about the global +X axis (pre-multiply), - // then rotate the local coordinate system by -90 (post multiply) - temp_trans = AngleAxisd(M_PI/2.0, Vector3d::UnitX()) * - temp_trans * + temp_trans = AngleAxisd(M_PI/2.0, Vector3d::UnitX()) * + temp_trans * AngleAxisd(-M_PI/2.0, Vector3d::UnitX()); - // Create and broadcast the transform - transform_stamped = eigAndFramesToTrans(temp_trans, "base_footprint", "base_link"); - transform_br_.sendTransform(transform_stamped); + transform_stamped = eigAndFramesToTrans(temp_trans, "base_footprint", "base_link", now); + transform_br_->sendTransform(transform_stamped); - - ///////////////////// - // FRONT RIGHT LEG // - ///////////////////// - // Shoulder transform_stamped = createTransform("base_link", "front_right_shoulder_link", smnc_.smc.body_length/2.0, -smnc_.smc.body_width/2.0, 0.0, - joint_angs.right_front.ang1, 0.0, 0.0); - transform_br_.sendTransform(transform_stamped); + joint_angs.right_front.ang1, 0.0, 0.0, now); + transform_br_->sendTransform(transform_stamped); - // leg transform_stamped = createTransform("front_right_shoulder_link","front_right_leg_link", 0.0, -smnc_.smc.hip_link_length, 0.0, - 0.0, -joint_angs.right_front.ang2, 0.0); - transform_br_.sendTransform(transform_stamped); + 0.0, -joint_angs.right_front.ang2, 0.0, now); + transform_br_->sendTransform(transform_stamped); - // foot transform_stamped = createTransform("front_right_leg_link","front_right_foot_link", 0.0, 0.0, -smnc_.smc.upper_leg_link_length, - 0.0, -joint_angs.right_front.ang3, 0.0); - transform_br_.sendTransform(transform_stamped); - + 0.0, -joint_angs.right_front.ang3, 0.0, now); + transform_br_->sendTransform(transform_stamped); - //////////////////// - // REAR RIGHT LEG // - //////////////////// - // shoulder transform_stamped = createTransform("base_link", "rear_right_shoulder_link", -smnc_.smc.body_length/2.0, -smnc_.smc.body_width/2.0, 0.0, - joint_angs.right_back.ang1, 0.0, 0.0); - transform_br_.sendTransform(transform_stamped); - - // leg + joint_angs.right_back.ang1, 0.0, 0.0, now); + transform_br_->sendTransform(transform_stamped); + transform_stamped = createTransform("rear_right_shoulder_link","rear_right_leg_link", 0.0, -smnc_.smc.hip_link_length, 0.0, - 0.0, -joint_angs.right_back.ang2, 0.0); - transform_br_.sendTransform(transform_stamped); + 0.0, -joint_angs.right_back.ang2, 0.0, now); + transform_br_->sendTransform(transform_stamped); - // foot transform_stamped = createTransform("rear_right_leg_link","rear_right_foot_link", 0.0, 0.0, -smnc_.smc.upper_leg_link_length, - 0.0, -joint_angs.right_back.ang3, 0.0); - transform_br_.sendTransform(transform_stamped); + 0.0, -joint_angs.right_back.ang3, 0.0, now); + transform_br_->sendTransform(transform_stamped); - - //////////////////// - // FRONT LEFT LEG // - //////////////////// - // Shoulder transform_stamped = createTransform("base_link", "front_left_shoulder_link", smnc_.smc.body_length/2.0, smnc_.smc.body_width/2.0, 0.0, - -joint_angs.left_front.ang1, 0.0, 0.0); - transform_br_.sendTransform(transform_stamped); + -joint_angs.left_front.ang1, 0.0, 0.0, now); + transform_br_->sendTransform(transform_stamped); - // leg transform_stamped = createTransform("front_left_shoulder_link","front_left_leg_link", 0.0, smnc_.smc.hip_link_length, 0.0, - 0.0, joint_angs.left_front.ang2, 0.0); - transform_br_.sendTransform(transform_stamped); + 0.0, joint_angs.left_front.ang2, 0.0, now); + transform_br_->sendTransform(transform_stamped); - // foot transform_stamped = createTransform("front_left_leg_link","front_left_foot_link", 0.0, 0.0, -smnc_.smc.upper_leg_link_length, - 0.0, joint_angs.left_front.ang3, 0.0); - transform_br_.sendTransform(transform_stamped); - + 0.0, joint_angs.left_front.ang3, 0.0, now); + transform_br_->sendTransform(transform_stamped); - /////////////////// - // REAR LEFT LEG // - /////////////////// - // shoulder transform_stamped = createTransform("base_link", "rear_left_shoulder_link", -smnc_.smc.body_length/2.0, smnc_.smc.body_width/2.0, 0.0, - -joint_angs.left_back.ang1, 0.0, 0.0); - transform_br_.sendTransform(transform_stamped); + -joint_angs.left_back.ang1, 0.0, 0.0, now); + transform_br_->sendTransform(transform_stamped); - // leg transform_stamped = createTransform("rear_left_shoulder_link","rear_left_leg_link", 0.0, smnc_.smc.hip_link_length, 0.0, - 0.0, joint_angs.left_back.ang2, 0.0); - transform_br_.sendTransform(transform_stamped); + 0.0, joint_angs.left_back.ang2, 0.0, now); + transform_br_->sendTransform(transform_stamped); - // foot transform_stamped = createTransform("rear_left_leg_link","rear_left_foot_link", 0.0, 0.0, -smnc_.smc.upper_leg_link_length, - 0.0, joint_angs.left_back.ang3, 0.0); - transform_br_.sendTransform(transform_stamped); + 0.0, joint_angs.left_back.ang3, 0.0, now); + transform_br_->sendTransform(transform_stamped); } - void SpotMicroMotionCmd::integrateOdometry() { - // Get loop time, heading, and rate commands float dt = smnc_.dt; float psi = robot_odometry_.euler_angs.psi; float x_spd = cmd_.getXSpeedCmd(); float y_spd = -cmd_.getYSpeedCmd(); float yaw_rate = -cmd_.getYawRateCmd(); - // This is the odometry coordinate frame (not the robot kinematic frame) float x_dot = x_spd*cos(psi) - y_spd*sin(psi); float y_dot = x_spd*sin(psi) + y_spd*cos(psi); float yaw_dot = yaw_rate; - // Integrate x and y position, and yaw angle, from commanded values - // y speed and yaw rate are reversed due to mismatch between command - // coordinate frame and world coordinate frame robot_odometry_.xyz_pos.x += x_dot*dt; robot_odometry_.xyz_pos.y += y_dot*dt; robot_odometry_.euler_angs.psi += yaw_dot*dt; -} - +} Affine3d SpotMicroMotionCmd::getOdometryTransform() { - // Create odemtry translation and rotation, and combine together Translation3d translation(robot_odometry_.xyz_pos.x, robot_odometry_.xyz_pos.y, 0.0); AngleAxisd rotation(robot_odometry_.euler_angs.psi, Vector3d::UnitZ()); return (translation * rotation); -} \ No newline at end of file +} diff --git a/spot_micro_motion_cmd/src/spot_micro_motion_cmd_node.cpp b/spot_micro_motion_cmd/src/spot_micro_motion_cmd_node.cpp index 2c96022..19261de 100644 --- a/spot_micro_motion_cmd/src/spot_micro_motion_cmd_node.cpp +++ b/spot_micro_motion_cmd/src/spot_micro_motion_cmd_node.cpp @@ -1,43 +1,31 @@ -// Node file to create object and initialising the ROS node -#include "spot_micro_motion_cmd.h" +#include "spot_micro_motion_cmd.h" #include - int main(int argc, char** argv) { - /* initialising the ROS node creating node handle - for regestring it to the master and then private node handle to - handle the parameters */ - ros::init(argc, argv, "spot_micro_motion_cmd"); - ros::NodeHandle nh; - ros::NodeHandle pnh("~"); - - SpotMicroMotionCmd node(nh,pnh); // Creating the object + rclcpp::init(argc, argv); + auto node = std::make_shared(); - ros::Rate rate(1.0/node.getNodeConfig().dt); // Defing the looping rate + rclcpp::Rate rate(1.0 / node->getNodeConfig().dt); + if (node->publishServoConfiguration()) { + bool debug_mode = node->getNodeConfig().debug_mode; + rclcpp::Time begin; - // Only proceed if servo configuration publishing succeeds - if (node.publishServoConfiguration()) { - - bool debug_mode = node.getNodeConfig().debug_mode; - ros::Time begin; - /* Looking for any interupt else it will continue looping */ - // Main loop runs indefinitely unless there is an interupt call - while (ros::ok()) - { - if (debug_mode) { - begin = ros::Time::now(); - } + while (rclcpp::ok()) { + if (debug_mode) { + begin = node->now(); + } - node.runOnce(); - ros::spinOnce(); - rate.sleep(); + node->runOnce(); + rclcpp::spin_some(node); + rate.sleep(); - if (debug_mode) { - std::cout << (ros::Time::now() - begin) << std::endl; - } + if (debug_mode) { + std::cout << (node->now() - begin).seconds() << std::endl; + } } - } + + rclcpp::shutdown(); return 0; } diff --git a/spot_micro_motion_cmd/src/utils.cpp b/spot_micro_motion_cmd/src/utils.cpp index da129ff..6bb900d 100644 --- a/spot_micro_motion_cmd/src/utils.cpp +++ b/spot_micro_motion_cmd/src/utils.cpp @@ -1,51 +1,40 @@ #include "utils.h" -#include #include #include "tf2/LinearMath/Quaternion.h" -#include "tf2_eigen/tf2_eigen.h" -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "tf2_eigen/tf2_eigen.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" using namespace Eigen; -using namespace geometry_msgs; - +using namespace geometry_msgs::msg; Affine3d matrix4fToAffine3d(const Matrix4f& in) { - // Convert a Eigen Matrix4F to an Affine3d by first casting - // float to double (to a Matrix4d), then calling the constructor for Affine3d - // with the Matrix4d return Affine3d(in.cast()); } - -// Create a ROS tf2 TransformStamped from a Eigen Affine3d, -// parent frame id and child frame id. Stamped with current time, -// so should be broadcast ASAP TransformStamped eigAndFramesToTrans( - const Affine3d& transform, - std::string parent_frame_id, std::string child_frame_id) { + const Affine3d& transform, + std::string parent_frame_id, std::string child_frame_id, + const rclcpp::Time& stamp) { TransformStamped transform_stamped = tf2::eigenToTransform(transform); - transform_stamped.header.stamp = ros::Time::now(); + transform_stamped.header.stamp = stamp; transform_stamped.header.frame_id = parent_frame_id; transform_stamped.child_frame_id = child_frame_id; return transform_stamped; } - -// Create a transform from a translation, rotation, and parent and -// child frame IDs. Will stamp the transform with ros::Time::now(), -// so the returned transform should be broadcast asap - TransformStamped createTransform( - std::string parent_frame_id, std::string child_frame_id, - double x, double y, double z, - double roll, double pitch, double yaw) { +TransformStamped createTransform( + std::string parent_frame_id, std::string child_frame_id, + double x, double y, double z, + double roll, double pitch, double yaw, + const rclcpp::Time& stamp) { TransformStamped tr_stamped; - tr_stamped.header.stamp = ros::Time::now(); + tr_stamped.header.stamp = stamp; tr_stamped.header.frame_id = parent_frame_id; tr_stamped.child_frame_id = child_frame_id; @@ -61,4 +50,3 @@ TransformStamped eigAndFramesToTrans( return tr_stamped; } - diff --git a/spot_micro_mujoco_sim/launch/mujoco_sim_launch.py b/spot_micro_mujoco_sim/launch/mujoco_sim_launch.py new file mode 100644 index 0000000..ce234b8 --- /dev/null +++ b/spot_micro_mujoco_sim/launch/mujoco_sim_launch.py @@ -0,0 +1,73 @@ +import os +import subprocess + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + pkg_share = get_package_share_directory("spot_micro_mujoco_sim") + default_model = os.path.join(pkg_share, "models", "spot_micro_sim.xml") + + # Process xacro to URDF for robot_state_publisher + xacro_path = os.path.join( + get_package_share_directory("spot_micro_rviz"), + "urdf", "spot_micro.urdf.xacro" + ) + try: + urdf_xml = subprocess.check_output(["xacro", xacro_path], text=True) + except Exception: + urdf_xml = "" + + rviz_config = os.path.join(pkg_share, "config", "spot_micro.rviz") + has_rviz_config = os.path.exists(rviz_config) + + return LaunchDescription([ + DeclareLaunchArgument( + "model_path", + default_value=default_model, + description="Path to MuJoCo MJCF model file", + ), + DeclareLaunchArgument( + "use_rviz", + default_value="true", + description="Launch RViz2 for visualization", + ), + + # MuJoCo simulation bridge + Node( + package="spot_micro_mujoco_sim", + executable="mujoco_sim_node", + name="mujoco_sim", + output="screen", + parameters=[{ + "model_path": LaunchConfiguration("model_path"), + "sim_rate_hz": 500.0, + "publish_rate_hz": 50.0, + "initial_body_z": 0.25, + }], + ), + + # Robot state publisher (publishes TF from joint_states + URDF) + Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="screen", + parameters=[{"robot_description": urdf_xml}], + ), + + # RViz2 + Node( + condition=IfCondition(LaunchConfiguration("use_rviz")), + package="rviz2", + executable="rviz2", + name="rviz2", + output="screen", + arguments=["-d", rviz_config] if has_rviz_config else [], + ), + ]) diff --git a/spot_micro_mujoco_sim/models/spot_micro_sim.xml b/spot_micro_mujoco_sim/models/spot_micro_sim.xml new file mode 100644 index 0000000..1847622 --- /dev/null +++ b/spot_micro_mujoco_sim/models/spot_micro_sim.xml @@ -0,0 +1,121 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/spot_micro_mujoco_sim/package.xml b/spot_micro_mujoco_sim/package.xml new file mode 100644 index 0000000..58c12d9 --- /dev/null +++ b/spot_micro_mujoco_sim/package.xml @@ -0,0 +1,29 @@ + + + + spot_micro_mujoco_sim + 0.1.0 + MuJoCo simulation bridge for SpotMicro robot + user + MIT + + ament_python + + rclpy + std_msgs + sensor_msgs + geometry_msgs + tf2_ros + i2cpwm_board + robot_state_publisher + joint_state_publisher + rviz2 + + ament_copyright + ament_flake8 + ament_pep257 + + + ament_python + + diff --git a/spot_micro_mujoco_sim/resource/spot_micro_mujoco_sim b/spot_micro_mujoco_sim/resource/spot_micro_mujoco_sim new file mode 100644 index 0000000..e69de29 diff --git a/spot_micro_mujoco_sim/setup.py b/spot_micro_mujoco_sim/setup.py new file mode 100644 index 0000000..e729e4e --- /dev/null +++ b/spot_micro_mujoco_sim/setup.py @@ -0,0 +1,28 @@ +from setuptools import find_packages, setup + +package_name = 'spot_micro_mujoco_sim' + +setup( + name=package_name, + version='0.1.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ('share/' + package_name + '/launch', ['launch/mujoco_sim_launch.py']), + ('share/' + package_name + '/models', ['models/spot_micro_sim.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='user', + maintainer_email='user@example.com', + description='MuJoCo simulation bridge for SpotMicro robot', + license='MIT', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'mujoco_sim_node = spot_micro_mujoco_sim.mujoco_sim_node:main', + ], + }, +) diff --git a/spot_micro_mujoco_sim/spot_micro_mujoco_sim/__init__.py b/spot_micro_mujoco_sim/spot_micro_mujoco_sim/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/spot_micro_mujoco_sim/spot_micro_mujoco_sim/mujoco_sim_node.py b/spot_micro_mujoco_sim/spot_micro_mujoco_sim/mujoco_sim_node.py new file mode 100644 index 0000000..f339fd7 --- /dev/null +++ b/spot_micro_mujoco_sim/spot_micro_mujoco_sim/mujoco_sim_node.py @@ -0,0 +1,456 @@ +#!/usr/bin/env python3 +"""MuJoCo simulation bridge for SpotMicro robot.""" + +import math +import os +import shutil +import subprocess +from typing import Dict, Optional + +import mujoco +import numpy as np +import rclpy +from geometry_msgs.msg import TransformStamped +from i2cpwm_board.msg import ServoArray +from i2cpwm_board.srv import ServosConfig +from PIL import Image +from rclpy.node import Node +from sensor_msgs.msg import JointState +from tf2_ros import TransformBroadcaster + + +class SpotMicroMujocoSim(Node): + """ROS2 node that bridges SpotMicro servo commands to MuJoCo simulation.""" + + # Default servo configuration extracted from spot_micro_motion_cmd.yaml. + # center_angle_deg: kinematic angle (rad) that corresponds to proportional=0. + # direction: hardware direction multiplier (-1 or 1). + DEFAULT_SERVO_CONFIG: Dict[str, Dict] = { + "RF_1": {"num": 3, "center_angle_deg": -5.4, "direction": -1}, + "RF_2": {"num": 2, "center_angle_deg": -27.6, "direction": 1}, + "RF_3": {"num": 1, "center_angle_deg": 88.2, "direction": 1}, + "RB_1": {"num": 6, "center_angle_deg": -4.4, "direction": 1}, + "RB_2": {"num": 5, "center_angle_deg": -35.4, "direction": 1}, + "RB_3": {"num": 4, "center_angle_deg": 85.8, "direction": 1}, + "LB_1": {"num": 9, "center_angle_deg": -0.4, "direction": -1}, + "LB_2": {"num": 8, "center_angle_deg": 38.7, "direction": 1}, + "LB_3": {"num": 7, "center_angle_deg": -73.9, "direction": 1}, + "LF_1": {"num": 10, "center_angle_deg": -7.6, "direction": 1}, + "LF_2": {"num": 11, "center_angle_deg": 38.6, "direction": 1}, + "LF_3": {"num": 12, "center_angle_deg": -82.8, "direction": 1}, + } + + # Kinematic -> URDF joint angle signs. These must match the TF convention in + # spot_micro_motion_cmd.cpp: + # Right side: shoulder=+ang1, leg=-ang2, foot=-ang3 + # Left side: shoulder=-ang1, leg=+ang2, foot=+ang3 + SERVO_TO_JOINT: Dict[str, str] = { + "RF_1": "front_right_shoulder", + "RF_2": "front_right_leg", + "RF_3": "front_right_foot", + "RB_1": "rear_right_shoulder", + "RB_2": "rear_right_leg", + "RB_3": "rear_right_foot", + "LF_1": "front_left_shoulder", + "LF_2": "front_left_leg", + "LF_3": "front_left_foot", + "LB_1": "rear_left_shoulder", + "LB_2": "rear_left_leg", + "LB_3": "rear_left_foot", + } + + # Default pose angles (all zero) — matches the MuJoCo model's default + # configuration where legs hang straight down. + DEFAULT_ANGLES_DEG: Dict[str, float] = { + "RF_1": 0.0, "RF_2": 0.0, "RF_3": 0.0, + "RB_1": 0.0, "RB_2": 0.0, "RB_3": 0.0, + "LF_1": 0.0, "LF_2": 0.0, "LF_3": 0.0, + "LB_1": 0.0, "LB_2": 0.0, "LB_3": 0.0, + } + + def __init__(self): + super().__init__("spot_micro_mujoco_sim") + + # Parameters + self.declare_parameter("model_path", "") + self.declare_parameter("servo_max_angle_deg", 82.5) + self.declare_parameter("sim_rate_hz", 500.0) + self.declare_parameter("publish_rate_hz", 50.0) + self.declare_parameter("initial_body_z", 0.25) + self.declare_parameter("initial_body_quat", [1.0, 0.0, 0.0, 0.0]) + + # Video recording parameters + self.declare_parameter("record_video", False) + self.declare_parameter("video_fps", 30.0) + self.declare_parameter("video_width", 1280) + self.declare_parameter("video_height", 720) + self.declare_parameter("video_output_path", "/tmp/spotmicro_sim_video.mp4") + + default_signs = { + "RF_1": 1.0, "RF_2": -1.0, "RF_3": -1.0, + "RB_1": 1.0, "RB_2": -1.0, "RB_3": -1.0, + "LF_1": -1.0, "LF_2": 1.0, "LF_3": 1.0, + "LB_1": -1.0, "LB_2": 1.0, "LB_3": 1.0, + } + for servo_name in self.DEFAULT_SERVO_CONFIG: + self.declare_parameter(f"joint_signs.{servo_name}", default_signs.get(servo_name, 1.0)) + + model_path = self.get_parameter("model_path").value + if not model_path or not os.path.exists(model_path): + pkg_share = os.path.join( + os.path.dirname(__file__), "..", "..", "share", "spot_micro_mujoco_sim" + ) + model_path = os.path.join(pkg_share, "models", "spot_micro_sim.xml") + if not os.path.exists(model_path): + model_path = "/tmp/spotmicro_mujoco/spot_micro_sim.xml" + self.get_logger().info(f"Using default model path: {model_path}") + + if not os.path.exists(model_path): + self.get_logger().error(f"MuJoCo model not found: {model_path}") + raise FileNotFoundError(f"Model not found: {model_path}") + + # Video recording setup + self.record_video = self.get_parameter("record_video").value + self.video_fps = self.get_parameter("video_fps").value + self.video_width = self.get_parameter("video_width").value + self.video_height = self.get_parameter("video_height").value + self.video_output_path = self.get_parameter("video_output_path").value + self._frame_dir = "/tmp/spotmicro_video_frames" + self._frame_count = 0 + self._last_frame_time = 0.0 + self._renderer = None + self._camera = None + + if self.record_video: + # Inject visual/global settings into MJCF so offscreen framebuffer is large enough + with open(model_path, "r") as f: + xml_src = f.read() + # Add if not present + if "" not in xml_src: + # Find the opening tag and insert visual block after it + import re + xml_src = re.sub( + r'(]*>)', + rf'\1\n \n \n ', + xml_src, + count=1, + ) + model_path_mod = "/tmp/spotmicro_mujoco/spot_micro_sim_with_visual.xml" + with open(model_path_mod, "w") as f: + f.write(xml_src) + model_path = model_path_mod + + os.makedirs(self._frame_dir, exist_ok=True) + # Clean old frames + for f in os.listdir(self._frame_dir): + os.remove(os.path.join(self._frame_dir, f)) + self.get_logger().info( + f"Video recording enabled: {self.video_width}x{self.video_height} @ {self.video_fps} FPS -> {self.video_output_path}" + ) + + # Load MuJoCo model + self.model = mujoco.MjModel.from_xml_path(model_path) + self.data = mujoco.MjData(self.model) + + self.get_logger().info( + f"MuJoCo model loaded: {self.model.njnt} joints, {self.model.nu} actuators, " + f"{self.model.nq} qpos, {self.model.nv} qvel" + ) + + # Build name -> id lookups + self.joint_ids: Dict[str, int] = {} + self.joint_qposadr: Dict[str, int] = {} + self.actuator_ids: Dict[str, int] = {} + + for i in range(self.model.njnt): + name = mujoco.mj_id2name(self.model, mujoco.mjtObj.mjOBJ_JOINT, i) + self.joint_ids[name] = i + self.joint_qposadr[name] = self.model.jnt_qposadr[i] + + for i in range(self.model.nu): + name = mujoco.mj_id2name(self.model, mujoco.mjtObj.mjOBJ_ACTUATOR, i) + self.actuator_ids[name] = i + + # Verify all required joints exist + for servo_name, joint_name in self.SERVO_TO_JOINT.items(): + if joint_name not in self.joint_ids: + self.get_logger().warn(f"Joint {joint_name} not found in MuJoCo model") + + # Servo state + self.servo_commands: Dict[int, float] = {} # servo_num -> proportional value + self.servo_config: Dict[str, Dict] = {} + self.servo_config_received = False + + # Simulation parameters + self.servo_max_angle_rad = ( + self.get_parameter("servo_max_angle_deg").value * math.pi / 180.0 + ) + self.sim_dt = 1.0 / self.get_parameter("sim_rate_hz").value + self.publish_dt = 1.0 / self.get_parameter("publish_rate_hz").value + self.model.opt.timestep = self.sim_dt + + # Initialize simulation pose + self._reset_simulation() + + # Setup video renderer after reset so data is valid + if self.record_video: + try: + self._renderer = mujoco.Renderer(self.model, self.video_height, self.video_width) + self._camera = mujoco.MjvCamera() + self._camera.type = mujoco.mjtCamera.mjCAMERA_TRACKING + self._camera.trackbodyid = self.model.body("base_link").id + self._camera.distance = 0.8 + self._camera.azimuth = 135 + self._camera.elevation = -20 + self.get_logger().info("Offscreen renderer initialized") + except Exception as e: + self.get_logger().error(f"Failed to initialize renderer: {e}") + self.record_video = False + self._renderer = None + + # ROS interfaces + self.servos_config_srv = self.create_service( + ServosConfig, "config_servos", self._config_servos_callback + ) + self.servos_proportional_sub = self.create_subscription( + ServoArray, "servos_proportional", self._servos_proportional_callback, 10 + ) + self.servos_absolute_sub = self.create_subscription( + ServoArray, "servos_absolute", self._servos_absolute_callback, 10 + ) + self.joint_state_pub = self.create_publisher(JointState, "joint_states", 10) + self.tf_broadcaster = TransformBroadcaster(self) + + # Timers + self.sim_timer = self.create_timer(self.sim_dt, self._sim_step) + self.pub_timer = self.create_timer(self.publish_dt, self._publish_state) + + self.get_logger().info("SpotMicro MuJoCo simulation node started") + + def _reset_simulation(self): + """Reset simulation to default pose.""" + mujoco.mj_resetData(self.model, self.data) + + initial_z = self.get_parameter("initial_body_z").value + initial_quat = self.get_parameter("initial_body_quat").value + + # Free joint qpos is first 7 elements: [x, y, z, qw, qx, qy, qz] + self.data.qpos[0:3] = [0.0, 0.0, initial_z] + self.data.qpos[3:7] = initial_quat + + # Set initial joint angles to default (all zero) + for servo_name, joint_name in self.SERVO_TO_JOINT.items(): + if joint_name not in self.joint_ids: + continue + qpos_adr = self.joint_qposadr[joint_name] + self.data.qpos[qpos_adr] = 0.0 + + # Set actuators to hold initial pose (all zero) + for servo_name, joint_name in self.SERVO_TO_JOINT.items(): + if joint_name not in self.joint_ids: + continue + actuator_name = f"{joint_name}_actuator" + if actuator_name in self.actuator_ids: + act_id = self.actuator_ids[actuator_name] + self.data.ctrl[act_id] = 0.0 + + # Forward kinematics to initialize all derived quantities + mujoco.mj_forward(self.model, self.data) + toe_z = [ + float(self.data.geom_xpos[i][2]) + for i in range(self.model.ngeom) + if mujoco.mj_id2name(self.model, mujoco.mjtObj.mjOBJ_GEOM, i) + and "toe" in (mujoco.mj_id2name(self.model, mujoco.mjtObj.mjOBJ_GEOM, i) or "") + ] + self.get_logger().info( + f"Simulation reset: body_z={self.data.qpos[2]:.3f}, toes z={toe_z}" + ) + + def _config_servos_callback(self, request, response): + """Handle servo config service (called by spot_micro_motion_cmd at startup).""" + self.get_logger().info( + f"Received servo config for {len(request.servos)} servos" + ) + for servo in request.servos: + # Find servo name by num + for name, cfg in self.DEFAULT_SERVO_CONFIG.items(): + if cfg["num"] == servo.servo: + self.servo_config[name] = { + "num": servo.servo, + "center": servo.center, + "range": servo.range, + "direction": servo.direction, + "center_angle_deg": cfg["center_angle_deg"], + } + break + self.servo_config_received = True + response.success = True + return response + + def _get_servo_cfg(self, servo_name: str) -> Dict: + """Return active config for a servo (service override or default).""" + if servo_name in self.servo_config: + return self.servo_config[servo_name] + return self.DEFAULT_SERVO_CONFIG.get(servo_name, {}) + + def _servos_proportional_callback(self, msg: ServoArray): + """Handle incoming servo proportional commands.""" + for servo in msg.servos: + self.servo_commands[servo.servo] = servo.value + self.get_logger().debug( + f"Received {len(msg.servos)} servo proportional commands" + ) + + def _servos_absolute_callback(self, msg: ServoArray): + """Handle incoming servo absolute commands (idle state sends all zeros = off).""" + pass + + def _proportional_to_joint_angle(self, servo_name: str, proportional: float) -> float: + """Convert proportional servo command to URDF joint angle in radians.""" + cfg = self._get_servo_cfg(servo_name) + if not cfg: + return 0.0 + sign = self.get_parameter(f"joint_signs.{servo_name}").value + direction = float(cfg.get("direction", 1)) + center_rad = cfg["center_angle_deg"] * math.pi / 180.0 + # Motion cmd computes: proportional = (cmd_kin - center) / max * direction + # Recover kinematic angle: cmd_kin = proportional * max * direction + center + cmd_kin_rad = proportional * self.servo_max_angle_rad * direction + center_rad + return sign * cmd_kin_rad + + def _capture_frame(self): + """Render current simulation state to a PNG frame.""" + if not self.record_video or self._renderer is None: + return + + now = self.get_clock().now().nanoseconds / 1e9 + if now - self._last_frame_time < (1.0 / self.video_fps): + return + self._last_frame_time = now + + try: + self._renderer.update_scene(self.data, camera=self._camera) + frame = self._renderer.render() + self._frame_count += 1 + path = os.path.join(self._frame_dir, f"frame_{self._frame_count:06d}.png") + Image.fromarray(frame).save(path) + except Exception as e: + self.get_logger().warning(f"Frame capture failed: {e}") + + def _save_video(self): + """Encode captured frames to MP4 using ffmpeg.""" + if not self.record_video or self._frame_count == 0: + return + + self.get_logger().info( + f"Encoding {self._frame_count} frames to {self.video_output_path} ..." + ) + cmd = [ + "ffmpeg", + "-y", + "-framerate", str(int(self.video_fps)), + "-i", os.path.join(self._frame_dir, "frame_%06d.png"), + "-c:v", "libx264", + "-pix_fmt", "yuv420p", + "-crf", "23", + self.video_output_path, + ] + try: + result = subprocess.run(cmd, capture_output=True, text=True) + if result.returncode == 0: + self.get_logger().info(f"Video saved: {self.video_output_path}") + else: + self.get_logger().error(f"ffmpeg failed: {result.stderr}") + except Exception as e: + self.get_logger().error(f"ffmpeg error: {e}") + finally: + # Optional: clean up frames + shutil.rmtree(self._frame_dir, ignore_errors=True) + + def _sim_step(self): + """Step the MuJoCo simulation one timestep.""" + if hasattr(self, "_step_count"): + self._step_count += 1 + else: + self._step_count = 1 + + if self._step_count % 500 == 0: + self.get_logger().debug(f"Step {self._step_count}: body_z={self.data.qpos[2]:.3f}") + + # Apply servo commands to actuators + for servo_name, joint_name in self.SERVO_TO_JOINT.items(): + cfg = self._get_servo_cfg(servo_name) + if not cfg: + continue + servo_num = cfg["num"] + proportional = self.servo_commands.get(servo_num, 0.0) + target_angle = self._proportional_to_joint_angle(servo_name, proportional) + + actuator_name = f"{joint_name}_actuator" + if actuator_name in self.actuator_ids: + act_id = self.actuator_ids[actuator_name] + self.data.ctrl[act_id] = target_angle + + # Step physics + mujoco.mj_step(self.model, self.data) + + def _publish_state(self): + """Publish joint states and TF transforms.""" + now = self.get_clock().now() + stamp = now.to_msg() + + # Joint state message + joint_state = JointState() + joint_state.header.stamp = stamp + joint_state.header.frame_id = "base_link" + + # Publish all actuated joints + for servo_name, joint_name in self.SERVO_TO_JOINT.items(): + if joint_name not in self.joint_ids: + continue + qpos_adr = self.joint_qposadr[joint_name] + joint_state.name.append(joint_name) + joint_state.position.append(float(self.data.qpos[qpos_adr])) + qvel_adr = self.model.jnt_dofadr[self.joint_ids[joint_name]] + joint_state.velocity.append(float(self.data.qvel[qvel_adr])) + joint_state.effort.append(0.0) + + self.joint_state_pub.publish(joint_state) + + # Publish TF for base link + base_pos = self.data.qpos[0:3].copy() + base_quat = self.data.qpos[3:7].copy() # [w, x, y, z] + + t = TransformStamped() + t.header.stamp = stamp + t.header.frame_id = "world" + t.child_frame_id = "base_link" + t.transform.translation.x = float(base_pos[0]) + t.transform.translation.y = float(base_pos[1]) + t.transform.translation.z = float(base_pos[2]) + t.transform.rotation.w = float(base_quat[0]) + t.transform.rotation.x = float(base_quat[1]) + t.transform.rotation.y = float(base_quat[2]) + t.transform.rotation.z = float(base_quat[3]) + self.tf_broadcaster.sendTransform(t) + + # Capture video frame if recording + self._capture_frame() + + +def main(args=None): + rclpy.init(args=args) + node = SpotMicroMujocoSim() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node._save_video() + if rclpy.ok(): + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/spot_micro_plot/CMakeLists.txt b/spot_micro_plot/CMakeLists.txt index 984656f..35c85ee 100644 --- a/spot_micro_plot/CMakeLists.txt +++ b/spot_micro_plot/CMakeLists.txt @@ -1,205 +1,23 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.8) project(spot_micro_plot) -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - rospy - std_msgs -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) +find_package(ament_cmake REQUIRED) -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES spot_micro_keyboard_command -# CATKIN_DEPENDS rospy std_msgs -# DEPENDS system_lib +install(PROGRAMS + scripts/spotMicroPlot.py + DESTINATION lib/${PROJECT_NAME} ) -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( -# include - ${catkin_INCLUDE_DIRS} +install(DIRECTORY scripts/spot_micro_kinematics_python + DESTINATION lib/${PROJECT_NAME} ) -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/spot_micro_keyboard_command.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/spot_micro_keyboard_command_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 -# catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -# install(TARGETS ${PROJECT_NAME}_node -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -# install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_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_spot_micro_keyboard_command.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) +ament_package() diff --git a/spot_micro_plot/launch/start_plotting.launch b/spot_micro_plot/launch/start_plotting.launch deleted file mode 100644 index b3085ab..0000000 --- a/spot_micro_plot/launch/start_plotting.launch +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - diff --git a/spot_micro_plot/launch/start_plotting_launch.py b/spot_micro_plot/launch/start_plotting_launch.py new file mode 100644 index 0000000..7cdb692 --- /dev/null +++ b/spot_micro_plot/launch/start_plotting_launch.py @@ -0,0 +1,11 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='spot_micro_plot', + executable='spotMicroPlot.py', + name='spot_micro_plot_node', + output='screen'), + ]) diff --git a/spot_micro_plot/package.xml b/spot_micro_plot/package.xml index 6670863..4a28234 100644 --- a/spot_micro_plot/package.xml +++ b/spot_micro_plot/package.xml @@ -1,65 +1,21 @@ - + + spot_micro_plot 0.0.0 The spot_micro_plot package - - - - mike - - - - - TODO + ament_cmake - - - - - + rclpy + std_msgs - - - - + ament_lint_auto + ament_lint_common - - - - - - - - - - - - - - - - - - - - - - catkin - rospy - std_msgs - rospy - std_msgs - rospy - std_msgs - - - - - + ament_cmake diff --git a/spot_micro_plot/scripts/spotMicroPlot.py b/spot_micro_plot/scripts/spotMicroPlot.py index c438cc0..720c38b 100755 --- a/spot_micro_plot/scripts/spotMicroPlot.py +++ b/spot_micro_plot/scripts/spotMicroPlot.py @@ -1,29 +1,28 @@ -#!/usr/bin/python +#!/usr/bin/env python3 import numpy as np import time +import threading -import rospy -from spot_micro_kinematics_python.spot_micro_stick_figure import SpotMicroStickFigure -from spot_micro_kinematics_python.utilities import transformations -from math import pi -from std_msgs.msg import Float32, Bool +import rclpy +from rclpy.node import Node from std_msgs.msg import Float32MultiArray + +import matplotlib +matplotlib.use('TkAgg') import matplotlib.pyplot as plt import mpl_toolkits.mplot3d.axes3d as p3 import matplotlib.animation as animation +# Local kinematics module (pure Python, no ROS deps) +from spot_micro_kinematics_python.spot_micro_stick_figure import SpotMicroStickFigure +from spot_micro_kinematics_python.utilities import transformations +from math import pi -######################################################################### -########################## Global Variables ############################# -######################################################################### num_servos = 12 - r2d = 180/pi d2r = pi/180 - -# Attaching 3D axis to the figure fig = plt.figure() ax = p3.Axes3D(fig) ax.set_proj_type('ortho') @@ -39,19 +38,37 @@ x = 0 -# def update_x_speed_cmd(msg): -# '''Updates x speed command from received message''' -# x = msg.data -# print('here') +latest_msg = None +msg_lock = threading.Lock() + +class BodyStateSubscriber(Node): + def __init__(self): + super().__init__('spot_micro_plot') + self.subscription = self.create_subscription( + Float32MultiArray, + '/body_state', + self.listener_callback, + 1) + + def listener_callback(self, msg): + global latest_msg + with msg_lock: + latest_msg = msg + def update_lines(num, x, lines): - msg = rospy.wait_for_message("/body_state", Float32MultiArray, timeout=None) + global latest_msg + with msg_lock: + msg = latest_msg + + if msg is None: + return lines foot_data = np.array([ [msg.data[0], msg.data[1], msg.data[2]], [msg.data[3], msg.data[4], msg.data[5]], [msg.data[6], msg.data[7], msg.data[8]], [msg.data[9], msg.data[10], msg.data[11]] ]) - + xpos = msg.data[12] ypos = msg.data[13] zpos = msg.data[14] @@ -60,7 +77,6 @@ def update_lines(num, x, lines): theta = msg.data[16] psi = msg.data[17] - # print(foot_data) sm.set_absolute_foot_coordinates(foot_data) temp_rot = transformations.rotxyz(phi, psi, theta) temp_pose = np.identity(4) @@ -71,13 +87,6 @@ def update_lines(num, x, lines): sm.set_absolute_body_pose(temp_pose) - # print(len(msg.data)) - # print(msg.data[0]) - # l.set_data([0, num/100.0], [0, num/100.0]) - # print(num/100.0) - # l.set_3d_properties([0, num/100.0]) - - # Get leg coordinates and append to data list coord_data = sm.get_leg_coordinates() line_to_leg__and_link_dict = {4:(0,0), @@ -97,7 +106,6 @@ def update_lines(num, x, lines): line.set_linewidth(4) if i < 4: - # First four lines are the square body if i == 3: ind = -1 else: @@ -105,71 +113,56 @@ def update_lines(num, x, lines): x_vals = [coord_data[ind][0][0], coord_data[ind+1][0][0]] y_vals = [coord_data[ind][0][1], coord_data[ind+1][0][1]] z_vals = [coord_data[ind][0][2], coord_data[ind+1][0][2]] - # NOTE: there is no .set_data() for 3 dim data... line.set_data(x_vals,z_vals) line.set_3d_properties(y_vals) - # Next 12 lines are legs - # Leg 1, link 1, link 2, link 3 - # Leg 2, link 1, link 2, link 3... else: leg_num = line_to_leg__and_link_dict[i][0] link_num = line_to_leg__and_link_dict[i][1] x_vals = [coord_data[leg_num][link_num][0], coord_data[leg_num][link_num+1][0]] y_vals = [coord_data[leg_num][link_num][1], coord_data[leg_num][link_num+1][1]] z_vals = [coord_data[leg_num][link_num][2], coord_data[leg_num][link_num+1][2]] - + line.set_data(x_vals,z_vals) line.set_3d_properties(y_vals) return lines -# Set up and title the ros node for this code -rospy.init_node('spot_micro_plot') -# Instantiate spot micro stick figure obeject +rclpy.init() +node = BodyStateSubscriber() + +# Spin the node in a background thread so matplotlib can run on the main thread +spin_thread = threading.Thread(target=rclpy.spin, args=(node,), daemon=True) +spin_thread.start() + sm = SpotMicroStickFigure(x=0,y=0.093,z=0) coords = sm.get_leg_coordinates() -# Initialize empty list top hold line objects lines = [] -# Construct the body of 4 lines from the first point of each leg (the four corners of the body) for i in range(4): - # For last leg, connect back to first leg point if i == 3: ind = -1 else: ind = i - # Due to mplot3d rotation and view limitations, swap y and z to make the stick figure - # appear oriented better x_vals = [coords[ind][0][0], coords[ind+1][0][0]] y_vals = [coords[ind][0][1], coords[ind+1][0][1]] z_vals = [coords[ind][0][2], coords[ind+1][0][2]] lines.append(ax.plot(x_vals,z_vals,y_vals,color='k')[0]) - -# Plot color order for leg links: (hip, upper leg, lower leg) plt_colors = ['r','c','b'] for leg in coords: for i in range(3): - - # Due to mplot3d rotation and view limitations, swap y and z to make the stick figure - # appear oriented better x_vals = [leg[i][0], leg[i+1][0]] y_vals = [leg[i][1], leg[i+1][1]] z_vals = [leg[i][2], leg[i+1][2]] lines.append(ax.plot(x_vals,z_vals,y_vals,color=plt_colors[i])[0]) - - - lines_ani = animation.FuncAnimation(fig, update_lines, frames=1000, fargs=(x,lines), interval=100) plt.show() - - - - +node.destroy_node() +rclpy.shutdown() diff --git a/spot_micro_plot/scripts/spot_micro_kinematics_python b/spot_micro_plot/scripts/spot_micro_kinematics_python index 29efae7..56d4015 160000 --- a/spot_micro_plot/scripts/spot_micro_kinematics_python +++ b/spot_micro_plot/scripts/spot_micro_kinematics_python @@ -1 +1 @@ -Subproject commit 29efae700b73c3d970d1ada4a81d0f93d53ff680 +Subproject commit 56d4015cbf65ffe6ca90583ad0c3c19994aebd00 diff --git a/spot_micro_rviz/CMakeLists.txt b/spot_micro_rviz/CMakeLists.txt index 9f2b73a..c7f093f 100644 --- a/spot_micro_rviz/CMakeLists.txt +++ b/spot_micro_rviz/CMakeLists.txt @@ -1,198 +1,14 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.8) project(spot_micro_rviz) -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - roscpp - rospy -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES spotmicroai -# CATKIN_DEPENDS roscpp rospy -# DEPENDS system_lib -) +find_package(ament_cmake REQUIRED) -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( -# include - ${catkin_INCLUDE_DIRS} +install(DIRECTORY launch rviz urdf + DESTINATION share/${PROJECT_NAME} ) -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/spotmicroai.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/spotmicroai_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_spotmicroai.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) +ament_package() diff --git a/spot_micro_rviz/launch/show_and_move_model_via_gui.launch b/spot_micro_rviz/launch/show_and_move_model_via_gui.launch deleted file mode 100644 index 395a6cf..0000000 --- a/spot_micro_rviz/launch/show_and_move_model_via_gui.launch +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/spot_micro_rviz/launch/show_and_move_model_via_gui_launch.py b/spot_micro_rviz/launch/show_and_move_model_via_gui_launch.py new file mode 100644 index 0000000..0a74fee --- /dev/null +++ b/spot_micro_rviz/launch/show_and_move_model_via_gui_launch.py @@ -0,0 +1,51 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import Command, LaunchConfiguration +from launch_ros.actions import Node + +def generate_launch_description(): + model = LaunchConfiguration('model') + gui = LaunchConfiguration('gui') + rvizconfig = LaunchConfiguration('rvizconfig') + + return LaunchDescription([ + DeclareLaunchArgument( + 'model', + default_value=os.path.join( + get_package_share_directory('spot_micro_rviz'), + 'urdf', + 'spot_micro.urdf.xacro'), + description='Path to robot URDF file'), + DeclareLaunchArgument( + 'gui', + default_value='true', + description='Flag to enable joint_state_publisher_gui'), + DeclareLaunchArgument( + 'rvizconfig', + default_value=os.path.join( + get_package_share_directory('spot_micro_rviz'), + 'rviz', + 'spot_micro.rviz'), + description='Path to RViz config file'), + + Node( + package='joint_state_publisher', + executable='joint_state_publisher', + name='joint_state_publisher', + parameters=[{'use_gui': gui}]), + + Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + parameters=[{'robot_description': Command(['xacro ', model])}]), + + Node( + package='rviz2', + executable='rviz2', + name='rviz', + arguments=['-d', rvizconfig], + output='screen'), + ]) diff --git a/spot_micro_rviz/launch/show_model.launch b/spot_micro_rviz/launch/show_model.launch deleted file mode 100644 index 1995ebe..0000000 --- a/spot_micro_rviz/launch/show_model.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - - diff --git a/spot_micro_rviz/launch/show_model_launch.py b/spot_micro_rviz/launch/show_model_launch.py new file mode 100644 index 0000000..1060466 --- /dev/null +++ b/spot_micro_rviz/launch/show_model_launch.py @@ -0,0 +1,40 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import Command, LaunchConfiguration +from launch_ros.actions import Node + +def generate_launch_description(): + model = LaunchConfiguration('model') + rvizconfig = LaunchConfiguration('rvizconfig') + + return LaunchDescription([ + DeclareLaunchArgument( + 'model', + default_value=os.path.join( + get_package_share_directory('spot_micro_rviz'), + 'urdf', + 'spot_micro.urdf.xacro'), + description='Path to robot URDF file'), + DeclareLaunchArgument( + 'rvizconfig', + default_value=os.path.join( + get_package_share_directory('spot_micro_rviz'), + 'rviz', + 'spot_micro.rviz'), + description='Path to RViz config file'), + + Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + parameters=[{'robot_description': Command(['xacro ', model])}]), + + Node( + package='rviz2', + executable='rviz2', + name='rviz', + arguments=['-d', rvizconfig], + output='screen'), + ]) diff --git a/spot_micro_rviz/launch/slam.launch b/spot_micro_rviz/launch/slam.launch deleted file mode 100644 index a623516..0000000 --- a/spot_micro_rviz/launch/slam.launch +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - diff --git a/spot_micro_rviz/launch/slam_launch.py b/spot_micro_rviz/launch/slam_launch.py new file mode 100644 index 0000000..610eced --- /dev/null +++ b/spot_micro_rviz/launch/slam_launch.py @@ -0,0 +1,40 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import Command, LaunchConfiguration +from launch_ros.actions import Node + +def generate_launch_description(): + model = LaunchConfiguration('model') + rvizconfig = LaunchConfiguration('rvizconfig') + + return LaunchDescription([ + DeclareLaunchArgument( + 'model', + default_value=os.path.join( + get_package_share_directory('spot_micro_rviz'), + 'urdf', + 'spot_micro.urdf.xacro'), + description='Path to robot URDF file'), + DeclareLaunchArgument( + 'rvizconfig', + default_value=os.path.join( + get_package_share_directory('spot_micro_rviz'), + 'rviz', + 'spot_micro_slam.rviz'), + description='Path to RViz config file'), + + Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + parameters=[{'robot_description': Command(['xacro ', model])}]), + + Node( + package='rviz2', + executable='rviz2', + name='rviz', + arguments=['-d', rvizconfig], + output='screen'), + ]) diff --git a/spot_micro_rviz/package.xml b/spot_micro_rviz/package.xml index 1708d24..0f3cce9 100644 --- a/spot_micro_rviz/package.xml +++ b/spot_micro_rviz/package.xml @@ -1,53 +1,24 @@ - + + spot_micro_rviz 0.0.0 - Spot Micro RVIZ Visualization Package - - - - - mike - - - - - + The spot_micro_rviz package + mike TODO + ament_cmake - - - - - - - - - - - - - - - - - - - - - catkin - roscpp - rospy - roscpp - rospy - roscpp - rospy + rviz2 + robot_state_publisher + joint_state_publisher + joint_state_publisher_gui + xacro + ament_lint_auto + ament_lint_common - - - + ament_cmake diff --git a/spot_micro_rviz/urdf/spot_micro.urdf b/spot_micro_rviz/urdf/spot_micro.urdf new file mode 100644 index 0000000..1470308 --- /dev/null +++ b/spot_micro_rviz/urdf/spot_micro.urdf @@ -0,0 +1,610 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +