From e8cf6b495bd3a663af4e4d398ab3854eb52c5a7d Mon Sep 17 00:00:00 2001 From: "David V. Lu!!" Date: Thu, 18 Feb 2021 08:24:03 -0500 Subject: [PATCH] Initial stretch_driver for ROS2 (#2) --- hello_helpers/CMakeLists.txt | 197 ---------------- hello_helpers/package.xml | 8 +- .../{COLCON_IGNORE => resource/hello_helpers} | 0 hello_helpers/setup.py | 24 +- hello_helpers/src/hello_helpers/hello_misc.py | 17 +- stretch_core/package.xml | 3 + .../stretch_core/joint_trajectory_server.py | 60 +++-- stretch_core/stretch_core/stretch_driver.py | 210 +++++++++--------- stretch_funmap/nodes/funmap | 6 +- stretch_funmap/src/stretch_funmap/navigate.py | 4 +- 10 files changed, 174 insertions(+), 355 deletions(-) delete mode 100644 hello_helpers/CMakeLists.txt rename hello_helpers/{COLCON_IGNORE => resource/hello_helpers} (100%) diff --git a/hello_helpers/CMakeLists.txt b/hello_helpers/CMakeLists.txt deleted file mode 100644 index 5956985..0000000 --- a/hello_helpers/CMakeLists.txt +++ /dev/null @@ -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) diff --git a/hello_helpers/package.xml b/hello_helpers/package.xml index b06ea28..09f2d2c 100644 --- a/hello_helpers/package.xml +++ b/hello_helpers/package.xml @@ -46,15 +46,11 @@ - catkin - rospy - rospy - rospy - + rclpy - + ament_python diff --git a/hello_helpers/COLCON_IGNORE b/hello_helpers/resource/hello_helpers similarity index 100% rename from hello_helpers/COLCON_IGNORE rename to hello_helpers/resource/hello_helpers diff --git a/hello_helpers/setup.py b/hello_helpers/setup.py index 59c85cf..5934ca3 100644 --- a/hello_helpers/setup.py +++ b/hello_helpers/setup.py @@ -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', +) diff --git a/hello_helpers/src/hello_helpers/hello_misc.py b/hello_helpers/src/hello_helpers/hello_misc.py index bcbc7e0..8fdef62 100644 --- a/hello_helpers/src/hello_helpers/hello_misc.py +++ b/hello_helpers/src/hello_helpers/hello_misc.py @@ -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()) diff --git a/stretch_core/package.xml b/stretch_core/package.xml index ea297f2..ab94179 100644 --- a/stretch_core/package.xml +++ b/stretch_core/package.xml @@ -50,6 +50,7 @@ actionlib_msgs geometry_msgs + hello_helpers nav_msgs control_msgs trajectory_msgs @@ -58,6 +59,7 @@ tf2 actionlib_msgs geometry_msgs + hello_helpers nav_msgs control_msgs trajectory_msgs @@ -66,6 +68,7 @@ tf2 actionlib_msgs geometry_msgs + hello_helpers nav_msgs control_msgs trajectory_msgs diff --git a/stretch_core/stretch_core/joint_trajectory_server.py b/stretch_core/stretch_core/joint_trajectory_server.py index 05661f2..0d60638 100644 --- a/stretch_core/stretch_core/joint_trajectory_server.py +++ b/stretch_core/stretch_core/joint_trajectory_server.py @@ -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) diff --git a/stretch_core/stretch_core/stretch_driver.py b/stretch_core/stretch_core/stretch_driver.py index ce39f22..f690e3d 100755 --- a/stretch_core/stretch_core/stretch_driver.py +++ b/stretch_core/stretch_core/stretch_driver.py @@ -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() \ No newline at end of file + main() diff --git a/stretch_funmap/nodes/funmap b/stretch_funmap/nodes/funmap index 373a879..1faf712 100755 --- a/stretch_funmap/nodes/funmap +++ b/stretch_funmap/nodes/funmap @@ -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 diff --git a/stretch_funmap/src/stretch_funmap/navigate.py b/stretch_funmap/src/stretch_funmap/navigate.py index 1fbaaf4..3b64488 100644 --- a/stretch_funmap/src/stretch_funmap/navigate.py +++ b/stretch_funmap/src/stretch_funmap/navigate.py @@ -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):