cccccccc
/
stretch_ros
mirror of https://github.com/hello-robot/stretch_ros.git
1
0
Fork 0
Code Issues 0 Projects 0 Releases 0 Wiki Activity
Browse Source

Make stretch_core a python package + port launch files

feature/ros2_diffdrive
JafarAbdi 3 years ago
parent
462615fa0c
commit
426057a0ce
34 changed files with 491 additions and 582 deletions
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. +0
    -206
      stretch_core/CMakeLists.txt
  2. +0
    -64
      stretch_core/launch/d435i_basic.launch
  3. +57
    -0
      stretch_core/launch/d435i_basic.launch.py
  4. +0
    -16
      stretch_core/launch/d435i_high_resolution.launch
  5. +18
    -0
      stretch_core/launch/d435i_high_resolution.launch.py
  6. +0
    -17
      stretch_core/launch/d435i_low_resolution.launch
  7. +17
    -0
      stretch_core/launch/d435i_low_resolution.launch.py
  8. +0
    -12
      stretch_core/launch/imu_filter.launch
  9. +14
    -0
      stretch_core/launch/imu_filter.launch.py
  10. +0
    -13
      stretch_core/launch/keyboard_teleop.launch
  11. +18
    -0
      stretch_core/launch/keyboard_teleop.launch.py
  12. +0
    -8
      stretch_core/launch/stretch_aruco.launch
  13. +16
    -0
      stretch_core/launch/stretch_aruco.launch.py
  14. +0
    -44
      stretch_core/launch/stretch_driver.launch
  15. +49
    -0
      stretch_core/launch/stretch_driver.launch.py
  16. +0
    -13
      stretch_core/launch/stretch_ekf.launch
  17. +19
    -0
      stretch_core/launch/stretch_ekf.launch.py
  18. +0
    -32
      stretch_core/launch/wheel_odometry_test.launch
  19. +73
    -0
      stretch_core/launch/wheel_odometry_test.launch.py
  20. +3
    -12
      stretch_core/package.xml
  21. +0
    -0
      stretch_core/resource/stretch_core
  22. +4
    -0
      stretch_core/setup.cfg
  23. +34
    -0
      stretch_core/setup.py
  24. +0
    -0
     
  25. +0
    -0
      stretch_core/stretch_core/command_groups.py
  26. +8
    -5
      stretch_core/stretch_core/d435i_accel_correction.py
  27. +14
    -10
      stretch_core/stretch_core/d435i_configure.py
  28. +17
    -13
      stretch_core/stretch_core/d435i_frustum_visualizer.py
  29. +66
    -62
      stretch_core/stretch_core/detect_aruco_markers.py
  30. +0
    -0
      stretch_core/stretch_core/joint_trajectory_server.py
  31. +6
    -5
      stretch_core/stretch_core/keyboard.py
  32. +37
    -33
      stretch_core/stretch_core/keyboard_teleop.py
  33. +0
    -0
      stretch_core/stretch_core/rwlock.py
  34. +21
    -17
      stretch_core/stretch_core/stretch_driver.py

+ 0
- 206
stretch_core/CMakeLists.txt View File

@ -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)

+ 0
- 64
stretch_core/launch/d435i_basic.launch View File

@ -1,64 +0,0 @@
<launch>
<!-- REDUCE RATE AND USE NUC TIME -->
<node name="d435i_accel_correction" pkg="stretch_core" type="d435i_accel_correction" output="screen" />
<!-- REALSENSE D435i -->
<include file="$(find realsense2_camera)/launch/rs_camera.launch">
<!-- "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/
https://github.com/intel-ros/realsense
-->
<arg name="accel_fps" value="63"/>
<arg name="gyro_fps" value="200"/>
<arg name="depth_fps" value="15"/>
<arg name="enable_infra1" value="false"/>
<arg name="enable_infra2" value="false"/>
<arg name="enable_accel" value="true"/>
<arg name="depth_width" value="$(arg depth_width)"/>
<arg name="depth_height" value="$(arg depth_height)"/>
<arg name="color_width" value="$(arg color_width)"/>
<arg name="color_height" value="$(arg color_height)"/>
<arg name="color_fps" value="15"/>
<!-- publish depth streams aligned to other streams -->
<arg name="align_depth" value="true"/>
<!-- publish an RGBD point cloud -->
<arg name="filters" value="pointcloud"/>
<!-- "tf_prefix: By default all frame's ids have the same prefix -
camera_. This allows changing it per camera."
https://github.com/intel-ros/realsense -->
<!-- "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."
https://github.com/intel-ros/realsense -->
<arg name="enable_sync" value="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. -->
<arg name="allow_no_texture_points" value="true"/>
<!-- "initial_reset: On occasions the device was not closed
properly and due to firmware issues needs to reset. If set to
true, the device will reset prior to usage."
https://github.com/intel-ros/realsense -->
<!--<arg name="initial_reset" value="true"/>-->
<!--<arg name="unite_imu_method" value="copy"/> linear_interpolation-->
</include>
</launch>

