#!/usr/bin/env python # -*- encoding: utf-8 -*- """ This script implements a ROS node, which is used to control cozmo's head angle and lift height via joy-pad. Copyright {2017} {Peter Rudolph} Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License. """ import rospy from sensor_msgs.msg import Joy from std_msgs.msg import Float64 MIN_HEAD_ANGLE = -25.0 MAX_HEAD_ANGLE = 44.5 SUM_HEAD_ANGLE = MAX_HEAD_ANGLE - MIN_HEAD_ANGLE MIN_LIFT_HEIGHT = 32.0 MAX_LIFT_HEIGHT = 92.0 SUM_LIFT_HEIGHT = MAX_LIFT_HEIGHT - MIN_LIFT_HEIGHT class HeadLiftJoy(object): """ The HeadLiftJoy object used to handle the ROS I/O. It subscribes to "joy" topic, checks if head/tilt button is pressed and sends the transformed value to corresponding "head_angle/lift_height" topic. The min/max parameters are taken from cozmo SDK. """ def __init__(self): """ Create HeadLiftJoy object. """ # params self._head_axes = rospy.get_param('~head_axes', 3) self._lift_axes = rospy.get_param('~lift_axes', 3) self._head_button = rospy.get_param('~head_button', 4) self._lift_button = rospy.get_param('~lift_button', 5) # pubs self._head_pub = rospy.Publisher('head_angle', Float64, queue_size=1) self._lift_pub = rospy.Publisher('lift_height', Float64, queue_size=1) # subs self._joy_sub = rospy.Subscriber('joy', Joy, self._joy_cb, queue_size=1) def _joy_cb(self, msg): """ The joy/game-pad message callback. :type msg: Joy :param msg: The incoming joy message. """ if msg.buttons[self._head_button] == 1: angle_deg = ((msg.axes[self._head_axes] + 1.0) / 2.0) * SUM_HEAD_ANGLE + MIN_HEAD_ANGLE rospy.logdebug('Send head angle: {} degrees.'.format(angle_deg)) self._head_pub.publish(data=angle_deg) if msg.buttons[self._lift_button] == 1: lift_mm = abs(msg.axes[self._lift_axes]) * SUM_LIFT_HEIGHT + MIN_LIFT_HEIGHT rospy.logdebug('Send lift height: {} mm.'.format(lift_mm)) self._lift_pub.publish(data=abs(msg.axes[self._lift_axes])) @staticmethod def run(): """ This function just keeps the node alive. """ rospy.spin() if __name__ == '__main__': rospy.init_node('cozmo_head_lift_joy_node') hlj = HeadLiftJoy() hlj.run()