diff --git a/ros_arduino_python/config/servo_example_params.yaml b/ros_arduino_python/config/servo_example_params.yaml new file mode 100644 index 0000000..18596c1 --- /dev/null +++ b/ros_arduino_python/config/servo_example_params.yaml @@ -0,0 +1,31 @@ +# A UDEV rule maps the Arduino serial number to /dev/arduino + +port: /dev/arduino +baud: 57600 +timeout: 0.1 + +rate: 50 +sensorstate_rate: 10 + +use_base_controller: False + +joint_update_rate: 10 + +# === Sensor definitions. Examples only - edit for your robot. +# Sensor type can be one of the follow (case sensitive!): +# * Analog +# * Digital +# * Ping +# * GP2D12 +# * PololuMotorCurrent +# * PhidgetsVoltage +# * PhidgetsCurrent (20 Amp, DC) + +sensors: { + onboard_led: {pin: 13, type: Digital, direction: output, rate: 1} +} + +# Joint name and pin assignment is an example only +joints: { + head_pan_joint: {pin: 3, init_position: 30, init_speed: 90, neutral: 90, min_angle: -90, max_angle: 90, invert: False, continous: False} +} diff --git a/ros_arduino_python/launch/servo_example.launch b/ros_arduino_python/launch/servo_example.launch new file mode 100755 index 0000000..d0097bb --- /dev/null +++ b/ros_arduino_python/launch/servo_example.launch @@ -0,0 +1,5 @@ + + + + + diff --git a/ros_arduino_python/nodes/sweep_servo.py b/ros_arduino_python/nodes/sweep_servo.py new file mode 100755 index 0000000..f061695 --- /dev/null +++ b/ros_arduino_python/nodes/sweep_servo.py @@ -0,0 +1,128 @@ +#!/usr/bin/env python + +''' sweep_servo.py - Version 1.0 2015-12-04 + + Move a servo back and forth between two positions + + Created for the Pi Robot Project: http://www.pirobot.org + Copyright (c) 2015 Patrick Goebel. All rights reserved. + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version.5 + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details at: + + http://www.gnu.org/licenses/gpl.html + +''' + +import rospy +import sys +import os +from std_msgs.msg import Float64 +from ros_arduino_msgs.srv import SetSpeed +from sensor_msgs.msg import JointState +from math import radians + +class SweepServo(): + def __init__(self): + # Set a name for the node + self.node_name = "sweep_servo" + + # Initialize the node + rospy.init_node(self.node_name) + + # Set a shutdown function to clean up when termniating the node + rospy.on_shutdown(self.shutdown) + + # Name of the joint we want to control + joint_name = rospy.get_param('~joint', 'head_pan_joint') + + if joint_name == '': + rospy.logino("Joint name for servo must be specified in launch file.") + os._exit(1) + + servo_pin = rospy.get_param('/arduino/joints/' + str(joint_name) + '/pin') + max_angle = radians(rospy.get_param('/arduino/joints/' + str(joint_name) + '/max_angle')) + min_angle = radians(rospy.get_param('/arduino/joints/' + str(joint_name) + '/min_angle')) + + # Set the sweep limits to 90% of the max limits + target_max = 0.9 * max_angle + target_min = 0.9 * min_angle + + # How fast should we sweep the servo + speed = rospy.get_param('~speed', 60) # degrees per second + + # Time between between sweeps + delay = rospy.get_param('~delay', 2) # seconds + + # Create a publisher for setting the joint position + joint_pub = rospy.Publisher('/' + joint_name + '/command', Float64, queue_size=5) + + self.joint_state = JointState() + + # Subscribe to the /joint_states topic so we know where the servo is positioned + rospy.Subscriber('/joint_states', JointState, self.update_joint_state) + + # Wait for the /joint_state message to come online + rospy.wait_for_message('/joint_states', JointState, 60) + + # Wait until we actually have a message + while self.joint_state == JointState(): + rospy.sleep(0.1) + + # Wait for the set_speed service + rospy.loginfo('Waiting for set_speed services...') + try: + rospy.wait_for_service('/' + joint_name + '/set_speed', 60) + rospy.loginfo('Ready.') + except: + rospy.loginfo('Could not connect to service!') + os._exit(1) + + # Create a proxy for the set speed service + set_speed = rospy.ServiceProxy('/' + joint_name + '/set_speed', SetSpeed) + + # Set the initial servo speed + set_speed(speed) + + rospy.loginfo('Sweeping servo...') + + while not rospy.is_shutdown(): + joint_pub.publish(target_max) + + while abs(self.get_joint_position(joint_name) - target_max) > 0.1: + rospy.sleep(0.1) + + rospy.sleep(delay) + + joint_pub.publish(target_min) + + while abs(self.get_joint_position(joint_name) - target_min) > 0.1: + rospy.sleep(0.1) + + rospy.sleep(delay) + + def update_joint_state(self, msg): + self.joint_state = msg + + def get_joint_position(self, joint_name): + index = self.joint_state.name.index(joint_name) + return self.joint_state.position[index] + + def shutdown(self): + rospy.loginfo('Shutting down ' + str(self.node_name)) + os._exit(0) + +if __name__ == '__main__': + try: + SweepServo() + except: + rospy.loginfo('Unexpected error: ' + str(sys.exc_info()[0])) + raise + \ No newline at end of file