Browse Source

Initial stretch_driver for ROS2 (#2)

feature/ros2_diffdrive
David V. Lu!! 3 years ago
committed by GitHub
parent
commit
e8cf6b495b
No known key found for this signature in database GPG Key ID: 4AEE18F83AFDEB23
10 changed files with 174 additions and 355 deletions
  1. +0
    -197
      hello_helpers/CMakeLists.txt
  2. +2
    -6
      hello_helpers/package.xml
  3. +0
    -0
      hello_helpers/resource/hello_helpers
  4. +18
    -6
      hello_helpers/setup.py
  5. +7
    -10
      hello_helpers/src/hello_helpers/hello_misc.py
  6. +3
    -0
      stretch_core/package.xml
  7. +28
    -32
      stretch_core/stretch_core/joint_trajectory_server.py
  8. +111
    -99
      stretch_core/stretch_core/stretch_driver.py
  9. +3
    -3
      stretch_funmap/nodes/funmap
  10. +2
    -2
      stretch_funmap/src/stretch_funmap/navigate.py

+ 0
- 197
hello_helpers/CMakeLists.txt View File

@ -1,197 +0,0 @@
cmake_minimum_required(VERSION 2.8.3)
project(hello_helpers)
## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
rospy
)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
# )
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# std_msgs # Or other packages containing msgs
# )
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES hello_helpers
# CATKIN_DEPENDS rospy
# DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include
${catkin_INCLUDE_DIRS}
)
## Declare a C++ library
# add_library(${PROJECT_NAME}
# src/${PROJECT_NAME}/hello_helpers.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/hello_helpers_node.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node
# ${catkin_LIBRARIES}
# )
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables and/or libraries for installation
# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_hello_helpers.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)

+ 2
- 6
hello_helpers/package.xml View File

@ -46,15 +46,11 @@
<!-- <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>rospy</build_depend>
<build_export_depend>rospy</build_export_depend>
<exec_depend>rospy</exec_depend>
<depend>rclpy</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>

hello_helpers/COLCON_IGNORE → hello_helpers/resource/hello_helpers View File


+ 18
- 6
hello_helpers/setup.py View File

