def makeMsg(name, path, members):
f = open(path+name+"CF.msg", 'w')
rospy.logino(' -> '+path+name+"CF.msg")
f.write("#DO NOT EDIT THIS FILE - IT IS AUTO GENERATED. ALL CONTENTS WILL BE REPLACED\n")
f.write("Header header\n")
for m in sorted(members.keys()):
# type, remove the _t or append 32
t = members[m].ctype[:-2] if members[m].ctype!="float" else members[m].ctype+"32"
file.write("%s %s\n" % (t, members[m].name))
f.close()
def makeMsg(name, path, members):
f = open(path + name + "CF.msg", 'w')
rospy.logino(' -> ' + path + name + "CF.msg")
f.write(
"#DO NOT EDIT THIS FILE - IT IS AUTO GENERATED. ALL CONTENTS WILL BE REPLACED\n"
)
f.write("Header header\n")
for m in sorted(members.keys()):
# type, remove the _t or append 32
t = members[m].ctype[:-2] if members[
m].ctype != "float" else members[m].ctype + "32"
file.write("%s %s\n" % (t, members[m].name))
f.close()
def __init__(self):
# Set a name for the node
self.node_name = "sweep_servo"
# Initialize the node
rospy.init_node(self.node_name, anonymous=True)
# 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 parameter file.")
os._exit(1)
servo_pin = rospy.get_param("/arduino/joints/" + str(joint_name) + "/pin")
max_position = radians(rospy.get_param("/arduino/joints/" + str(joint_name) + "/max_position"))
min_position = radians(rospy.get_param("/arduino/joints/" + str(joint_name) + "/min_position"))
target_max = max_position
target_min = min_position
# How fast should we sweep the servo
speed = rospy.get_param("~speed", 1.0) # rad/s
# 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():
while abs(self.get_joint_position(joint_name) - target_max) > 0.1:
joint_pub.publish(target_max)
rospy.sleep(0.1)
rospy.sleep(delay)
while abs(self.get_joint_position(joint_name) - target_min) > 0.1:
joint_pub.publish(target_min)
rospy.sleep(0.1)
rospy.sleep(delay)
def __init__(self):
# Set a name for the node
self.node_name = "sweep_servo"
# Initialize the node
rospy.init_node(self.node_name, anonymous=True)
# 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 parameter file.")
os._exit(1)
servo_pin = rospy.get_param('/arduino/joints/' + str(joint_name) +
'/pin')
max_position = radians(
rospy.get_param('/arduino/joints/' + str(joint_name) +
'/max_position'))
min_position = radians(
rospy.get_param('/arduino/joints/' + str(joint_name) +
'/min_position'))
target_max = max_position
target_min = min_position
# How fast should we sweep the servo
speed = rospy.get_param('~speed', 1.0) # rad/s
# 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():
while abs(self.get_joint_position(joint_name) - target_max) > 0.1:
joint_pub.publish(target_max)
rospy.sleep(0.1)
rospy.sleep(delay)
while abs(self.get_joint_position(joint_name) - target_min) > 0.1:
joint_pub.publish(target_min)
rospy.sleep(0.1)
rospy.sleep(delay)
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 is None or joint_name == '':
rospy.logino("Joint name for servo must be specified in parameter file.")
os._exit(1)
max_position = radians(rospy.get_param('/arduino/joints/' + str(joint_name) + '/max_position'))
min_position = radians(rospy.get_param('/arduino/joints/' + str(joint_name) + '/min_position'))
target_max = max_position
target_min = min_position
# How fast should we sweep the servo
servo_speed = rospy.get_param('~servo_speed', 1.0) # rad/s
# Time delay between between sweeps
delay = rospy.get_param('~delay', 0) # 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(servo_speed)
rospy.loginfo('Sweeping servo...')
while not rospy.is_shutdown():
while abs(self.get_joint_position(joint_name) - target_max) > 0.1:
joint_pub.publish(target_max)
rospy.sleep(0.1)
rospy.sleep(delay)
while abs(self.get_joint_position(joint_name) - target_min) > 0.1:
joint_pub.publish(target_min)
rospy.sleep(0.1)
rospy.sleep(delay)
