diff --git a/stretch_core/CMakeLists.txt b/stretch_core/CMakeLists.txt deleted file mode 100644 index 596e2a1..0000000 --- a/stretch_core/CMakeLists.txt +++ /dev/null @@ -1,206 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(stretch_core) - -## 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 - actionlib - actionlib_msgs - geometry_msgs - nav_msgs - control_msgs - trajectory_msgs - rospy - std_msgs - std_srvs - tf - tf2 -) - -## 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 -# actionlib_msgs# geometry_msgs# nav_msgs# 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 stretch_core -# CATKIN_DEPENDS actionlib actionlib_msgs geometry_msgs nav_msgs rospy std_msgs tf tf2 -# 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}/stretch_core.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/stretch_core_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 -# 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_stretch_core.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/stretch_core/launch/d435i_basic.launch b/stretch_core/launch/d435i_basic.launch deleted file mode 100644 index 68f99cc..0000000 --- a/stretch_core/launch/d435i_basic.launch +++ /dev/null @@ -1,64 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/stretch_core/launch/d435i_basic.launch.py b/stretch_core/launch/d435i_basic.launch.py new file mode 100644 index 0000000..9318913 --- /dev/null +++ b/stretch_core/launch/d435i_basic.launch.py @@ -0,0 +1,57 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import PathJoinSubstitution, LaunchConfiguration +from launch_ros.substitutions import FindPackageShare + +# https://github.com/intel-ros/realsense +launch_arguments = {'accel_fps': '63', + 'gyro_fps': '200', + 'depth_fps': '15', + 'enable_infra1': 'false', + 'enable_infra2': 'false', + 'enable_accel': 'true', + 'depth_width': LaunchConfiguration('depth_width'), + 'depth_height': LaunchConfiguration('depth_height'), + 'color_width': LaunchConfiguration('color_width'), + 'color_height': LaunchConfiguration('color_height'), + 'color_fps': '15', + # publish depth streams aligned to other streams + 'align_depth': 'true', + # publish an RGBD point cloud + 'filters': 'pointcloud', + # enable_sync: gathers closest frames of different sensors, + # infra red, color and depth, to be sent with the same + # timetag. This happens automatically when such filters as + # pointcloud are enabled. + 'enable_sync': 'true', + # You can have a full depth to pointcloud, coloring the regions beyond the texture with zeros... + # Set to true in order to make use of the full field of view of + # the depth image instead of being restricted to the field of + # view associated with the narrower RGB camera. Note that + # points out of the RGB camera's field of view will have their + # colors set to 0,0,0. + 'allow_no_texture_points': 'true'}.items() + + +def generate_launch_description(): + d435i_accel_correction_node = Node(package='stretch_core', + executable='d435i_accel_correction', + output='screen') + + # "The D435i depth camera generates and transmits the gyro and + # accelerometer samples independently, as the inertial sensors + # exhibit different FPS rates (200/400Hz for gyro, 63/250Hz for + # accelerometer)." + # https://realsense.intel.com/how-to-getting-imu-data-from-d435i-and-t265/ + realsense2_camera_node = IncludeLaunchDescription(PythonLaunchDescriptionSource( + PathJoinSubstitution([FindPackageShare('realsense2_camera'), 'config', 'rs_launch.py'])), + launch_arguments=launch_arguments) + + return LaunchDescription([DeclareLaunchArgument('depth_width'), + DeclareLaunchArgument('depth_height'), + DeclareLaunchArgument('color_width'), + DeclareLaunchArgument('color_height'), + d435i_accel_correction_node, + realsense2_camera_node]) diff --git a/stretch_core/launch/d435i_high_resolution.launch b/stretch_core/launch/d435i_high_resolution.launch deleted file mode 100644 index ef22242..0000000 --- a/stretch_core/launch/d435i_high_resolution.launch +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - diff --git a/stretch_core/launch/d435i_high_resolution.launch.py b/stretch_core/launch/d435i_high_resolution.launch.py new file mode 100644 index 0000000..c605fe8 --- /dev/null +++ b/stretch_core/launch/d435i_high_resolution.launch.py @@ -0,0 +1,18 @@ +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import ThisLaunchFileDir, PathJoinSubstitution + + +# HIGHEST RESOLUTION, but also has the highest minimum depth +# (280mm Min-Z) below which objects generate bad noise, such as +# when the arm and gripper are raised close to the camera. +def generate_launch_description(): + return LaunchDescription( + # REALSENSE D435i + [IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([ThisLaunchFileDir(), 'd435i_basic.launch.py'])), + launch_arguments={'depth_width': '1280', + 'depth_height': '720', + 'color_width': '1280', + 'color_height': '720'}.items())]) diff --git a/stretch_core/launch/d435i_low_resolution.launch b/stretch_core/launch/d435i_low_resolution.launch deleted file mode 100644 index 495f739..0000000 --- a/stretch_core/launch/d435i_low_resolution.launch +++ /dev/null @@ -1,17 +0,0 @@ - - - - - - - - - - - - - diff --git a/stretch_core/launch/d435i_low_resolution.launch.py b/stretch_core/launch/d435i_low_resolution.launch.py new file mode 100644 index 0000000..6ad66a7 --- /dev/null +++ b/stretch_core/launch/d435i_low_resolution.launch.py @@ -0,0 +1,17 @@ +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import ThisLaunchFileDir, PathJoinSubstitution + +# LOWEST RESOLUTION, but also has the lowest minimum depth +# (105mm Min-Z) below which objects generate bad noise, such as +# when the arm and gripper are raised close to the camera. +def generate_launch_description(): + return LaunchDescription( + # REALSENSE D435i + [IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([ThisLaunchFileDir(), 'd435i_basic.launch.py'])), + launch_arguments={'depth_width': '424', + 'depth_height': '240', + 'color_width': '424', + 'color_height': '240'}.items())]) diff --git a/stretch_core/launch/imu_filter.launch b/stretch_core/launch/imu_filter.launch deleted file mode 100644 index e2f620d..0000000 --- a/stretch_core/launch/imu_filter.launch +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - diff --git a/stretch_core/launch/imu_filter.launch.py b/stretch_core/launch/imu_filter.launch.py new file mode 100644 index 0000000..4276df9 --- /dev/null +++ b/stretch_core/launch/imu_filter.launch.py @@ -0,0 +1,14 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + + return LaunchDescription([ + Node( + package='imu_filter_madgwick', executable='imu_filter_madgwick_node', name='imu_filter', output='screen', + remappings=[('/imu/data_raw', '/imu_mobile_base'), + ('/imu/mag', '/magnetometer_mobile_base')], + parameters=[{'use_mag': False, + 'fixed_frame': 'map'}] + ) + ]) \ No newline at end of file diff --git a/stretch_core/launch/keyboard_teleop.launch b/stretch_core/launch/keyboard_teleop.launch deleted file mode 100644 index 2a80c3b..0000000 --- a/stretch_core/launch/keyboard_teleop.launch +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - diff --git a/stretch_core/launch/keyboard_teleop.launch.py b/stretch_core/launch/keyboard_teleop.launch.py new file mode 100644 index 0000000..476bc9a --- /dev/null +++ b/stretch_core/launch/keyboard_teleop.launch.py @@ -0,0 +1,18 @@ +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from launch.substitutions import ThisLaunchFileDir, PathJoinSubstitution + + +def generate_launch_description(): + return LaunchDescription([ + # STRETCH DRIVER + IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([ThisLaunchFileDir(), 'stretch_driver.launch.py'])), + launch_arguments={'broadcast_odom_tf': 'true', + 'fail_out_of_range_goal': 'false'}.items()), + # KEYBOARD TELEOP + Node(package='stretch_core', + executable='keyboard_teleop', + output='screen')]) diff --git a/stretch_core/launch/stretch_aruco.launch b/stretch_core/launch/stretch_aruco.launch deleted file mode 100644 index b86f5f1..0000000 --- a/stretch_core/launch/stretch_aruco.launch +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - diff --git a/stretch_core/launch/stretch_aruco.launch.py b/stretch_core/launch/stretch_aruco.launch.py new file mode 100644 index 0000000..9ae6047 --- /dev/null +++ b/stretch_core/launch/stretch_aruco.launch.py @@ -0,0 +1,16 @@ +from launch import LaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + # ARUCO MARKER DETECTOR + return LaunchDescription([Node(package='stretch_core', + executable='detect_aruco_markers', + output='screen', + parameters=[PathJoinSubstitution([FindPackageShare('stretch_core'), + 'config', + 'stretch_marker_dict.yaml'])] + ) + ]) diff --git a/stretch_core/launch/stretch_driver.launch b/stretch_core/launch/stretch_driver.launch deleted file mode 100644 index 8fadf0f..0000000 --- a/stretch_core/launch/stretch_driver.launch +++ /dev/null @@ -1,44 +0,0 @@ - - - - - - - - - [/stretch/joint_states] - - - - - - - - - - - - - - - - - - - diff --git a/stretch_core/launch/stretch_driver.launch.py b/stretch_core/launch/stretch_driver.launch.py new file mode 100644 index 0000000..187e99e --- /dev/null +++ b/stretch_core/launch/stretch_driver.launch.py @@ -0,0 +1,49 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.substitutions import LaunchConfiguration +from launch.actions import DeclareLaunchArgument +import xacro + + +def generate_launch_description(): + robot_description_path = os.path.join(get_package_share_directory('stretch_description'), + 'urdf', + 'stretch.urdf') + + calibrated_controller_yaml_file = os.path.join(get_package_share_directory('stretch_core'), + 'config', + 'controller_calibration_head.yaml') + + joint_state_publisher = Node(package='joint_state_publisher', + executable='joint_state_publisher', + name='joint_state_publisher', + arguments=[robot_description_path], + output='log', + parameters=[{'source_list': ["/stretch/joint_states"]}, + {'rate': 15}]) + + robot_state_publisher = Node(package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='both', + parameters=[{'robot_description': xacro.process_file(robot_description_path).toxml()}, + {'publish_frequency': 15}]) + + stretch_driver = Node(package='stretch_core', + executable='stretch_driver', + name='stretch_driver', + remappings=[('cmd_vel', '/stretch/cmd_vel'), + ('joint_states', '/stretch/joint_states')], + parameters=[{'rate': 25.0}, + {'timeout': 0.5}, + {'controller_calibration_file': calibrated_controller_yaml_file}, + {'broadcast_odom_tf': LaunchConfiguration('broadcast_odom_tf')}, + {'fail_out_of_range_goal': LaunchConfiguration('fail_out_of_range_goal')}]) + + return LaunchDescription([DeclareLaunchArgument('broadcast_odom_tf'), + DeclareLaunchArgument('fail_out_of_range_goal'), + joint_state_publisher, + robot_state_publisher, + stretch_driver]) diff --git a/stretch_core/launch/stretch_ekf.launch b/stretch_core/launch/stretch_ekf.launch deleted file mode 100644 index 0d91667..0000000 --- a/stretch_core/launch/stretch_ekf.launch +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - diff --git a/stretch_core/launch/stretch_ekf.launch.py b/stretch_core/launch/stretch_ekf.launch.py new file mode 100644 index 0000000..74875f0 --- /dev/null +++ b/stretch_core/launch/stretch_ekf.launch.py @@ -0,0 +1,19 @@ +from launch import LaunchDescription +from launch.substitutions import PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + return LaunchDescription([Node(package='robot_localization', + executable='ekf_node', + parameters=[PathJoinSubstitution([FindPackageShare('stretch_core'), + 'launch', + 'stretch_ekf.yaml'])], + remappings=[ + # Placeholder for output topic remapping + # ('odometry/filtered', ''), + # ('accel/filtered', '') + ]) + ] + ) diff --git a/stretch_core/launch/wheel_odometry_test.launch b/stretch_core/launch/wheel_odometry_test.launch deleted file mode 100644 index f6583bc..0000000 --- a/stretch_core/launch/wheel_odometry_test.launch +++ /dev/null @@ -1,32 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/stretch_core/launch/wheel_odometry_test.launch.py b/stretch_core/launch/wheel_odometry_test.launch.py new file mode 100644 index 0000000..756bacc --- /dev/null +++ b/stretch_core/launch/wheel_odometry_test.launch.py @@ -0,0 +1,73 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import PathJoinSubstitution, LaunchConfiguration, EnvironmentVariable, ThisLaunchFileDir +from launch_ros.substitutions import FindPackageShare + +# TODO(JafarAbdi): Check if the following args are needed, they're not being used +configurable_parameters = [ + {'name': 'urdf_file', 'default': PathJoinSubstitution([FindPackageShare('stretch_description'), + 'urdf', + 'stretch.urdf'])}, + {'name': 'controller_yaml_file', 'default': PathJoinSubstitution([FindPackageShare('stretch_core'), + 'config', + 'controller_calibration_head.yaml'])}, + {'name': 'calibration_directory', 'default': PathJoinSubstitution([EnvironmentVariable('HELLO_FLEET_PATH'), + EnvironmentVariable('HELLO_FLEET_ID'), + 'calibration_ros'])}] + + +def declare_configurable_parameters(parameters): + return [DeclareLaunchArgument(param['name'], default_value=param['default']) for param in parameters] + + +def set_configurable_parameters(parameters): + return dict([(param['name'], LaunchConfiguration(param['name'])) for param in parameters]) + + +def generate_launch_description(): + # REALSENSE D435i + d435i_low_resolution_launch_description = IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([ThisLaunchFileDir(), 'd435i_low_resolution.launch.py']))) + + d435i_configure_node = Node(package='stretch_core', + executable='d435i_configure', + output='screen', + parameters=[{'initial_mode': 'High Accuracy'}] + ) + + # STRETCH DRIVER + stretch_driver_launch_description = IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([ThisLaunchFileDir(), 'stretch_driver.launch.py'])), + launch_arguments={'broadcast_odom_tf': 'true', + 'fail_out_of_range_goal': 'false'}.items()) + + # TODO(JafarAbdi): This's not ported yet to ROS2 + # LASER RANGE FINDER + # rplidar_launch_description = IncludeLaunchDescription( + # PythonLaunchDescriptionSource(PathJoinSubstitution([ThisLaunchFileDir(), 'rplidar.launch.py']))) + + # KEYBOARD TELEOP + keyboard_teleop_node = Node(package='stretch_core', + executable='keyboard_teleop', + output='screen' + ) + + # VISUALIZE + rviz_config_file = PathJoinSubstitution([FindPackageShare('stretch_core'), + 'rviz', + 'wheel_odometry_test.rviz']) + rviz_node = Node(package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=['-d', rviz_config_file] + ) + + return LaunchDescription(declare_configurable_parameters(configurable_parameters) + + [d435i_low_resolution_launch_description, + d435i_configure_node, + stretch_driver_launch_description, + keyboard_teleop_node, + rviz_node]) diff --git a/stretch_core/package.xml b/stretch_core/package.xml index 9d1e5bb..80a7f8d 100644 --- a/stretch_core/package.xml +++ b/stretch_core/package.xml @@ -48,45 +48,36 @@ - catkin - actionlib + ament_python actionlib_msgs geometry_msgs nav_msgs control_msgs trajectory_msgs - rospy std_msgs std_srvs - tf tf2 - actionlib actionlib_msgs geometry_msgs nav_msgs control_msgs trajectory_msgs - rospy std_msgs std_srvs - tf tf2 - actionlib actionlib_msgs geometry_msgs nav_msgs control_msgs trajectory_msgs - rospy + rclpy std_msgs std_srvs - tf tf2 - - + ament_python diff --git a/stretch_core/COLCON_IGNORE b/stretch_core/resource/stretch_core similarity index 100% rename from stretch_core/COLCON_IGNORE rename to stretch_core/resource/stretch_core diff --git a/stretch_core/setup.cfg b/stretch_core/setup.cfg new file mode 100644 index 0000000..ffc0452 --- /dev/null +++ b/stretch_core/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script-dir=$base/lib/stretch_core +[install] +install-scripts=$base/lib/stretch_core \ No newline at end of file diff --git a/stretch_core/setup.py b/stretch_core/setup.py new file mode 100644 index 0000000..c5cb38a --- /dev/null +++ b/stretch_core/setup.py @@ -0,0 +1,34 @@ +from setuptools import setup, find_packages +from glob import glob + +package_name = 'stretch_core' + +setup( + name=package_name, + version='0.2.0', + packages=find_packages(), + data_files=[ + ('share/ament_index/resource_index/packages', ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ('share/' + package_name + '/launch', glob('launch/*.launch.py')), + ('share/' + package_name + '/launch', ['launch/stretch_ekf.yaml']), + ('share/' + package_name + '/config', glob('config/*')) + ], + install_requires=['setuptools'], + url='', + license='', + author='Hello Robot Inc.', + author_email='support@hello-robot.com', + description='The stretch_core package', + entry_points={ + 'console_scripts': [ + 'd435i_accel_correction = stretch_core.d435i_accel_correction:main', + 'd435i_configure = stretch_core.d435i_configure:main', + 'd435i_frustum_visualizer = stretch_core.d435i_frustum_visualizer:main', + 'detect_aruco_markers = stretch_core.detect_aruco_markers:main', + 'keyboard = stretch_core.keyboard:main', + 'keyboard_teleop = stretch_core.keyboard_teleop:main', + 'stretch_driver = stretch_core.stretch_driver:main', + ], + }, +) diff --git a/stretch_core/stretch_core/__init__.py b/stretch_core/stretch_core/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/stretch_core/nodes/command_groups.py b/stretch_core/stretch_core/command_groups.py similarity index 100% rename from stretch_core/nodes/command_groups.py rename to stretch_core/stretch_core/command_groups.py diff --git a/stretch_core/nodes/d435i_accel_correction b/stretch_core/stretch_core/d435i_accel_correction.py similarity index 93% rename from stretch_core/nodes/d435i_accel_correction rename to stretch_core/stretch_core/d435i_accel_correction.py index 32f845a..6fc3e71 100755 --- a/stretch_core/nodes/d435i_accel_correction +++ b/stretch_core/stretch_core/d435i_accel_correction.py @@ -10,11 +10,11 @@ class D435iAccelCorrectionNode: def __init__(self): self.num_samples_to_skip = 4 self.sample_count = 0 - + def accel_callback(self, accel): self.accel = accel self.sample_count += 1 - if (self.sample_count % self.num_samples_to_skip) == 0: + if (self.sample_count % self.num_samples_to_skip) == 0: # This can avoid issues with the D435i's time stamps being too # far ahead for TF. self.accel.header.stamp = rospy.Time.now() @@ -28,7 +28,7 @@ class D435iAccelCorrectionNode: self.corrected_accel_pub.publish(self.accel) - + def main(self): rospy.init_node('D435iAccelCorrectionNode') self.node_name = rospy.get_name() @@ -36,11 +36,11 @@ class D435iAccelCorrectionNode: self.topic_name = '/camera/accel/sample' self.accel_subscriber = rospy.Subscriber(self.topic_name, Imu, self.accel_callback) - + self.corrected_accel_pub = rospy.Publisher('/camera/accel/sample_corrected', Imu, queue_size=1) -if __name__ == '__main__': +def main(): node = D435iAccelCorrectionNode() node.main() try: @@ -48,3 +48,6 @@ if __name__ == '__main__': except KeyboardInterrupt: print('interrupt received, so shutting down') + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/stretch_core/nodes/d435i_configure b/stretch_core/stretch_core/d435i_configure.py similarity index 95% rename from stretch_core/nodes/d435i_configure rename to stretch_core/stretch_core/d435i_configure.py index 89da89c..e97a54a 100755 --- a/stretch_core/nodes/d435i_configure +++ b/stretch_core/stretch_core/d435i_configure.py @@ -12,21 +12,21 @@ class D435iConfigureNode: self.rate = 1.0 self.visual_preset = None self.mode_lock = threading.Lock() - + def turn_on_default_mode(self): - with self.mode_lock: + with self.mode_lock: self.locked_mode_id = 1 self.locked_mode_name = 'Default' self.parameter_client.update_configuration({'visual_preset' : self.locked_mode_name}) rospy.loginfo("Set D435i to {0} mode".format(self.locked_mode_name)) - + def turn_on_high_accuracy_mode(self): - with self.mode_lock: + with self.mode_lock: self.locked_mode_id = 3 self.locked_mode_name = 'High Accuracy' self.parameter_client.update_configuration({'visual_preset' : self.locked_mode_name}) rospy.loginfo("Set D435i to {0} mode".format(self.locked_mode_name)) - + def default_mode_service_callback(self, request): self.turn_on_default_mode() return TriggerResponse( @@ -56,7 +56,7 @@ class D435iConfigureNode: self.high_accuracy_mode_service_callback) initial_mode = rospy.get_param('~initial_mode') - + rospy.loginfo("initial_mode = {0}".format(initial_mode)) if initial_mode == 'High Accuracy': @@ -67,15 +67,19 @@ class D435iConfigureNode: error_string = 'initial_mode = {0} not recognized. Setting to D435i to Default mode.'.format(initial_mode) rospy.logerr(error_string) self.turn_on_default_mode() - + rate = rospy.Rate(self.rate) while not rospy.is_shutdown(): rate.sleep() - -if __name__ == '__main__': - try: + +def main(): + try: node = D435iConfigureNode() node.main() except KeyboardInterrupt: print('interrupt received, so shutting down') + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/stretch_core/nodes/d435i_frustum_visualizer b/stretch_core/stretch_core/d435i_frustum_visualizer.py similarity index 97% rename from stretch_core/nodes/d435i_frustum_visualizer rename to stretch_core/stretch_core/d435i_frustum_visualizer.py index 2e1bbd0..00a4a05 100755 --- a/stretch_core/nodes/d435i_frustum_visualizer +++ b/stretch_core/stretch_core/d435i_frustum_visualizer.py @@ -12,7 +12,7 @@ from visualization_msgs.msg import MarkerArray from geometry_msgs.msg import Transform, Pose, Vector3, Quaternion, Point from sensor_msgs.msg import CameraInfo - + class FrustumNode: def __init__(self): self.color_count = 0 @@ -20,8 +20,8 @@ class FrustumNode: self.camera_types = ['depth', 'color'] # only publish a single frustum for every N camera info # messages received - self.skip_publishing = 5 - + self.skip_publishing = 5 + # minimum-Z depth for D435i # 1280x720 0.28 m # 848x480 0.195 m @@ -45,12 +45,12 @@ class FrustumNode: if (self.depth_count % self.skip_publishing) == 0: self.camera_info_callback(camera_info, camera_type='depth') self.depth_count = self.depth_count + 1 - + def color_camera_info_callback(self, camera_info): if (self.color_count % self.skip_publishing) == 0: self.camera_info_callback(camera_info, camera_type='color') self.color_count = self.color_count + 1 - + def camera_info_callback(self, camera_info, camera_type=None): if camera_type not in self.camera_types: print('WARNING: FrustumNode camera_info_callback camera_type = {0} unrecognized. Valid types = {1}.'.format(camera_type, self.camera_types)) @@ -110,21 +110,21 @@ class FrustumNode: marker.scale.x = 1.0 marker.scale.y = 1.0 marker.scale.z = 1.0 - if camera_type == 'depth': + if camera_type == 'depth': # frustum color and transparency # gray marker.color.r = 1.0 marker.color.g = 1.0 marker.color.b = 1.0 marker.color.a = 0.4 - elif camera_type == 'color': + elif camera_type == 'color': # frustum color and transparency # yellow marker.color.r = 1.0 marker.color.g = 1.0 marker.color.b = 0.0 marker.color.a = 0.4 - + def adjacent_corners(c0, c1): # return True if the corners are adjacent to one another and False otherwise faces0 = c0[1] @@ -172,23 +172,23 @@ class FrustumNode: points = [] quad_ids = ['near', 'far', 'top', 'bottom', 'left', 'right'] - for s in quad_ids: + for s in quad_ids: quad_xyz = corners_to_quad(frustum_corners_xyz, s) triangles = quad_to_triangles(quad_xyz) points.extend(triangles) marker.points = points - + if camera_type == 'depth': self.depth_frustum_marker_pub.publish(marker) elif camera_type == 'color': self.color_frustum_marker_pub.publish(marker) - + def main(self): rospy.init_node('FrustumNode') self.node_name = rospy.get_name() rospy.loginfo("{0} started".format(self.node_name)) - + self.color_camera_topic = '/camera/color/camera_info' self.depth_camera_topic = '/camera/depth/camera_info' self.depth_camera_info_subscriber = rospy.Subscriber(self.depth_camera_topic, CameraInfo, self.depth_camera_info_callback) @@ -198,10 +198,14 @@ class FrustumNode: self.color_frustum_marker_pub = rospy.Publisher('/frustum_marker/color_camera', Marker, queue_size=1) -if __name__ == '__main__': +def main(): node = FrustumNode() node.main() try: rospy.spin() except KeyboardInterrupt: print('interrupt received, so shutting down') + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/stretch_core/nodes/detect_aruco_markers b/stretch_core/stretch_core/detect_aruco_markers.py similarity index 95% rename from stretch_core/nodes/detect_aruco_markers rename to stretch_core/stretch_core/detect_aruco_markers.py index 353cbad..9cac1d3 100755 --- a/stretch_core/nodes/detect_aruco_markers +++ b/stretch_core/stretch_core/detect_aruco_markers.py @@ -40,7 +40,7 @@ class ArucoMarker: id_color_image = cv2.applyColorMap(image, colormap) bgr = id_color_image[0,0] self.id_color = [bgr[2], bgr[1], bgr[0]] - + self.frame_id = '/camera_color_optical_frame' self.info = marker_info.get(str(self.aruco_id), None) @@ -48,12 +48,12 @@ class ArucoMarker: self.info = marker_info['default'] self.length_of_marker_mm = self.info['length_mm'] self.use_rgb_only = self.info['use_rgb_only'] - + # Distance beyond which the depth image depth will be # used to estimate the marker position. # 280mm is the minimum depth for the D435i at 1280x720 resolution - self.min_z_to_use_depth_image = 0.28 + (self.length_of_marker_mm/1000.0) - + self.min_z_to_use_depth_image = 0.28 + (self.length_of_marker_mm/1000.0) + self.marker = Marker() self.marker.type = self.marker.CUBE self.marker.action = self.marker.ADD @@ -72,10 +72,10 @@ class ArucoMarker: self.used_depth_image = False self.broadcasted = False - + def get_marker_point_cloud(self): return self.points_array - + def get_plane_fit_point_cloud(self): if self.plane is None: return None @@ -138,7 +138,7 @@ class ArucoMarker: # TODO: Better handle situations when the cropped rectangle # contains no reasonable depth values. - + # First, weakly filter the points using the RGB only depth # estimate from the ArUco code. This is a weak filter due to # the estimate's sensitivity to corner detection errors. @@ -167,7 +167,7 @@ class ArucoMarker: # depths. points_array = points_array[(mask_crop.flatten() > 0) & mask_z.flatten()] - if display_images: + if display_images: display_depth_crop = cv2.normalize(depth_crop, None, 0, 255, cv2.NORM_MINMAX, cv2.CV_8U) cv2.imshow('Cropped Depth {0}'.format(id_num), display_depth_crop) display_mask_crop = cv2.normalize(mask_crop, None, 0, 255, cv2.NORM_MINMAX, cv2.CV_8U) @@ -176,11 +176,11 @@ class ArucoMarker: points_array = np.empty((0,3), dtype=np.float64) self.points_array = points_array - - + + def update(self, corners, timestamp, frame_number, camera_info, depth_image=None): self.ready = True - self.corners = corners + self.corners = corners self.timestamp = timestamp self.frame_number = frame_number self.camera_info = camera_info @@ -192,15 +192,15 @@ class ArucoMarker: self.camera_matrix, self.distortion_coefficients) self.aruco_rotation = rvecs[0][0] - + # Convert ArUco position estimate to be in meters. self.aruco_position = tvecs[0][0]/1000.0 aruco_depth_estimate = self.aruco_position[2] - + self.center = np.average(self.corners, axis=1).flatten() self.min_dists = np.min((self.corners - self.center), axis=1).flatten() self.max_dists = np.max((self.corners - self.center), axis=1).flatten() - + if (self.depth_image is not None) and (aruco_depth_estimate > self.min_z_to_use_depth_image) and (not self.use_rgb_only): only_use_rgb = False @@ -209,7 +209,7 @@ class ArucoMarker: # do not proceed with fitting a plane and instead only use # the RGB image to perform the 3D estimation using the # ArUco code. - + self.update_marker_point_cloud(aruco_depth_estimate) num_points = self.points_array.shape[0] min_number_of_points_for_plane_fitting = 16 @@ -228,12 +228,12 @@ class ArucoMarker: # and their projections are used to estimate the marker's # position and the coordinate system on the plane (x axis # and y axis). The z axis is normal to the fit plane. - + self.used_depth_image = True - + self.plane = fp.FitPlane() self.plane.fit_svd(self.points_array, verbose=False) - + # Find the points on the fit plane corresponding with the # four ArUco corners. Then, use the mean of the 4 points # as the 3D center for the marker. @@ -274,30 +274,30 @@ class ArucoMarker: # Use the corners on the fit plane to estimate the x and y # axes for the marker. top_left, top_right, bottom_right, bottom_left = corner_points - + y_axis = (top_left + top_right) - (bottom_left + bottom_right) y_length = np.linalg.norm(y_axis) - if y_length > 0.0: + if y_length > 0.0: y_axis = y_axis/y_length else: y_axis = None - + x_axis = (top_right + bottom_right) - (top_left + bottom_left) x_length = np.linalg.norm(x_axis) if x_length > 0.0: x_axis = x_axis/x_length else: x_axis = None - + plane_normal = self.plane.get_plane_normal() R = np.identity(4) R[:3,:3] = cv2.Rodrigues(self.aruco_rotation)[0] - if x_axis is not None: + if x_axis is not None: old_x_axis = np.reshape(x_axis, (3,1)) else: - old_x_axis = np.reshape(R[:3,0], (3,1)) - if y_axis is not None: + old_x_axis = np.reshape(R[:3,0], (3,1)) + if y_axis is not None: old_y_axis = np.reshape(y_axis, (3,1)) else: old_y_axis = np.reshape(R[:3,1], (3,1)) @@ -327,10 +327,10 @@ class ArucoMarker: new_y_axis_1 = new_y_axis_1/np.linalg.norm(new_y_axis_1) new_x_axis_1 = np.reshape(np.cross(new_y_axis_1.flatten(), new_z_axis.flatten()), (3,1)) - + new_x_axis_2 = old_x_axis - (np.matmul(new_z_axis.transpose(), old_x_axis) * new_z_axis) new_x_axis_2 = new_x_axis_2/np.linalg.norm(new_x_axis_2) - + new_x_axis = (new_x_axis_1 + new_x_axis_2)/2.0 new_x_axis = new_x_axis/np.linalg.norm(new_x_axis) new_y_axis = np.reshape(np.cross(new_z_axis.flatten(), new_x_axis.flatten()), (3,1)) @@ -338,7 +338,7 @@ class ArucoMarker: self.x_axis = new_x_axis.flatten() self.y_axis = new_y_axis.flatten() self.z_axis = new_z_axis.flatten() - + R[:3,0] = self.x_axis R[:3,1] = self.y_axis R[:3,2] = self.z_axis @@ -366,17 +366,17 @@ class ArucoMarker: poly_points = np.round(poly_points).astype(np.int32) return poly_points - def draw_marker_poly(self, image): + def draw_marker_poly(self, image): poly_points = self.get_marker_poly() cv2.fillConvexPoly(image, poly_points, (255, 0, 0)) def broadcast_tf(self, tf_broadcaster, force_redundant=False): # Create TF frame for the marker. By default, only broadcast a # single time after an update. - if (not self.broadcasted) or force_redundant: + if (not self.broadcasted) or force_redundant: tf_broadcaster.sendTransform(self.marker_position, self.marker_quaternion, self.timestamp, self.marker.text, self.frame_id) self.broadcasted = True - + def get_ros_marker(self): if not self.ready: return None @@ -405,12 +405,12 @@ class ArucoMarker: self.marker.pose.orientation.x = q[0] self.marker.pose.orientation.y = q[1] self.marker.pose.orientation.z = q[2] - self.marker.pose.orientation.w = q[3] + self.marker.pose.orientation.w = q[3] return self.marker - def create_axis_marker(self, axis, id_num, rgba=None, name=None): + def create_axis_marker(self, axis, id_num, rgba=None, name=None): marker = Marker() marker.header.frame_id = self.frame_id marker.header.stamp = self.timestamp @@ -422,7 +422,7 @@ class ArucoMarker: marker.text = name axis_arrow = {'head_diameter': 0.005, 'shaft_diameter': 0.003, - 'head_length': 0.003, + 'head_length': 0.003, 'length': 0.02} # "scale.x is the shaft diameter, and scale.y is the # head diameter. If scale.z is not zero, it specifies @@ -432,7 +432,7 @@ class ArucoMarker: marker.scale.y = axis_arrow['head_diameter'] marker.scale.z = axis_arrow['head_length'] - if rgba is None: + if rgba is None: # make as bright as possible den = float(np.max(self.id_color)) marker.color.r = self.id_color[2]/den @@ -458,7 +458,7 @@ class ArucoMarker: marker.points = [start_point, end_point] return marker - + def get_ros_z_axis_marker(self): if not self.ready: return None @@ -466,15 +466,15 @@ class ArucoMarker: if self.plane is None: return None - if self.used_depth_image and (self.z_axis is not None): + if self.used_depth_image and (self.z_axis is not None): id_num = 3 * self.aruco_id return self.create_axis_marker(self.z_axis, id_num, rgba=None) else: return None - + def get_ros_axes_markers(self): markers = [] - + if not self.ready: return markers @@ -484,7 +484,7 @@ class ArucoMarker: # z axis is blue base_name = self.info['name'] - + if self.z_axis is not None: id_num = 3 * self.aruco_id rgba = [0.0, 0.0, 1.0, 0.33] @@ -500,11 +500,11 @@ class ArucoMarker: rgba = [0.0, 1.0, 0.0, 0.33] name = base_name = '_y_axis' markers.append(self.create_axis_marker(self.y_axis, id_num, rgba, name)) - + return markers - - + + class ArucoMarkerCollection: def __init__(self, marker_info): self.marker_info = marker_info @@ -549,8 +549,8 @@ class ArucoMarkerCollection: num_detected = 0 else: num_detected = len(self.aruco_ids) - - if self.aruco_ids is not None: + + if self.aruco_ids is not None: for corners, aruco_id in zip(self.aruco_corners, self.aruco_ids): aruco_id = int(aruco_id) marker = self.collection.get(aruco_id, None) @@ -561,7 +561,7 @@ class ArucoMarkerCollection: self.collection[aruco_id].update(corners, self.timestamp, self.frame_number, self.camera_info, self.depth_image) def get_ros_marker_array(self): - marker_array = MarkerArray() + marker_array = MarkerArray() for key in self.collection: marker = self.collection[key] if marker.frame_number == self.frame_number: @@ -570,7 +570,7 @@ class ArucoMarkerCollection: return marker_array def get_ros_axes_array(self, include_z_axes=True, include_axes=True): - marker_array = MarkerArray() + marker_array = MarkerArray() for key in self.collection: marker = self.collection[key] if marker.frame_number == self.frame_number: @@ -582,15 +582,15 @@ class ArucoMarkerCollection: ros_axes_markers= marker.get_ros_axes_markers() marker_array.markers.extend(ros_axes_markers) return marker_array - - + + class DetectArucoNode: def __init__(self): self.cv_bridge = CvBridge() self.rgb_image = None self.rgb_image_timestamp = None self.depth_image = None - self.depth_image_timestamp = None + self.depth_image_timestamp = None self.camera_info = None self.all_points = [] self.display_images = False @@ -606,7 +606,7 @@ class DetectArucoNode: print(error) self.camera_info = rgb_camera_info - + # Copy the depth image to avoid a change to the depth image # during the update. time_diff = self.rgb_image_timestamp - self.depth_image_timestamp @@ -616,25 +616,25 @@ class DetectArucoNode: print(' The time difference between their timestamps =', closest_time_diff, 's') self.aruco_marker_collection.update(self.rgb_image, self.camera_info, self.depth_image, self.rgb_image_timestamp) - + marker_array = self.aruco_marker_collection.get_ros_marker_array() include_axes = True include_z_axes = False axes_array = None - if include_axes or include_z_axes: + if include_axes or include_z_axes: axes_array = self.aruco_marker_collection.get_ros_axes_array(include_z_axes, include_axes) # Create TF frames for each of the markers. Only broadcast # each marker a single time after it has been updated. self.aruco_marker_collection.broadcast_tf(self.tf_broadcaster) - + for m in marker_array.markers: if m.text == 'wrist_inside': self.wrist_inside_marker_pub.publish(m) if m.text == 'wrist_top': self.wrist_top_marker_pub.publish(m) - - if self.publish_marker_point_clouds: + + if self.publish_marker_point_clouds: for marker in self.aruco_marker_collection: marker_points = marker.get_marker_point_cloud() self.add_point_array_to_point_cloud(marker_points) @@ -642,7 +642,7 @@ class DetectArucoNode: self.add_point_array_to_point_cloud(plane_points) self.publish_point_cloud() self.visualize_markers_pub.publish(marker_array) - if axes_array is not None: + if axes_array is not None: self.visualize_axes_pub.publish(axes_array) # save rotation for last @@ -660,7 +660,7 @@ class DetectArucoNode: def add_point_array_to_point_cloud(self, point_array): self.all_points.extend(list(point_array)) - + def publish_point_cloud(self): header = Header() header.frame_id = '/camera_color_optical_frame' @@ -678,12 +678,12 @@ class DetectArucoNode: point_cloud = point_cloud2.create_cloud(header, fields, points) self.visualize_point_cloud_pub.publish(point_cloud) self.all_points = [] - + def main(self): rospy.init_node('DetectArucoNode') self.node_name = rospy.get_name() rospy.loginfo("{0} started".format(self.node_name)) - + self.marker_info = rospy.get_param('/aruco_marker_info') self.aruco_marker_collection = ArucoMarkerCollection(self.marker_info) @@ -700,7 +700,7 @@ class DetectArucoNode: self.synchronizer = message_filters.TimeSynchronizer([self.rgb_image_subscriber, self.depth_image_subscriber, self.camera_info_subscriber], 10) self.synchronizer.registerCallback(self.image_callback) - + self.visualize_markers_pub = rospy.Publisher('/aruco/marker_array', MarkerArray, queue_size=1) self.visualize_axes_pub = rospy.Publisher('/aruco/axes', MarkerArray, queue_size=1) self.visualize_point_cloud_pub = rospy.Publisher('/aruco/point_cloud2', PointCloud2, queue_size=1) @@ -711,7 +711,7 @@ class DetectArucoNode: self.tf_broadcaster = tf.TransformBroadcaster() -if __name__ == '__main__': +def main(): node = DetectArucoNode() node.main() try: @@ -719,3 +719,7 @@ if __name__ == '__main__': except KeyboardInterrupt: print('interrupt received, so shutting down') cv2.destroyAllWindows() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/stretch_core/nodes/joint_trajectory_server.py b/stretch_core/stretch_core/joint_trajectory_server.py similarity index 100% rename from stretch_core/nodes/joint_trajectory_server.py rename to stretch_core/stretch_core/joint_trajectory_server.py diff --git a/stretch_core/nodes/keyboard.py b/stretch_core/stretch_core/keyboard.py similarity index 94% rename from stretch_core/nodes/keyboard.py rename to stretch_core/stretch_core/keyboard.py index 5d8c050..4f4695c 100644 --- a/stretch_core/nodes/keyboard.py +++ b/stretch_core/stretch_core/keyboard.py @@ -34,9 +34,10 @@ def getch(): return ch -if __name__ == '__main__': - - while True: - c = getch() - print('c') +def main(): + while True: + c = getch() + print('c') +if __name__ == '__main__': + main() diff --git a/stretch_core/nodes/keyboard_teleop b/stretch_core/stretch_core/keyboard_teleop.py similarity index 95% rename from stretch_core/nodes/keyboard_teleop rename to stretch_core/stretch_core/keyboard_teleop.py index e7f2bc8..f9c29c1 100755 --- a/stretch_core/nodes/keyboard_teleop +++ b/stretch_core/stretch_core/keyboard_teleop.py @@ -45,9 +45,9 @@ class GetKeyboardCommands: if self.step_size == 'small': deltas = {'rad': self.small_rad, 'translate': self.small_translate} if self.step_size == 'medium': - deltas = {'rad': self.medium_rad, 'translate': self.medium_translate} + deltas = {'rad': self.medium_rad, 'translate': self.medium_translate} if self.step_size == 'big': - deltas = {'rad': self.big_rad, 'translate': self.big_translate} + deltas = {'rad': self.big_rad, 'translate': self.big_translate} return deltas def print_commands(self): @@ -89,114 +89,114 @@ class GetKeyboardCommands: c = kb.getch() #rospy.loginfo('c =', c) - + #################################################### ## MOSTLY MAPPING RELATED CAPABILITIES ## (There are non-mapping outliers.) #################################################### - + # Sequential performs a fixed number of autonomus mapping iterations if (c == '!') and self.mapping_on: number_iterations = 4 for n in range(number_iterations): # Trigger a 3D scan with the D435i - trigger_request = TriggerRequest() + trigger_request = TriggerRequest() trigger_result = node.trigger_head_scan_service(trigger_request) rospy.loginfo('trigger_result = {0}'.format(trigger_result)) # Trigger driving the robot to the estimated next best place to scan - trigger_request = TriggerRequest() + trigger_request = TriggerRequest() trigger_result = node.trigger_drive_to_scan_service(trigger_request) rospy.loginfo('trigger_result = {0}'.format(trigger_result)) - + # Trigger localizing the robot to a new pose anywhere on the current map if ((c == '+') or (c == '=')) and self.mapping_on: - trigger_request = TriggerRequest() + trigger_request = TriggerRequest() trigger_result = node.trigger_global_localization_service(trigger_request) rospy.loginfo('trigger_result = {0}'.format(trigger_result)) # Trigger localizing the robot to a new pose that is near its current pose on the map if ((c == '-') or (c == '_')) and self.mapping_on: - trigger_request = TriggerRequest() + trigger_request = TriggerRequest() trigger_result = node.trigger_local_localization_service(trigger_request) rospy.loginfo('trigger_result = {0}'.format(trigger_result)) # Trigger driving the robot to the estimated next best place to perform a 3D scan if ((c == '\\') or (c == '|')) and self.mapping_on: - trigger_request = TriggerRequest() + trigger_request = TriggerRequest() trigger_result = node.trigger_drive_to_scan_service(trigger_request) rospy.loginfo('trigger_result = {0}'.format(trigger_result)) # Trigger performing a 3D scan using the D435i if (c == ' ') and self.mapping_on: - trigger_request = TriggerRequest() + trigger_request = TriggerRequest() trigger_result = node.trigger_head_scan_service(trigger_request) rospy.loginfo('trigger_result = {0}'.format(trigger_result)) # Trigger rotating the mobile base to align with the nearest 3D cliff detected visually if ((c == '[') or (c == '{')) and self.mapping_on: - trigger_request = TriggerRequest() + trigger_request = TriggerRequest() trigger_result = node.trigger_align_with_nearest_cliff_service(trigger_request) rospy.loginfo('trigger_result = {0}'.format(trigger_result)) # DEPRECATED: Trigger extend arm until contact if ((c == ']') or (c == '}')) and self.mapping_on: - trigger_request = TriggerRequest() + trigger_request = TriggerRequest() trigger_result = node.trigger_reach_until_contact_service(trigger_request) rospy.loginfo('trigger_result = {0}'.format(trigger_result)) # DEPRECATED: Trigger lower arm until contact if ((c == ':') or (c == ';')) and self.mapping_on: - trigger_request = TriggerRequest() + trigger_request = TriggerRequest() trigger_result = node.trigger_lower_until_contact_service(trigger_request) rospy.loginfo('trigger_result = {0}'.format(trigger_result)) - - + + #################################################### ## OTHER CAPABILITIES #################################################### # Trigger Hello World whiteboard writing demo if ((c == '`') or (c == '~')) and self.hello_world_on: - trigger_request = TriggerRequest() + trigger_request = TriggerRequest() trigger_result = node.trigger_write_hello_service(trigger_request) rospy.loginfo('trigger_result = {0}'.format(trigger_result)) # Trigger open drawer demo with downward hook motion if ((c == 'z') or (c == 'Z')) and self.open_drawer_on: - trigger_request = TriggerRequest() + trigger_request = TriggerRequest() trigger_result = node.trigger_open_drawer_down_service(trigger_request) rospy.loginfo('trigger_result = {0}'.format(trigger_result)) # Trigger open drawer demo with upward hook motion if ((c == '.') or (c == '>')) and self.open_drawer_on: - trigger_request = TriggerRequest() + trigger_request = TriggerRequest() trigger_result = node.trigger_open_drawer_up_service(trigger_request) rospy.loginfo('trigger_result = {0}'.format(trigger_result)) # Trigger clean surface demo if ((c == '/') or (c == '?')) and self.clean_surface_on: - trigger_request = TriggerRequest() + trigger_request = TriggerRequest() trigger_result = node.trigger_clean_surface_service(trigger_request) rospy.loginfo('trigger_result = {0}'.format(trigger_result)) - - # Trigger grasp object demo + + # Trigger grasp object demo if ((c == '\'') or (c == '\"')) and self.grasp_object_on: - trigger_request = TriggerRequest() + trigger_request = TriggerRequest() trigger_result = node.trigger_grasp_object_service(trigger_request) rospy.loginfo('trigger_result = {0}'.format(trigger_result)) - # Trigger deliver object demo + # Trigger deliver object demo if ((c == 'y') or (c == 'Y')) and self.deliver_object_on: - trigger_request = TriggerRequest() + trigger_request = TriggerRequest() trigger_result = node.trigger_deliver_object_service(trigger_request) rospy.loginfo('trigger_result = {0}'.format(trigger_result)) - + #################################################### ## BASIC KEYBOARD TELEOPERATION COMMANDS #################################################### - + # 8 or up arrow if c == '8' or c == '\x1b[A': command = {'joint': 'joint_lift', 'delta': self.get_deltas()['translate']} @@ -217,7 +217,7 @@ class GetKeyboardCommands: # 6 or right arrow if c == '6' or c == '\x1b[C': command = {'joint': 'translate_mobile_base', 'inc': -self.get_deltas()['translate']} - # 1 or end key + # 1 or end key if c == '7' or c == '\x1b[H': command = {'joint': 'rotate_mobile_base', 'inc': self.get_deltas()['rad']} # 3 or pg down 5~ @@ -290,7 +290,7 @@ class KeyboardTeleopNode(hm.HelloNode): point.time_from_start = rospy.Duration(0.0) trajectory_goal = FollowJointTrajectoryGoal() trajectory_goal.goal_time_tolerance = rospy.Time(1.0) - + joint_name = command['joint'] trajectory_goal.trajectory.joint_names = [joint_name] if 'inc' in command: @@ -313,7 +313,7 @@ class KeyboardTeleopNode(hm.HelloNode): def main(self): hm.HelloNode.main(self, 'keyboard_teleop', 'keyboard_teleop', wait_for_first_pointcloud=False) - if self.mapping_on: + if self.mapping_on: rospy.loginfo('Node ' + self.node_name + ' waiting to connect to /funmap/trigger_head_scan.') rospy.wait_for_service('/funmap/trigger_head_scan') @@ -344,7 +344,7 @@ class KeyboardTeleopNode(hm.HelloNode): rospy.loginfo('Node ' + self.node_name + ' connected to /funmap/trigger_lower_until_contact.') self.trigger_lower_until_contact_service = rospy.ServiceProxy('/funmap/trigger_lower_until_contact', Trigger) - if self.hello_world_on: + if self.hello_world_on: rospy.wait_for_service('/hello_world/trigger_write_hello') rospy.loginfo('Node ' + self.node_name + ' connected to /hello_world/trigger_write_hello.') self.trigger_write_hello_service = rospy.ServiceProxy('/hello_world/trigger_write_hello', Trigger) @@ -358,7 +358,7 @@ class KeyboardTeleopNode(hm.HelloNode): rospy.loginfo('Node ' + self.node_name + ' connected to /open_drawer/trigger_open_drawer_up.') self.trigger_open_drawer_up_service = rospy.ServiceProxy('/open_drawer/trigger_open_drawer_up', Trigger) - + if self.clean_surface_on: rospy.wait_for_service('/clean_surface/trigger_clean_surface') rospy.loginfo('Node ' + self.node_name + ' connected to /clean_surface/trigger_clean_surface.') @@ -386,7 +386,7 @@ class KeyboardTeleopNode(hm.HelloNode): rate.sleep() -if __name__ == '__main__': +def main(): try: parser = ap.ArgumentParser(description='Keyboard teleoperation for stretch.') parser.add_argument('--mapping_on', action='store_true', help='Turn on mapping control. For example, the space bar will trigger a head scan. This requires that the mapping node be run (funmap).') @@ -408,3 +408,7 @@ if __name__ == '__main__': node.main() except KeyboardInterrupt: rospy.loginfo('interrupt received, so shutting down') + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/stretch_core/nodes/rwlock.py b/stretch_core/stretch_core/rwlock.py similarity index 100% rename from stretch_core/nodes/rwlock.py rename to stretch_core/stretch_core/rwlock.py diff --git a/stretch_core/nodes/stretch_driver b/stretch_core/stretch_core/stretch_driver.py similarity index 98% rename from stretch_core/nodes/stretch_driver rename to stretch_core/stretch_core/stretch_driver.py index 5319f86..ce39f22 100755 --- a/stretch_core/nodes/stretch_driver +++ b/stretch_core/stretch_core/stretch_driver.py @@ -4,7 +4,7 @@ from __future__ import print_function import yaml import numpy as np import threading -from rwlock import RWLock +from .rwlock import RWLock import stretch_body.robot as rb from stretch_body.hello_utils import ThreadServiceExit @@ -91,7 +91,7 @@ class StretchBodyNode: # set new mobile base velocities, if appropriate # check on thread safety for this with callback that sets velocity command values - if self.robot_mode == 'navigation': + if self.robot_mode == 'navigation': time_since_last_twist = rospy.get_time() - self.last_twist_time if time_since_last_twist < self.timeout: self.robot.base.set_velocity(self.linear_velocity_mps, self.angular_velocity_radps) @@ -109,7 +109,7 @@ class StretchBodyNode: # motor control boards and other boards. These would need to # be synchronized with the rospy clock. #robot_time = robot_status['timestamp_pc'] - #rospy.loginfo('robot_time =', robot_time) + #rospy.loginfo('robot_time =', robot_time) #current_time = rospy.Time.from_sec(robot_time) current_time = rospy.Time.now() @@ -138,10 +138,10 @@ class StretchBodyNode: arm_status = robot_status['arm'] if self.backlash_state['wrist_extension_retracted']: arm_backlash_correction = self.wrist_extension_calibrated_retracted_offset_m - else: + else: arm_backlash_correction = 0.0 - if BACKLASH_DEBUG: + if BACKLASH_DEBUG: print('arm_backlash_correction =', arm_backlash_correction) pos_out = arm_status['pos'] + arm_backlash_correction vel_out = arm_status['vel'] @@ -152,7 +152,7 @@ class StretchBodyNode: vel_up = lift_status['vel'] eff_up = lift_status['motor']['effort'] - if self.use_robotis_end_of_arm: + if self.use_robotis_end_of_arm: # assign relevant wrist status to variables wrist_status = robot_status['end_of_arm']['wrist_yaw'] wrist_rad = wrist_status['pos'] @@ -161,24 +161,24 @@ class StretchBodyNode: # assign relevant gripper status to variables gripper_status = robot_status['end_of_arm']['stretch_gripper'] - if GRIPPER_DEBUG: + if GRIPPER_DEBUG: print('-----------------------') print('gripper_status[\'pos\'] =', gripper_status['pos']) print('gripper_status[\'pos_pct\'] =', gripper_status['pos_pct']) gripper_aperture_m, gripper_finger_rad, gripper_finger_effort, gripper_finger_vel = self.gripper_conversion.status_to_all(gripper_status) - if GRIPPER_DEBUG: + if GRIPPER_DEBUG: print('gripper_aperture_m =', gripper_aperture_m) print('gripper_finger_rad =', gripper_finger_rad) print('-----------------------') - if self.use_robotis_head: + if self.use_robotis_head: # assign relevant head pan status to variables head_pan_status = robot_status['head']['head_pan'] if self.backlash_state['head_pan_looked_left']: - pan_backlash_correction = self.head_pan_calibrated_looked_left_offset_rad + pan_backlash_correction = self.head_pan_calibrated_looked_left_offset_rad else: pan_backlash_correction = 0.0 - if BACKLASH_DEBUG: + if BACKLASH_DEBUG: print('pan_backlash_correction =', pan_backlash_correction) head_pan_rad = head_pan_status['pos'] + self.head_pan_calibrated_offset_rad + pan_backlash_correction head_pan_vel = head_pan_status['vel'] @@ -190,7 +190,7 @@ class StretchBodyNode: tilt_backlash_correction = self.head_tilt_calibrated_looking_up_offset_rad else: tilt_backlash_correction = 0.0 - if BACKLASH_DEBUG: + if BACKLASH_DEBUG: print('tilt_backlash_correction =', tilt_backlash_correction) head_tilt_rad = head_tilt_status['pos'] + self.head_tilt_calibrated_offset_rad + tilt_backlash_correction head_tilt_vel = head_tilt_status['vel'] @@ -198,7 +198,7 @@ class StretchBodyNode: q = tf_conversions.transformations.quaternion_from_euler(0, 0, theta) - if self.broadcast_odom_tf: + if self.broadcast_odom_tf: # publish odometry via TF t = TransformStamped() t.header.stamp = current_time @@ -270,7 +270,7 @@ class StretchBodyNode: velocities.append(head_tilt_vel) efforts.append(head_tilt_effort) - if self.use_robotis_end_of_arm: + if self.use_robotis_end_of_arm: end_of_arm_joint_names = ['joint_wrist_yaw', 'joint_gripper_finger_left', 'joint_gripper_finger_right'] joint_state.name.extend(end_of_arm_joint_names) @@ -389,7 +389,7 @@ class StretchBodyNode: x = base_status['x'] y = base_status['y'] theta = base_status['theta'] - self.mobile_base_manipulation_origin = {'x':x, 'y':y, 'theta':theta} + self.mobile_base_manipulation_origin = {'x':x, 'y':y, 'theta':theta} self.change_mode('manipulation', code_to_run) def turn_on_position_mode(self): @@ -602,7 +602,7 @@ class StretchBodyNode: self.switch_to_position_mode_service = rospy.Service('/switch_to_position_mode', Trigger, self.position_mode_service_callback) - + self.stop_the_robot_service = rospy.Service('/stop_the_robot', Trigger, self.stop_the_robot_callback) @@ -622,6 +622,10 @@ class StretchBodyNode: rospy.signal_shutdown("stretch_driver shutdown") -if __name__ == '__main__': +def main(): node = StretchBodyNode() node.main() + + +if __name__ == '__main__': + main() \ No newline at end of file