@ -1,8 +1,20 @@
#!/usr/bin/env python
from setuptools import setup
from distutils.core import setup
from catkin_pkg.python_setup import generate_distutils_setup
package_name = 'hello_helpers'
setup_args = generate_distutils_setup( packages=['hello_helpers'], package_dir={'': 'src'})
setup(**setup_args)
setup(
name=package_name,
version='0.2.0',
package_dir={'': 'src'},
packages=[package_name],
data_files=[
('share/ament_index/resource_index/packages', ['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
],
install_requires=['setuptools'],
url='',
license='Apache License 2.0',
author='Hello Robot Inc.',
author_email='support@hello-robot.com',
description='The hello_helpers package',
)

+ 7
- 10
hello_helpers/src/hello_helpers/hello_misc.py View File

@ -7,19 +7,17 @@ import os
import glob
import math
import rospy
import rclpy
import tf2_ros
import ros_numpy
# import ros_numpy TODO(dlu): Fix https://github.com/eric-wieser/ros_numpy/issues/20
import numpy as np
import cv2
import actionlib
from control_msgs.msg import FollowJointTrajectoryAction
from control_msgs.msg import FollowJointTrajectoryGoal
from control_msgs.action import FollowJointTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint
import tf2_ros
from sensor_msgs.msg import PointCloud2
from std_srvs.srv import Trigger, TriggerRequest
from std_srvs.srv import Trigger
#######################
@ -75,11 +73,10 @@ class HelloNode:
def point_cloud_callback(self, point_cloud):
self.point_cloud = point_cloud
def move_to_pose(self, pose, async=False, custom_contact_thresholds=False):
def move_to_pose(self, pose, wait_for_result=False, custom_contact_thresholds=False):
joint_names = [key for key in pose]
point = JointTrajectoryPoint()
point.time_from_start = rospy.Duration(0.0)
trajectory_goal = FollowJointTrajectoryGoal()
trajectory_goal.goal_time_tolerance = rospy.Time(1.0)
@ -100,7 +97,7 @@ class HelloNode:
trajectory_goal.trajectory.points = [point]
trajectory_goal.trajectory.header.stamp = rospy.Time.now()
self.trajectory_client.send_goal(trajectory_goal)
if not async:
if not wait_for_result:
self.trajectory_client.wait_for_result()
#print('Received the following result:')
#print(self.trajectory_client.get_result())

+ 3
- 0
stretch_core/package.xml View File

@ -50,6 +50,7 @@
<!-- <doc_depend>doxygen</doc_depend> -->
<build_depend>actionlib_msgs</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>hello_helpers</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>control_msgs</build_depend>
<build_depend>trajectory_msgs</build_depend>
@ -58,6 +59,7 @@
<build_depend>tf2</build_depend>
<build_export_depend>actionlib_msgs</build_export_depend>
<build_export_depend>geometry_msgs</build_export_depend>
<build_export_depend>hello_helpers</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>
@ -66,6 +68,7 @@
<build_export_depend>tf2</build_export_depend>
<exec_depend>actionlib_msgs</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>hello_helpers</exec_depend>
<exec_depend>nav_msgs</exec_depend>
<exec_depend>control_msgs</exec_depend>
<exec_depend>trajectory_msgs</exec_depend>

+ 28
- 32
stretch_core/stretch_core/joint_trajectory_server.py View File

@ -1,29 +1,25 @@
#! /usr/bin/env python
from __future__ import print_function
import rospy
import actionlib
from control_msgs.msg import FollowJointTrajectoryAction
from control_msgs.msg import FollowJointTrajectoryFeedback
from control_msgs.msg import FollowJointTrajectoryResult
import rclpy
from rclpy.action import ActionServer
from control_msgs.action import FollowJointTrajectory
from trajectory_msgs.msg import JointTrajectoryPoint
from command_groups import HeadPanCommandGroup, HeadTiltCommandGroup, \
WristYawCommandGroup, GripperCommandGroup, \
TelescopingCommandGroup, LiftCommandGroup, \
MobileBaseCommandGroup
from .command_groups import HeadPanCommandGroup, HeadTiltCommandGroup, \
WristYawCommandGroup, GripperCommandGroup, \
TelescopingCommandGroup, LiftCommandGroup, \
MobileBaseCommandGroup
class JointTrajectoryAction:
def __init__(self, node):
self.node = node
self.server = actionlib.SimpleActionServer('/stretch_controller/follow_joint_trajectory',
FollowJointTrajectoryAction,
execute_cb=self.execute_cb,
auto_start=False)
self.feedback = FollowJointTrajectoryFeedback()
self.result = FollowJointTrajectoryResult()
self.server = ActionServer(self.node, FollowJointTrajectory, '/stretch_controller/follow_joint_trajectory',
self.execute_cb)
self.feedback = FollowJointTrajectory.Feedback()
self.result = FollowJointTrajectory.Result()
r = self.node.robot
head_pan_range_ticks = r.head.motors['head_pan'].params['range_t']
@ -63,8 +59,8 @@ class JointTrajectoryAction:
# For now, ignore goal time and configuration tolerances.
commanded_joint_names = goal.trajectory.joint_names
rospy.loginfo(("{0} joint_traj action: New trajectory received with joint_names = "
"{1}").format(self.node.node_name, commanded_joint_names))
self.node.get_logger().info(("{0} joint_traj action: New trajectory received with joint_names = "
"{1}").format(self.node.node_name, commanded_joint_names))
###################################################
# Decide what to do based on the commanded joints.
@ -98,8 +94,8 @@ class JointTrajectoryAction:
# Try to reach each of the goals in sequence until
# an error is detected or success is achieved.
for pointi, point in enumerate(goal.trajectory.points):
rospy.logdebug(("{0} joint_traj action: "
"target point #{1} = <{2}>").format(self.node.node_name, pointi, point))
self.node.get_logger().debug(("{0} joint_traj action: "
"target point #{1} = <{2}>").format(self.node.node_name, pointi, point))
valid_goals = [c.set_goal(point, self.invalid_goal_callback, self.node.fail_out_of_range_goal,
manipulation_origin=self.node.mobile_base_manipulation_origin)
@ -117,11 +113,11 @@ class JointTrajectoryAction:
self.node.robot.push_command()
goals_reached = [c.goal_reached() for c in command_groups]
update_rate = rospy.Rate(15.0)
goal_start_time = rospy.Time.now()
update_rate = self.node.create_rate(15.0)
goal_start_time = self.node.get_clock().now()
while not all(goals_reached):
if (rospy.Time.now() - goal_start_time) > self.node.default_goal_timeout_duration:
if (self.node.get_clock().now() - goal_start_time) > self.node.default_goal_timeout_duration:
err_str = ("Time to execute the current goal point = <{0}> exceeded the "
"default_goal_timeout = {1}").format(point, self.node.default_goal_timeout_s)
self.goal_tolerance_violated_callback(err_str)
@ -131,10 +127,10 @@ class JointTrajectoryAction:
# Check if a premption request has been received.
with self.node.robot_stop_lock:
if self.node.stop_the_robot or self.server.is_preempt_requested():
rospy.logdebug(("{0} joint_traj action: PREEMPTION REQUESTED, but not stopping "
"current motions to allow smooth interpolation between "
"old and new commands.").format(self.node.node_name))
self.server.set_preempted()
self.node.get_logger().debug(("{0} joint_traj action: PREEMPTION REQUESTED, but not stopping "
"current motions to allow smooth interpolation between "
"old and new commands.").format(self.node.node_name))
self.node.stop_the_robot = False
self.node.robot_mode_rwlock.release_read()
return
@ -151,7 +147,7 @@ class JointTrajectoryAction:
goals_reached = [c.goal_reached() for c in command_groups]
update_rate.sleep()
rospy.logdebug("{0} joint_traj action: Achieved target point.".format(self.node.node_name))
self.node.get_logger().debug("{0} joint_traj action: Achieved target point.".format(self.node.node_name))
self.success_callback("Achieved all target points.")
self.node.robot_mode_rwlock.release_read()
@ -159,21 +155,21 @@ class JointTrajectoryAction:
def invalid_joints_callback(self, err_str):
if self.server.is_active() or self.server.is_preempt_requested():
rospy.logerr("{0} joint_traj action: {1}".format(self.node.node_name, err_str))
self.node.get_logger().error("{0} joint_traj action: {1}".format(self.node.node_name, err_str))
self.result.error_code = self.result.INVALID_JOINTS
self.result.error_string = err_str
self.server.set_aborted(self.result)
def invalid_goal_callback(self, err_str):
if self.server.is_active() or self.server.is_preempt_requested():
rospy.logerr("{0} joint_traj action: {1}".format(self.node.node_name, err_str))
self.node.get_logger().error("{0} joint_traj action: {1}".format(self.node.node_name, err_str))
self.result.error_code = self.result.INVALID_GOAL
self.result.error_string = err_str
self.server.set_aborted(self.result)
def goal_tolerance_violated_callback(self, err_str):
if self.server.is_active() or self.server.is_preempt_requested():
rospy.logerr("{0} joint_traj action: {1}".format(self.node.node_name, err_str))
self.node.get_logger().error("{0} joint_traj action: {1}".format(self.node.node_name, err_str))
self.result.error_code = self.result.GOAL_TOLERANCE_VIOLATED
self.result.error_string = err_str
self.server.set_aborted(self.result)
@ -193,8 +189,8 @@ class JointTrajectoryAction:
error_point.positions.append(clean_named_errors_dict[commanded_joint_name])
actual_point.positions.append(desired_point.positions[i] - clean_named_errors_dict[commanded_joint_name])
rospy.logdebug("{0} joint_traj action: sending feedback".format(self.node.node_name))
self.feedback.header.stamp = rospy.Time.now()
self.node.get_logger().debug("{0} joint_traj action: sending feedback".format(self.node.node_name))
self.feedback.header.stamp = self.node.get_clock().now()
self.feedback.joint_names = commanded_joint_names
self.feedback.desired = desired_point
self.feedback.actual = actual_point
@ -202,7 +198,7 @@ class JointTrajectoryAction:
self.server.publish_feedback(self.feedback)
def success_callback(self, success_str):
rospy.loginfo("{0} joint_traj action: {1}".format(self.node.node_name, success_str))
self.node.get_logger().info("{0} joint_traj action: {1}".format(self.node.node_name, success_str))
self.result.error_code = self.result.SUCCESSFUL
self.result.error_string = success_str
self.server.set_succeeded(self.result)

+ 111
- 99
stretch_core/stretch_core/stretch_driver.py View File

@ -9,40 +9,38 @@ import stretch_body.robot as rb
from stretch_body.hello_utils import ThreadServiceExit
import tf2_ros
import tf_conversions
import pyquaternion
import rospy
import rclpy
from rclpy.duration import Duration
from rclpy.node import Node
from geometry_msgs.msg import Quaternion
from geometry_msgs.msg import Twist
from geometry_msgs.msg import TransformStamped
import actionlib
from control_msgs.msg import FollowJointTrajectoryAction
from control_msgs.msg import FollowJointTrajectoryResult
from std_srvs.srv import Trigger, TriggerResponse
from std_srvs.srv import SetBool, SetBoolResponse
from std_srvs.srv import Trigger
from std_srvs.srv import SetBool
from nav_msgs.msg import Odometry
from sensor_msgs.msg import JointState, Imu, MagneticField
from std_msgs.msg import Header
import hello_helpers.hello_misc as hm
from hello_helpers.gripper_conversion import GripperConversion
from joint_trajectory_server import JointTrajectoryAction
from .joint_trajectory_server import JointTrajectoryAction
GRIPPER_DEBUG = False
BACKLASH_DEBUG = False
class StretchBodyNode:
class StretchBodyNode(Node):
def __init__(self):
super().__init__('stretch_driver')
self.use_robotis_head = True
self.use_robotis_end_of_arm = True
self.default_goal_timeout_s = 10.0
self.default_goal_timeout_duration = rospy.Duration(self.default_goal_timeout_s)
self.default_goal_timeout_duration = Duration(seconds=self.default_goal_timeout_s)
# Initialize calibration offsets
self.head_tilt_calibrated_offset_rad = 0.0
@ -75,11 +73,11 @@ class StretchBodyNode:
self.robot_mode_rwlock.acquire_read()
if self.robot_mode != 'navigation':
error_string = '{0} action server must be in navigation mode to receive a twist on cmd_vel. Current mode = {1}.'.format(self.node_name, self.robot_mode)
rospy.logerr(error_string)
self.get_logger().error(error_string)
return
self.linear_velocity_mps = twist.linear.x
self.angular_velocity_radps = twist.angular.z
self.last_twist_time = rospy.get_time()
self.last_twist_time = self.node.get_clock().now()
self.robot_mode_rwlock.release_read()
def command_mobile_base_velocity_and_publish_state(self):
@ -92,7 +90,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':
time_since_last_twist = rospy.get_time() - self.last_twist_time
time_since_last_twist = self.node.get_clock().now() - self.last_twist_time
if time_since_last_twist < self.timeout:
self.robot.base.set_velocity(self.linear_velocity_mps, self.angular_velocity_radps)
self.robot.push_command()
@ -107,12 +105,12 @@ class StretchBodyNode:
# In the future, consider using time stamps from the robot's
# motor control boards and other boards. These would need to
# be synchronized with the rospy clock.
# be synchronized with the ros clock.
#robot_time = robot_status['timestamp_pc']
#rospy.loginfo('robot_time =', robot_time)
#self.get_logger().info('robot_time =', robot_time)
#current_time = rospy.Time.from_sec(robot_time)
current_time = rospy.Time.now()
current_time = self.get_clock().now()
# obtain odometry
# assign relevant base status to variables
@ -196,7 +194,7 @@ class StretchBodyNode:
head_tilt_vel = head_tilt_status['vel']
head_tilt_effort = head_tilt_status['effort']
q = tf_conversions.transformations.quaternion_from_euler(0, 0, theta)
q = pyquaternion.Quaternion(axis=[0, 0, 1], angle=theta)
if self.broadcast_odom_tf:
# publish odometry via TF
@ -207,10 +205,10 @@ class StretchBodyNode:
t.transform.translation.x = x
t.transform.translation.y = y
t.transform.translation.z = 0.0
t.transform.rotation.x = q[0]
t.transform.rotation.y = q[1]
t.transform.rotation.z = q[2]
t.transform.rotation.w = q[3]
t.transform.rotation.x = q.x
t.transform.rotation.y = q.y
t.transform.rotation.z = q.z
t.transform.rotation.w = q.w
self.tf_broadcaster.sendTransform(t)
# publish odometry via the odom topic
@ -220,10 +218,10 @@ class StretchBodyNode:
odom.child_frame_id = self.base_frame_id
odom.pose.pose.position.x = x
odom.pose.pose.position.y = y
odom.pose.pose.orientation.x = q[0]
odom.pose.pose.orientation.y = q[1]
odom.pose.pose.orientation.z = q[2]
odom.pose.pose.orientation.w = q[3]
odom.pose.pose.orientation.x = q.x
odom.pose.pose.orientation.y = q.y
odom.pose.pose.orientation.z = q.z
odom.pose.pose.orientation.w = q.w
odom.twist.twist.linear.x = x_vel
odom.twist.twist.angular.z = theta_vel
self.odom_pub.publish(odom)
@ -238,21 +236,21 @@ class StretchBodyNode:
joint_state.name = ['wrist_extension', 'joint_lift', 'joint_arm_l3', 'joint_arm_l2', 'joint_arm_l1', 'joint_arm_l0']
# set positions of the telescoping joints
positions = [pos_out/4.0 for i in xrange(4)]
positions = [pos_out/4.0 for i in range(4)]
# set lift position
positions.insert(0, pos_up)
# set wrist_extension position
positions.insert(0, pos_out)
# set velocities of the telescoping joints
velocities = [vel_out/4.0 for i in xrange(4)]
velocities = [vel_out/4.0 for i in range(4)]
# set lift velocity
velocities.insert(0, vel_up)
# set wrist_extension velocity
velocities.insert(0, vel_out)
# set efforts of the telescoping joints
efforts = [eff_out for i in xrange(4)]
efforts = [eff_out for i in range(4)]
# set lift effort
efforts.insert(0, eff_up)
# set wrist_extension effort
@ -356,7 +354,7 @@ class StretchBodyNode:
self.robot_mode_rwlock.acquire_write()
self.robot_mode = new_mode
code_to_run()
rospy.loginfo('{0}: Changed to mode = {1}'.format(self.node_name, self.robot_mode))
self.get_logger().info('{0}: Changed to mode = {1}'.format(self.node_name, self.robot_mode))
self.robot_mode_rwlock.release_write()
# TODO : add a freewheel mode or something comparable for the mobile base?
@ -427,29 +425,29 @@ class StretchBodyNode:
self.robot.end_of_arm.move_by('stretch_gripper', 0.0)
self.robot.push_command()
rospy.loginfo('Received stop_the_robot service call, so commanded all actuators to stop.')
return TriggerResponse(
self.get_logger().info('Received stop_the_robot service call, so commanded all actuators to stop.')
return Trigger.Result(
success=True,
message='Stopped the robot.'
)
def navigation_mode_service_callback(self, request):
self.turn_on_navigation_mode()
return TriggerResponse(
return Trigger.Result(
success=True,
message='Now in navigation mode.'
)
def manipulation_mode_service_callback(self, request):
self.turn_on_manipulation_mode()
return TriggerResponse(
return Trigger.Result(
success=True,
message='Now in manipulation mode.'
)
def position_mode_service_callback(self, request):
self.turn_on_position_mode()
return TriggerResponse(
return Trigger.Result(
success=True,
message='Now in position mode.'
)
@ -475,98 +473,103 @@ class StretchBodyNode:
else:
self.robot.pimu.runstop_event_reset()
return SetBoolResponse(
return SetBool.Result(
success=True,
message='is_runstopped: {0}'.format(request.data)
)
########### MAIN ############
def main(self):
########### ROS Setup #######
def ros_setup(self):
self.node_name = self.get_name()
rospy.init_node('stretch_driver')
self.node_name = rospy.get_name()
self.get_logger().info("For use with S T R E T C H (TM) RESEARCH EDITION from Hello Robot Inc.")
rospy.loginfo("For use with S T R E T C H (TM) RESEARCH EDITION from Hello Robot Inc.")
self.get_logger().info("{0} started".format(self.node_name))
rospy.loginfo("{0} started".format(self.node_name))
self.declare_parameter('mode', 'position')
self.declare_parameter('broadcast_odom_tf', False)
self.declare_parameter('controller_calibration_file', 'not_set')
self.declare_parameter('rate', 15.0)
self.declare_parameter('use_fake_mechaduinos', False)
self.declare_parameter('fail_out_of_range_goal', True)
# self.set_parameters_callback(self.parameter_callback)
mode = rospy.get_param('~mode', "position")
rospy.loginfo('mode = ' + str(mode))
mode = self.get_parameter('mode').value
self.get_logger().info('mode = ' + str(mode))
self.broadcast_odom_tf = rospy.get_param('~broadcast_odom_tf', False)
rospy.loginfo('broadcast_odom_tf = ' + str(self.broadcast_odom_tf))
self.broadcast_odom_tf = self.get_parameter('broadcast_odom_tf').value
self.get_logger().info('broadcast_odom_tf = ' + str(self.broadcast_odom_tf))
if self.broadcast_odom_tf:
self.tf_broadcaster = tf2_ros.TransformBroadcaster()
large_ang = 45.0 * np.pi/180.0
filename = rospy.get_param('~controller_calibration_file')
rospy.loginfo('Loading controller calibration parameters for the head from YAML file named {0}'.format(filename))
filename = self.get_parameter('controller_calibration_file').value
self.get_logger().info('Loading controller calibration parameters for the head from YAML file named {0}'.format(filename))
with open(filename, 'r') as fid:
controller_parameters = yaml.safe_load(fid)
rospy.loginfo('controller parameters loaded = {0}'.format(controller_parameters))
self.get_logger().info('controller parameters loaded = {0}'.format(controller_parameters))
self.head_tilt_calibrated_offset_rad = controller_parameters['tilt_angle_offset']
ang = self.head_tilt_calibrated_offset_rad
if (abs(ang) > large_ang):
rospy.loginfo('WARNING: self.head_tilt_calibrated_offset_rad HAS AN UNUSUALLY LARGE MAGNITUDE')
rospy.loginfo('self.head_tilt_calibrated_offset_rad in degrees = {0}'.format(np.degrees(self.head_tilt_calibrated_offset_rad)))
self.get_logger().info('WARNING: self.head_tilt_calibrated_offset_rad HAS AN UNUSUALLY LARGE MAGNITUDE')
self.get_logger().info('self.head_tilt_calibrated_offset_rad in degrees = {0}'.format(np.degrees(self.head_tilt_calibrated_offset_rad)))
self.head_pan_calibrated_offset_rad = controller_parameters['pan_angle_offset']
ang = self.head_pan_calibrated_offset_rad
if (abs(ang) > large_ang):
rospy.loginfo('WARNING: self.head_pan_calibrated_offset_rad HAS AN UNUSUALLY LARGE MAGNITUDE')
rospy.loginfo('self.head_pan_calibrated_offset_rad in degrees = {0}'.format(np.degrees(self.head_pan_calibrated_offset_rad)))
self.get_logger().info('WARNING: self.head_pan_calibrated_offset_rad HAS AN UNUSUALLY LARGE MAGNITUDE')
self.get_logger().info('self.head_pan_calibrated_offset_rad in degrees = {0}'.format(np.degrees(self.head_pan_calibrated_offset_rad)))
self.head_pan_calibrated_looked_left_offset_rad = controller_parameters['pan_looked_left_offset']
ang = self.head_pan_calibrated_looked_left_offset_rad
if (abs(ang) > large_ang):
rospy.loginfo('WARNING: self.head_pan_calibrated_looked_left_offset_rad HAS AN UNUSUALLY LARGE MAGNITUDE')
rospy.loginfo('self.head_pan_calibrated_looked_left_offset_rad in degrees = {0}'.format(np.degrees(self.head_pan_calibrated_looked_left_offset_rad)))
self.get_logger().info('WARNING: self.head_pan_calibrated_looked_left_offset_rad HAS AN UNUSUALLY LARGE MAGNITUDE')
self.get_logger().info('self.head_pan_calibrated_looked_left_offset_rad in degrees = {0}'.format(np.degrees(self.head_pan_calibrated_looked_left_offset_rad)))
self.head_tilt_backlash_transition_angle_rad = controller_parameters['tilt_angle_backlash_transition']
rospy.loginfo('self.head_tilt_backlash_transition_angle_rad in degrees = {0}'.format(np.degrees(self.head_tilt_backlash_transition_angle_rad)))
self.get_logger().info('self.head_tilt_backlash_transition_angle_rad in degrees = {0}'.format(np.degrees(self.head_tilt_backlash_transition_angle_rad)))
self.head_tilt_calibrated_looking_up_offset_rad = controller_parameters['tilt_looking_up_offset']
ang = self.head_tilt_calibrated_looking_up_offset_rad
if (abs(ang) > large_ang):
rospy.loginfo('WARNING: self.head_tilt_calibrated_looking_up_offset_rad HAS AN UNUSUALLY LARGE MAGNITUDE')
rospy.loginfo('self.head_tilt_calibrated_looking_up_offset_rad in degrees = {0}'.format(np.degrees(self.head_tilt_calibrated_looking_up_offset_rad)))
self.get_logger().info('WARNING: self.head_tilt_calibrated_looking_up_offset_rad HAS AN UNUSUALLY LARGE MAGNITUDE')
self.get_logger().info('self.head_tilt_calibrated_looking_up_offset_rad in degrees = {0}'.format(np.degrees(self.head_tilt_calibrated_looking_up_offset_rad)))
self.wrist_extension_calibrated_retracted_offset_m = controller_parameters['arm_retracted_offset']
m = self.wrist_extension_calibrated_retracted_offset_m
if (abs(m) > 0.05):
rospy.loginfo('WARNING: self.wrist_extension_calibrated_retracted_offset_m HAS AN UNUSUALLY LARGE MAGNITUDE')
rospy.loginfo('self.wrist_extension_calibrated_retracted_offset_m in meters = {0}'.format(self.wrist_extension_calibrated_retracted_offset_m))
self.get_logger().info('WARNING: self.wrist_extension_calibrated_retracted_offset_m HAS AN UNUSUALLY LARGE MAGNITUDE')
self.get_logger().info('self.wrist_extension_calibrated_retracted_offset_m in meters = {0}'.format(self.wrist_extension_calibrated_retracted_offset_m))
self.linear_velocity_mps = 0.0 # m/s ROS SI standard for cmd_vel (REP 103)
self.angular_velocity_radps = 0.0 # rad/s ROS SI standard for cmd_vel (REP 103)
self.max_arm_height = 1.1
self.odom_pub = rospy.Publisher('odom', Odometry, queue_size=1)
self.odom_pub = self.node.create_publisher(Odometry, 'odom')
self.imu_mobile_base_pub = rospy.Publisher('imu_mobile_base', Imu, queue_size=1)
self.magnetometer_mobile_base_pub = rospy.Publisher('magnetometer_mobile_base', MagneticField, queue_size=1)
self.imu_wrist_pub = rospy.Publisher('imu_wrist', Imu, queue_size=1)
self.imu_mobile_base_pub = self.node.create_publisher(Imu, 'imu_mobile_base')
self.magnetometer_mobile_base_pub = self.node.create_publisher(MagneticField, 'magnetometer_mobile_base')
self.imu_wrist_pub = self.node.create_publisher(Imu, 'imu_wrist')
rospy.Subscriber("cmd_vel", Twist, self.set_mobile_base_velocity_callback)
self.node.create_subscription(Twist, "cmd_vel", self.set_mobile_base_velocity_callback)
# ~ symbol gets parameter from private namespace
self.joint_state_rate = rospy.get_param('~rate', 15.0)
self.timeout = rospy.get_param('~timeout', 1.0)
rospy.loginfo("{0} rate = {1} Hz".format(self.node_name, self.joint_state_rate))
rospy.loginfo("{0} timeout = {1} s".format(self.node_name, self.timeout))
self.joint_state_rate = self.get_parameter('rate').value
self.timeout = self.get_parameter('timeout').value
self.get_logger().info("{0} rate = {1} Hz".format(self.node_name, self.joint_state_rate))
self.get_logger().info("{0} timeout = {1} s".format(self.node_name, self.timeout))
self.use_fake_mechaduinos = rospy.get_param('~use_fake_mechaduinos', False)
rospy.loginfo("{0} use_fake_mechaduinos = {1}".format(rospy.get_name(), self.use_fake_mechaduinos))
self.use_fake_mechaduinos = self.get_parameter('use_fake_mechaduinos').value
self.get_logger().info("{0} use_fake_mechaduinos = {1}".format(self.node_name, self.use_fake_mechaduinos))
self.base_frame_id = 'base_link'
rospy.loginfo("{0} base_frame_id = {1}".format(self.node_name, self.base_frame_id))
self.get_logger().info("{0} base_frame_id = {1}".format(self.node_name, self.base_frame_id))
self.odom_frame_id = 'odom'
rospy.loginfo("{0} odom_frame_id = {1}".format(self.node_name, self.odom_frame_id))
self.get_logger().info("{0} odom_frame_id = {1}".format(self.node_name, self.odom_frame_id))
self.robot = rb.Robot()
self.robot.startup()
@ -574,13 +577,13 @@ class StretchBodyNode:
# TODO: check with the actuators to see if calibration is required
#self.calibrate()
self.joint_state_pub = rospy.Publisher('joint_states', JointState, queue_size=1)
self.joint_state_pub = self.node.create_publisher(JointState, 'joint_states')
command_base_velocity_and_publish_joint_state_rate = rospy.Rate(self.joint_state_rate)
self.last_twist_time = rospy.get_time()
self.command_base_velocity_and_publish_joint_state_rate = self.create_rate(self.joint_state_rate)
self.last_twist_time = self.node.get_clock().now()
# start action server for joint trajectories
self.fail_out_of_range_goal = rospy.get_param('~fail_out_of_range_goal', True)
self.fail_out_of_range_goal = self.get_parameter('fail_out_of_range_goal').value
self.joint_trajectory_action = JointTrajectoryAction(self)
self.joint_trajectory_action.server.start()
@ -591,41 +594,50 @@ class StretchBodyNode:
elif mode == "manipulation":
self.turn_on_manipulation_mode()
self.switch_to_manipulation_mode_service = rospy.Service('/switch_to_manipulation_mode',
Trigger,
self.manipulation_mode_service_callback)
self.switch_to_manipulation_mode_service = self.create_service(Trigger,
'/switch_to_manipulation_mode',
self.manipulation_mode_service_callback)
self.switch_to_navigation_mode_service = rospy.Service('/switch_to_navigation_mode',
Trigger,
self.navigation_mode_service_callback)
self.switch_to_navigation_mode_service = self.create_service(Trigger,
'/switch_to_navigation_mode',
self.navigation_mode_service_callback)
self.switch_to_position_mode_service = rospy.Service('/switch_to_position_mode',
Trigger,
self.position_mode_service_callback)
self.switch_to_position_mode_service = self.create_service(Trigger,
'/switch_to_position_mode',
self.position_mode_service_callback)
self.stop_the_robot_service = rospy.Service('/stop_the_robot',
Trigger,
self.stop_the_robot_callback)
self.stop_the_robot_service = self.create_service(Trigger,
'/stop_the_robot',
self.stop_the_robot_callback)
self.runstop_service = rospy.Service('/runstop',
SetBool,
self.runstop_service_callback)
self.runstop_service = self.create_service(SetBool,
'/runstop',
self.runstop_service_callback)
def parameter_callback(self, parameters):
self.get_logger().warn('Dynamic parameters not available yet')
########### MAIN ############
def main(self):
self.ros_setup()
try:
# start loop to command the mobile base velocity, publish
# odometry, and publish joint states
while not rospy.is_shutdown():
while rclpy.ok():
self.command_mobile_base_velocity_and_publish_state()
command_base_velocity_and_publish_joint_state_rate.sleep()
except (rospy.ROSInterruptException, ThreadServiceExit):
self.command_base_velocity_and_publish_joint_state_rate.sleep()
except (KeyboardInterrupt, ThreadServiceExit):
self.robot.stop()
rospy.signal_shutdown("stretch_driver shutdown")
def main():
rclpy.init()
node = StretchBodyNode()
node.main()
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
main()

+ 3
- 3
stretch_funmap/nodes/funmap View File

@ -184,7 +184,7 @@ class ContactDetector():
if position is not None:
new_target = self.get_position() + move_increment
pose = {joint_name : new_target}
move_to_pose(pose, async=True)
move_to_pose(pose, wait_for_result=True)
move_rate.sleep()
if self.is_in_contact():
@ -196,7 +196,7 @@ class ContactDetector():
else:
new_target = self.position() - 0.001 #- 0.002
pose = {joint_name : new_target}
move_to_pose(pose, async=False)
move_to_pose(pose, wait_for_result=False)
rospy.loginfo('backing off after contact: moving away from surface to decrease force')
success = True
message = 'Successfully reached until contact.'
@ -423,7 +423,7 @@ class FunmapNode(hm.HelloNode):
safe_target_m = safe_target_m + 0.03
if safe_target_m > self.wrist_position:
pose = {'wrist_extension' : safe_target_m}
self.move_to_pose(pose, async=False)
self.move_to_pose(pose, wait_for_result=False)
# target depth within the surface
target_depth_m = 0.08

+ 2
- 2
stretch_funmap/src/stretch_funmap/navigate.py View File

@ -269,7 +269,7 @@ class MoveBase():
trigger_request = TriggerRequest()
pose = {'translate_mobile_base': forward_distance_m}
self.node.move_to_pose(pose, async=True)
self.node.move_to_pose(pose, wait_for_result=True)
while (not at_goal) and (not obstacle_detected) and (not unsuccessful_action):
if detect_obstacles:
@ -319,7 +319,7 @@ class MoveBase():
(turn_attempts < max_turn_attempts))):
pose = {'rotate_mobile_base': turn_angle_error_rad}
self.node.move_to_pose(pose, async=True)
self.node.move_to_pose(pose, wait_for_result=True)
at_goal = False
unsuccessful_action = False
while (not at_goal) and (not unsuccessful_action):

Loading…
Cancel
Save