+ 57
- 0
stretch_core/launch/d435i_basic.launch.py View File

@ -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])

+ 0
- 16
stretch_core/launch/d435i_high_resolution.launch View File

@ -1,16 +0,0 @@
<launch>
<!-- REALSENSE D435i -->
<include file="$(find stretch_core)/launch/d435i_basic.launch" pass_all_args="true">
<!--
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.
-->
<arg name="depth_width" value="1280"/>
<arg name="depth_height" value="720"/>
<arg name="color_width" value="1280"/>
<arg name="color_height" value="720"/>
</include>
</launch>

+ 18
- 0
stretch_core/launch/d435i_high_resolution.launch.py View File

@ -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())])

+ 0
- 17
stretch_core/launch/d435i_low_resolution.launch View File

@ -1,17 +0,0 @@
<launch>
<!-- REALSENSE D435i -->
<include file="$(find stretch_core)/launch/d435i_basic.launch" pass_all_args="true">
<!--
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.
-->
<arg name="depth_width" value="424"/>
<arg name="depth_height" value="240"/>
<arg name="color_width" value="424"/>
<arg name="color_height" value="240"/>
</include>
</launch>

+ 17
- 0
stretch_core/launch/d435i_low_resolution.launch.py View File

@ -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())])

+ 0
- 12
stretch_core/launch/imu_filter.launch View File

@ -1,12 +0,0 @@
<launch>
<!-- imu_filter_madgwick -->
<node name="imu_filter_node" pkg="imu_filter_madgwick" type="imu_filter_node" output="screen">
<remap from="imu/data_raw" to="/imu_mobile_base"/>
<remap from="imu/mag" to="/magnetometer_mobile_base"/>
<param name="use_mag" value="false"/>
<param name="fixed_frame" value="map"/>
</node>
<!-- -->
</launch>

+ 14
- 0
stretch_core/launch/imu_filter.launch.py View File

@ -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'}]
)
])

+ 0
- 13
stretch_core/launch/keyboard_teleop.launch View File

@ -1,13 +0,0 @@
<launch>
<!-- STRETCH DRIVER -->
<param name="/stretch_driver/broadcast_odom_tf" type="bool" value="true"/>
<param name="/stretch_driver/fail_out_of_range_goal" type="bool" value="false"/>
<include file="$(find stretch_core)/launch/stretch_driver.launch" pass_all_args="true"/>
<!-- -->
<!-- KEYBOARD TELEOP -->
<node name="keyboard_teleop" pkg="stretch_core" type="keyboard_teleop" output="screen" args=''/>
<!-- -->
</launch>

+ 18
- 0
stretch_core/launch/keyboard_teleop.launch.py View File

@ -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')])

+ 0
- 8
stretch_core/launch/stretch_aruco.launch View File

@ -1,8 +0,0 @@
<launch>
<!-- ARUCO MARKER DETECTOR -->
<rosparam command="load" file="$(find stretch_core)/config/stretch_marker_dict.yaml" />
<node name="detect_aruco_markers" pkg="stretch_core" type="detect_aruco_markers" output="screen"/>
<!-- -->
</launch>

+ 16
- 0
stretch_core/launch/stretch_aruco.launch.py View File

@ -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'])]
)
])

+ 0
- 44
stretch_core/launch/stretch_driver.launch View File

@ -1,44 +0,0 @@
<launch>
<param name="robot_description" textfile="$(find stretch_description)/urdf/stretch.urdf" />
<arg name="calibrated_controller_yaml_file" value="$(find stretch_core)/config/controller_calibration_head.yaml"/>
<node
name="joint_state_publisher"
pkg="joint_state_publisher"
type="joint_state_publisher" >
<param name="rate" value="15.0"/>
<rosparam param="source_list">
[/stretch/joint_states]
</rosparam>
</node>
<!-- GUI WITH JOINT SLIDERS -->
<!--
<node
name="joint_state_publisher_gui"
pkg="joint_state_publisher_gui"
type="joint_state_publisher_gui" >
<param name="rate" value="15.0"/>
<rosparam param="source_list">
[/stretch/joint_states]
</rosparam>
</node>
-->
<node
name="robot_state_publisher"
pkg="robot_state_publisher"
type="robot_state_publisher" >
<param name="publish_frequency" value="15.0"/>
</node>
<node name="stretch_driver" pkg="stretch_core" type="stretch_driver" output="screen">
<param name="rate" type="double" value="25.0"/>
<param name="timeout" type="double" value="0.5"/>
<remap from="cmd_vel" to="/stretch/cmd_vel" />
<remap from="joint_states" to="/stretch/joint_states" />
<param name="controller_calibration_file" type="string" value="$(arg calibrated_controller_yaml_file)"/>
</node>
</launch>

+ 49
- 0
stretch_core/launch/stretch_driver.launch.py View File

@ -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])

+ 0
- 13
stretch_core/launch/stretch_ekf.launch View File

