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