yikestone/ros_arduino_bridge
1
0
Fork 0
mirror of https://github.com/YikeStone/ros_arduino_bridge.git synced 2025-08-05 20:14:07 +05:30
Code Issues Projects Releases Wiki Activity

Fixed sweep_demo.py loop to prevent stalling and added sweep_demo.launch file

Browse Source
This commit is contained in:
Patrick Goebel 2015-12-15 06:46:24 -08:00
parent 4fd35aa2b5
commit 3559a4461a
2 changed files with 17 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
ros_arduino_python/launch/sweep_servo.launch Executable file
View File

@ -0,0 +1,7 @@
<launch>
<arg name="joint" default="head_pan_joint" />
<node pkg="ros_arduino_python" name="$(anon sweep_servo)" type="sweep_servo.py" output="screen" clear_params="true">
<param name="joint" value="$(arg joint)" />
</node>
</launch>

23
ros_arduino_python/nodes/sweep_servo.py
View File

@ -35,7 +35,7 @@ class SweepServo():
self.node_name = "sweep_servo" self.node_name = "sweep_servo"
# Initialize the node # Initialize the node
rospy.init_node(self.node_name) rospy.init_node(self.node_name, anonymous=True)
# Set a shutdown function to clean up when termniating the node # Set a shutdown function to clean up when termniating the node
rospy.on_shutdown(self.shutdown) rospy.on_shutdown(self.shutdown)
@ -44,19 +44,18 @@ class SweepServo():
joint_name = rospy.get_param('~joint', 'head_pan_joint') joint_name = rospy.get_param('~joint', 'head_pan_joint')
if joint_name == '': if joint_name == '':
rospy.logino("Joint name for servo must be specified in launch file.") rospy.logino("Joint name for servo must be specified in parameter file.")
os._exit(1) os._exit(1)
servo_pin = rospy.get_param('/arduino/joints/' + str(joint_name) + '/pin') servo_pin = rospy.get_param('/arduino/joints/' + str(joint_name) + '/pin')
max_angle = radians(rospy.get_param('/arduino/joints/' + str(joint_name) + '/max_angle')) 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')) 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 = max_angle
target_max = 0.9 * max_angle target_min = min_angle
target_min = 0.9 * min_angle
# How fast should we sweep the servo # How fast should we sweep the servo
speed = rospy.get_param('~speed', 60) # degrees per second speed = rospy.get_param('~speed', 1.0) # rad/s
# Time between between sweeps # Time between between sweeps
delay = rospy.get_param('~delay', 2) # seconds delay = rospy.get_param('~delay', 2) # seconds
@ -93,17 +92,15 @@ class SweepServo():
rospy.loginfo('Sweeping servo...') rospy.loginfo('Sweeping servo...')
while not rospy.is_shutdown(): while not rospy.is_shutdown():
joint_pub.publish(target_max)
while abs(self.get_joint_position(joint_name) - target_max) > 0.1: while abs(self.get_joint_position(joint_name) - target_max) > 0.1:
joint_pub.publish(target_max)
rospy.sleep(0.1) rospy.sleep(0.1)
rospy.sleep(delay) rospy.sleep(delay)
joint_pub.publish(target_min) while abs(self.get_joint_position(joint_name) - target_min) > 0.1:
joint_pub.publish(target_min)
while abs(self.get_joint_position(joint_name) - target_min) > 0.1:
rospy.sleep(0.1) rospy.sleep(0.1)
rospy.sleep(delay) rospy.sleep(delay)
@ -125,4 +122,4 @@ if __name__ == '__main__':
except: except:
rospy.loginfo('Unexpected error: ' + str(sys.exc_info()[0])) rospy.loginfo('Unexpected error: ' + str(sys.exc_info()[0]))
raise raise