From 6f58a58db8eb11318511ca49f7ba015d2eb1ade7 Mon Sep 17 00:00:00 2001 From: Joseph Coombe Date: Thu, 21 Jun 2018 18:43:07 -0500 Subject: [PATCH 1/2] Initial commit of hebiros_wrapper convenience class --- hebiros_utils/CMakeLists.txt | 201 ++++++++++++++++++ hebiros_utils/package.xml | 77 +++++++ hebiros_utils/setup.py | 11 + hebiros_utils/src/hebiros_utils/__init__.py | 0 .../src/hebiros_utils/hebiros_wrapper.py | 200 +++++++++++++++++ 5 files changed, 489 insertions(+) create mode 100644 hebiros_utils/CMakeLists.txt create mode 100644 hebiros_utils/package.xml create mode 100644 hebiros_utils/setup.py create mode 100644 hebiros_utils/src/hebiros_utils/__init__.py create mode 100644 hebiros_utils/src/hebiros_utils/hebiros_wrapper.py diff --git a/hebiros_utils/CMakeLists.txt b/hebiros_utils/CMakeLists.txt new file mode 100644 index 0000000..0edcef0 --- /dev/null +++ b/hebiros_utils/CMakeLists.txt @@ -0,0 +1,201 @@ +cmake_minimum_required(VERSION 2.8.3) +project(hebiros_utils) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + actionlib + roscpp + rospy + sensor_msgs + hebiros +) + +## 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 run_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 run_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 +# sensor_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 run_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 hebiros_utils +# CATKIN_DEPENDS actionlib roscpp rospy sensor_msgs +# 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}/hebiros_utils.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/hebiros_utils_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_hebiros_utils.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/hebiros_utils/package.xml b/hebiros_utils/package.xml new file mode 100644 index 0000000..2b136b7 --- /dev/null +++ b/hebiros_utils/package.xml @@ -0,0 +1,77 @@ + + + hebiros_utils + 1.0.0 + The hebiros_utils package + + + + + Joseph Coombe + + + + + + MIT + + + + + + + https://www.metamorphsoftware.com/ + + + + + + + Joseph Coombe + + + + + + + + + + + + + + + + + + + + + + + + catkin + actionlib + roscpp + rospy + sensor_msgs + hebiros + actionlib + roscpp + rospy + sensor_msgs + hebiros + actionlib + roscpp + rospy + sensor_msgs + hebiros + + + + + + + + diff --git a/hebiros_utils/setup.py b/hebiros_utils/setup.py new file mode 100644 index 0000000..5be1774 --- /dev/null +++ b/hebiros_utils/setup.py @@ -0,0 +1,11 @@ +#!/usr/bin/env python + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['hebiros_utils'], + package_dir={'': 'src'} +) + +setup(**d) diff --git a/hebiros_utils/src/hebiros_utils/__init__.py b/hebiros_utils/src/hebiros_utils/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/hebiros_utils/src/hebiros_utils/hebiros_wrapper.py b/hebiros_utils/src/hebiros_utils/hebiros_wrapper.py new file mode 100644 index 0000000..36d3675 --- /dev/null +++ b/hebiros_utils/src/hebiros_utils/hebiros_wrapper.py @@ -0,0 +1,200 @@ +""" +# Name: hebiros_wrapper.py +# Company: MetaMorph, Inc. +# Author(s): Joseph Coombe +# Create Date: 04/13/2018 +# Edit Date: 06/20/2018 +# +# Description: +# Convenience class to cut down on boilerplate code when using hebiros package (http://wiki.ros.org/hebiros) +# from Python. +""" +from functools import wraps + +import rospy +from sensor_msgs.msg import JointState +from actionlib import SimpleActionClient +from hebiros.srv import AddGroupFromNamesSrv, AddGroupFromURDFSrv, EntryListSrv, SendCommandWithAcknowledgementSrv, \ + SetCommandLifetimeSrv, SetFeedbackFrequencySrv, SizeSrv +from hebiros.msg import CommandMsg, EntryListMsg, EntryMsg, FeedbackMsg, PidGainsMsg, SettingsMsg, WaypointMsg +from hebiros.msg import TrajectoryAction, TrajectoryGoal + + +class HebirosWrapper(object): + def __init__(self, hebi_group_name, hebi_families=None, hebi_names=None, from_urdf=False, lite_mode=False): + """ + :type hebi_group_name: str + :param hebi_group_name: Unique name for set of HEBI Actuators + + :type hebi_families: list[str] + :param hebi_families: Ordered list of HEBI Actuator Families, e.g.['LeftArm', 'RightArm', 'HEBI'] + + :type hebi_names: list[str] + :param hebi_names: Ordered list of HEBI Actuators Families / Name, e.g.['Hip', 'Knee', 'Ankle'] + + :type from_urdf: bool + :param from_urdf: Create HEBI Group from URDF on ROS Parameter Server (/robot_description) + + :type lite_mode: bool + :param lite_mode: If True, do not create any common Subscribers, Publishers, or Service/Action Clients + """ + + self.hebi_group_name = hebi_group_name + + if not from_urdf: + if not isinstance(hebi_families, list) or not isinstance(hebi_names, list): + raise TypeError("hebi_families and hebi_names must be of type list!") + if len(hebi_families) != 1 and len(hebi_families) != len(hebi_names): + raise ValueError("Invalid number of families and names for group {}".format(hebi_group_name)) + + self.hebi_families = hebi_families + self.hebi_names = hebi_names + + add_group_from_names = rospy.ServiceProxy('/hebiros/add_group_from_names', AddGroupFromNamesSrv) + rospy.loginfo(" Waiting for AddGroupFromNamesSrv at %s ...", '/hebiros/add_group_from_names') + rospy.wait_for_service('/hebiros/add_group_from_names') # block until service server starts + rospy.loginfo(" AddGroupFromNamesSrv AVAILABLE.") + add_group_from_names(hebi_group_name, hebi_names, hebi_families) + + else: # from_urdf + add_group_from_urdf = rospy.ServiceProxy('/hebiros/add_group_from_urdf', AddGroupFromURDFSrv) + rospy.loginfo(" Waiting for AddGroupFromURDFSrv at %s ...", '/hebiros/add_group_from_urdf') + rospy.wait_for_service('/hebiros/add_group_from_urdf') # block until service server starts + rospy.loginfo(" AddGroupFromURDFSrv AVAILABLE.") + add_group_from_urdf(hebi_group_name) + + entry_list = rospy.ServiceProxy('/hebiros/entry_list', EntryListSrv) + rospy.wait_for_service('/hebiros/entry_list') # block until service server starts + srv_reponse = entry_list() + self.hebi_families = [entry.family for entry in srv_reponse.entry_list.entries] + self.hebi_names = [entry.name for entry in srv_reponse.entry_list.entries] + + self.hebi_mapping = [family + "/" + name for family, name in zip(self.hebi_families, self.hebi_names)] + self.hebi_count = len(self.hebi_mapping) + + # Topics + self.feedback_topic = '/hebiros/'+hebi_group_name+'/feedback' + rospy.loginfo(" feedback_topic: %s", self.feedback_topic) + self.feedback_joint_state_topic = '/hebiros/'+hebi_group_name+'/feedback/joint_state' + rospy.loginfo(" feedback_joint_state_topic: %s", self.feedback_joint_state_topic) + self.command_topic = '/hebiros/'+hebi_group_name+'/command' + rospy.loginfo(" command_topic: %s", self.command_topic) + self.command_joint_state_topic = '/hebiros/'+hebi_group_name+'/command/joint_state' + rospy.loginfo(" command_joint_state_topic: %s", self.command_joint_state_topic) + + # Services + self.entry_list_srv = '/hebiros/entry_list' + rospy.loginfo(" entry_list_srv: %s", self.entry_list_srv) + self.send_command_with_acknowledgement_srv = '/hebiros/'+hebi_group_name+'/send_command_with_acknowledgement' + rospy.loginfo(" send_command_with_acknowledgement_srv: %s", self.send_command_with_acknowledgement_srv) + self.set_command_lifetime_srv = '/hebiros/'+hebi_group_name+'/set_command_lifetime' + rospy.loginfo(" set_command_lifetime_srv: %s", self.set_command_lifetime_srv) + self.set_feedback_frequency_srv = '/hebiros/'+hebi_group_name+'/set_feedback_frequency_srv' + rospy.loginfo(" set_feedback_frequency_srv: %s", self.set_feedback_frequency_srv) + self.size_srv = '/hebiros/'+hebi_group_name+'/size' + rospy.loginfo(" size_srv: %s", self.size_srv) + + # Actions + self.trajectory_action = '/hebiros/'+hebi_group_name+'/trajectory' + rospy.loginfo(" trajectory_action: %s", self.trajectory_action) + + self._lite_mode = lite_mode + + if not lite_mode: + # Service Clients + self.entry_list = rospy.ServiceProxy(self.entry_list_srv, EntryListSrv) + rospy.loginfo(" Waiting for EntryListSrv at %s ...", self.entry_list) + rospy.wait_for_service(self.entry_list_srv) # block until service server starts + rospy.loginfo(" EntryListSrv AVAILABLE.") + + self.send_command_with_acknowledgement = rospy.ServiceProxy(self.send_command_with_acknowledgement_srv, + SendCommandWithAcknowledgementSrv) + rospy.loginfo(" Waiting for SendCommandWithAcknowledgementSrv at %s ...", + self.send_command_with_acknowledgement) + rospy.wait_for_service(self.send_command_with_acknowledgement_srv) # block until service server starts + rospy.loginfo(" SendCommandWithAcknowledgementSrv AVAILABLE.") + + self.set_command_lifetime = rospy.ServiceProxy(self.set_command_lifetime_srv, SetCommandLifetimeSrv) + rospy.loginfo(" Waiting for SetCommandLifetimeSrv at %s ...", self.set_command_lifetime) + rospy.wait_for_service(self.set_command_lifetime_srv) # block until service server starts + rospy.loginfo(" SetCommandLifetimeSrv AVAILABLE.") + + self.set_feedback_frequency = rospy.ServiceProxy(self.set_feedback_frequency_srv, SetFeedbackFrequencySrv) + rospy.loginfo(" Waiting for SetFeedbackFrequencySrv at %s ...", self.set_feedback_frequency) + rospy.wait_for_service(self.set_command_lifetime_srv) # block until service server starts + rospy.loginfo(" SetFeedbackFrequencySrv AVAILABLE.") + + self.size = rospy.ServiceProxy(self.size_srv, SizeSrv) + rospy.loginfo(" Waiting for SizeSrv at %s ...", self.size) + rospy.wait_for_service(self.size_srv) # block until service server starts + rospy.loginfo(" SizeSrv AVAILABLE.") + + # Simple Action Client + self.trajectory_action_client = SimpleActionClient(self.trajectory_action, TrajectoryAction) + rospy.loginfo(" Waiting for TrajectoryActionServer at %s ...", self.trajectory_action) + self.trajectory_action_client.wait_for_server() # block until action server starts + rospy.loginfo(" TrajectoryActionServer AVAILABLE.") + + # Publishers + self.joint_state_publisher = rospy.Publisher(self.command_joint_state_topic, JointState, queue_size=1) + + # Subscribers + self.joint_state_subscriber = rospy.Subscriber(self.feedback_joint_state_topic, JointState, + self._feedback_joint_state_cb) + self.joint_state_feedback = JointState() + + # Additional callback functions + self._additional_feedback_callbacks = [] # NOTE: Careful. Should be short & sweet (i.e. non-blocking). + + @classmethod + def from_names(cls, hebi_group_name, hebi_families=None, hebi_names=None, lite_mode=False): + return cls(hebi_group_name, hebi_families, hebi_names, lite_mode=lite_mode) + + @classmethod + def from_urdf(cls, hebi_group_name, lite_mode=False): + return cls(hebi_group_name, lite_mode=lite_mode) + + def _feedback_joint_state_cb(self, msg): + assert isinstance(msg, JointState) + self.joint_state_feedback = msg + # Execute additional callbacks + for cb in self._additional_feedback_callbacks: + cb(msg) + + def lite_mode(self): + return self._lite_mode + + def _not_available_in_lite_mode(method): + @wraps(method) + def wrapper(self, *args, **kwargs): + if self.lite_mode(): + rospy.logwarn("Not available in Lite mode...") + return None + else: + return method(self, *args, **kwargs) + + return wrapper + + @_not_available_in_lite_mode + def add_feedback_callback(self, cb): + self._additional_feedback_callbacks.append(cb) + + @_not_available_in_lite_mode + def remove_feedback_callback(self, cb): + self._additional_feedback_callbacks.remove(cb) + + @_not_available_in_lite_mode + def get_joint_state_feedback(self): + return self.joint_state_feedback + + @_not_available_in_lite_mode + def get_joint_positions(self): + return self.get_joint_state_feedback().position + + @_not_available_in_lite_mode + def get_joint_velocities(self): + return self.get_joint_state_feedback().velocity + + @_not_available_in_lite_mode + def get_joint_efforts(self): + return self.get_joint_state_feedback().effort From 8702465ef53c1a55c93de6b0a6237cc0d820ff9c Mon Sep 17 00:00:00 2001 From: Joseph Coombe Date: Fri, 22 Jun 2018 08:19:03 -0500 Subject: [PATCH 2/2] Fix from_urdf classmethod in hebiros_wrapper --- hebiros_utils/src/hebiros_utils/hebiros_wrapper.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hebiros_utils/src/hebiros_utils/hebiros_wrapper.py b/hebiros_utils/src/hebiros_utils/hebiros_wrapper.py index 36d3675..7630a83 100644 --- a/hebiros_utils/src/hebiros_utils/hebiros_wrapper.py +++ b/hebiros_utils/src/hebiros_utils/hebiros_wrapper.py @@ -152,7 +152,7 @@ def from_names(cls, hebi_group_name, hebi_families=None, hebi_names=None, lite_m @classmethod def from_urdf(cls, hebi_group_name, lite_mode=False): - return cls(hebi_group_name, lite_mode=lite_mode) + return cls(hebi_group_name, from_urdf=True, lite_mode=lite_mode) def _feedback_joint_state_cb(self, msg): assert isinstance(msg, JointState)