@ -1,13 +0,0 @@
<launch>
<node pkg="robot_localization" type="ekf_localization_node" name="ekf_se" clear_params="true">
<rosparam command="load" file="$(find stretch_core)/launch/stretch_ekf.yaml" />
<!-- Placeholder for output topic remapping
<remap from="odometry/filtered" to=""/>
<remap from="accel/filtered" to=""/>
-->
</node>
</launch>

+ 19
- 0
stretch_core/launch/stretch_ekf.launch.py View File

@ -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', '')
])
]
)

+ 0
- 32
stretch_core/launch/wheel_odometry_test.launch View File

@ -1,32 +0,0 @@
<launch>
<arg name="urdf_file" value="$(find stretch_description)/urdf/stretch.urdf"/>
<arg name="controller_yaml_file" value="$(find stretch_core)/config/controller_calibration_head.yaml"/>
<arg name="calibration_directory" value="$(env HELLO_FLEET_PATH)/$(env HELLO_FLEET_ID)/calibration_ros/"/>
<!-- REALSENSE D435i -->
<include file="$(find stretch_core)/launch/d435i_low_resolution.launch" />
<node name="d435i_configure" pkg="stretch_core" type="d435i_configure" output="screen" >
<param name="initial_mode" type="string" value="High Accuracy"/>
</node>
<!-- -->
<!-- STRETCH DRIVER -->
<param name="/stretch_driver/broadcast_odom_tf" type="bool" value="true"/>
<param name="/stretch_driver/fail_out_of_range_goal" type="bool" value="false"/>
<include file="$(find stretch_core)/launch/stretch_driver.launch" pass_all_args="true"/>
<!-- -->
<!-- LASER RANGE FINDER -->
<include file="$(find stretch_core)/launch/rplidar.launch" />
<!-- -->
<!-- KEYBOARD TELEOP -->
<node name="keyboard_teleop" pkg="stretch_core" type="keyboard_teleop" output="screen" args=''/>
<!-- -->
<!-- VISUALIZE -->
<node name="rviz" pkg="rviz" type="rviz" output="screen" args="-d $(find stretch_core)/rviz/wheel_odometry_test.rviz" />
<!-- -->
</launch>

+ 73
- 0
stretch_core/launch/wheel_odometry_test.launch.py View File

@ -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])

+ 3
- 12
stretch_core/package.xml View File

@ -48,45 +48,36 @@
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>actionlib</build_depend>
<buildtool_depend>ament_python</buildtool_depend>
<build_depend>actionlib_msgs</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>control_msgs</build_depend>
<build_depend>trajectory_msgs</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>std_srvs</build_depend>
<build_depend>tf</build_depend>
<build_depend>tf2</build_depend>
<build_export_depend>actionlib</build_export_depend>
<build_export_depend>actionlib_msgs</build_export_depend>
<build_export_depend>geometry_msgs</build_export_depend>
<build_export_depend>nav_msgs</build_export_depend>
<build_export_depend>control_msgs</build_export_depend>
<build_export_depend>trajectory_msgs</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>std_srvs</build_export_depend>
<build_export_depend>tf</build_export_depend>
<build_export_depend>tf2</build_export_depend>
<exec_depend>actionlib</exec_depend>
<exec_depend>actionlib_msgs</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>nav_msgs</exec_depend>
<exec_depend>control_msgs</exec_depend>
<exec_depend>trajectory_msgs</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>rclpy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>std_srvs</exec_depend>
<exec_depend>tf</exec_depend>
<exec_depend>tf2</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
<build_type>ament_python</build_type>
</export>
</package>

stretch_core/COLCON_IGNORE → stretch_core/resource/stretch_core View File


+ 4
- 0
stretch_core/setup.cfg View File

@ -0,0 +1,4 @@
[develop]
script-dir=$base/lib/stretch_core
[install]
install-scripts=$base/lib/stretch_core

+ 34
- 0
stretch_core/setup.py View File

@ -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',
],
},
)

+ 0
- 0
View File


stretch_core/nodes/command_groups.py → stretch_core/stretch_core/command_groups.py View File


stretch_core/nodes/d435i_accel_correction → stretch_core/stretch_core/d435i_accel_correction.py View File

@ -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()

stretch_core/nodes/d435i_configure → stretch_core/stretch_core/d435i_configure.py View File

@ -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()

stretch_core/nodes/d435i_frustum_visualizer → stretch_core/stretch_core/d435i_frustum_visualizer.py View File

@ -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()

stretch_core/nodes/detect_aruco_markers → stretch_core/stretch_core/detect_aruco_markers.py View File

@ -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()

stretch_core/nodes/joint_trajectory_server.py → stretch_core/stretch_core/joint_trajectory_server.py View File


stretch_core/nodes/keyboard.py → stretch_core/stretch_core/keyboard.py View File

@ -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()

stretch_core/nodes/keyboard_teleop → stretch_core/stretch_core/keyboard_teleop.py View File

@ -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()

stretch_core/nodes/rwlock.py → stretch_core/stretch_core/rwlock.py View File


stretch_core/nodes/stretch_driver → stretch_core/stretch_core/stretch_driver.py View File

@ -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()

Write Preview
Loading…
Cancel
Save