Ejemplo n.º 1
0
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()
Ejemplo n.º 2
0
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()
Ejemplo n.º 3
0
    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)
Ejemplo n.º 4
0
    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)
Ejemplo n.º 5
0
    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)