Python rospy.init_node() Examples

def start(self, node_name='polly_node', service_name='polly'):
        """The entry point of a ROS service node.

        Details of the service API can be found in Polly.srv.

        :param node_name: name of ROS node
        :param service_name:  name of ROS service
        :return: it doesn't return
        """
        rospy.init_node(node_name)

        service = rospy.Service(service_name, Polly, self._node_request_handler)

        rospy.loginfo('polly running: {}'.format(service.uri))

        rospy.spin() 

def listener():

	global obj
	global pub

	rospy.loginfo("Starting prediction node")

	rospy.init_node('listener', anonymous = True)

	rospy.Subscriber("sensor_read", Int32, read_sensor_data)

	pub = rospy.Publisher('predict', Int32, queue_size=1)


	obj = Classify_Data()

	obj.read_file('pos_readings.csv')

	obj.train()

	rospy.spin() 

def main():
##########################################################################
##########################################################################
    rospy.init_node('virtual_joystick')
    rospy.loginfo('virtual_joystick started')
    global x_min
    global x_max
    global r_min
    global r_max
    
    x_min = rospy.get_param("~x_min", -0.20)
    x_max = rospy.get_param("~x_max", 0.20)
    r_min = rospy.get_param("~r_min", -1.0)
    r_max = rospy.get_param("~r_max", 1.0)
    
    app = QtGui.QApplication(sys.argv)
    ex = MainWindow()
    sys.exit(app.exec_()) 

def talker_ctrl():
    global rate_value
    rospy.init_node('usv_simple_ctrl', anonymous=True)
    rate = rospy.Rate(rate_value) # 10h
    # publishes to thruster and rudder topics
    pub_motor = rospy.Publisher('thruster_command', JointState, queue_size=10)
    pub_rudder = rospy.Publisher('joint_setpoint', JointState, queue_size=10)
    pub_result = rospy.Publisher('move_usv/result', Float64, queue_size=10)
    
    # subscribe to state and targer point topics
    rospy.Subscriber("state", Odometry, get_pose)  # get usv position (add 'gps' position latter)
    rospy.Subscriber("move_usv/goal", Odometry, get_target)  # get target position

    while not rospy.is_shutdown():
        try:  
            pub_motor.publish(thruster_ctrl_msg())
            pub_rudder.publish(rudder_ctrl_msg())
            pub_result.publish(verify_result())
            rate.sleep()
        except rospy.ROSInterruptException:
	    rospy.logerr("ROS Interrupt Exception! Just ignore the exception!")
        except rospy.ROSTimeMovedBackwardsException:
	    rospy.logerr("ROS Time Backwards! Just ignore the exception!") 

def talker_ctrl():
    global rate_value
    # publishes to thruster and rudder topics
    pub_motor = rospy.Publisher('thruster_command', JointState, queue_size=10)
    pub_rudder = rospy.Publisher('joint_setpoint', JointState, queue_size=10)
    rospy.init_node('usv_simple_ctrl', anonymous=True)
    rate = rospy.Rate(rate_value) # 10h
    
    # subscribe to state and targer point topics
    rospy.Subscriber("state", Odometry, get_pose)  # get usv position (add 'gps' position latter)
    rospy.Subscriber("usv_position_setpoint", Odometry, get_target)  # get target position

    while not rospy.is_shutdown():
        pub_motor.publish(thruster_ctrl_msg())
        pub_rudder.publish(rudder_ctrl_msg())
        rate.sleep() 

def navigation():
    pub = rospy.Publisher('usv_position_setpoint', Odometry, queue_size=10) # only create a rostopic, navigation system TODO
    rospy.init_node('navigation_publisher')
    rate = rospy.Rate(60) # 10h

    x = -20.0
    y = -20.0

    msg = Odometry()
   # msg.header = Header()
    msg.header.stamp = rospy.Time.now()
    msg.header.frame_id = "navigation"
    msg.pose.pose.position = Point(x, y, 0.)
 
    while not rospy.is_shutdown():
            pub.publish(msg)
            rate.sleep() 

def talker_ctrl():
    global rate_value
    global result
    # publishes to thruster and rudder topics
    pub_motor = rospy.Publisher('thruster_command', JointState, queue_size=10)
    pub_result = rospy.Publisher('move_usv/result', Float64, queue_size=10)
    rospy.init_node('usv_simple_ctrl', anonymous=True)
    rate = rospy.Rate(rate_value) # 10h
    
    # subscribe to state and targer point topics
    rospy.Subscriber("state", Odometry, get_pose)  # get usv position (add 'gps' position latter)
    rospy.Subscriber("move_usv/goal", Odometry, get_target)  # get target position

    while not rospy.is_shutdown():
        try:    
            pub_motor.publish(thruster_ctrl_msg())
            pub_result.publish(verify_result())
            rate.sleep()
        except rospy.ROSInterruptException:
	    rospy.logerr("ROS Interrupt Exception! Just ignore the exception!")
        except rospy.ROSTimeMovedBackwardsException:
	    rospy.logerr("ROS Time Backwards! Just ignore the exception!") 

def main():
##########################################################################
##########################################################################
    rospy.init_node('virtual_joystick')
    rospy.loginfo('virtual_joystick started')
    global x_min
    global x_max
    global r_min
    global r_max
    
    x_min = rospy.get_param("~x_min", -0.20)
    x_max = rospy.get_param("~x_max", 0.20)
    r_min = rospy.get_param("~r_min", -1.0)
    r_max = rospy.get_param("~r_max", 1.0)
    
    app = QtGui.QApplication(sys.argv)
    ex = MainWindow()
    sys.exit(app.exec_()) 

def talker_ctrl():
    global rate_value
    rospy.init_node('usv_simple_ctrl', anonymous=True)
    rate = rospy.Rate(rate_value) # 10h
    # publishes to thruster and rudder topics
    pub_motor = rospy.Publisher('thruster_command', JointState, queue_size=10)
    pub_rudder = rospy.Publisher('joint_setpoint', JointState, queue_size=10)
    pub_result = rospy.Publisher('move_usv/result', Float64, queue_size=10)
    
    # subscribe to state and targer point topics
    rospy.Subscriber("state", Odometry, get_pose)  # get usv position (add 'gps' position latter)
    rospy.Subscriber("move_usv/goal", Odometry, get_target)  # get target position

    while not rospy.is_shutdown():
        try:  
            pub_motor.publish(thruster_ctrl_msg())
            pub_rudder.publish(rudder_ctrl_msg())
            pub_result.publish(verify_result())
            rate.sleep()
        except rospy.ROSInterruptException:
	    rospy.logerr("ROS Interrupt Exception! Just ignore the exception!")
        except rospy.ROSTimeMovedBackwardsException:
	    rospy.logerr("ROS Time Backwards! Just ignore the exception!") 

def __init__(self):
    ###############################################
        rospy.init_node("wheel_loopback");
        self.nodename = rospy.get_name()
        rospy.loginfo("%s started" % self.nodename)
        
        self.rate = rospy.get_param("~rate", 200)
        self.timeout_secs = rospy.get_param("~timeout_secs", 0.5)
        self.ticks_meter = float(rospy.get_param('~ticks_meter', 50))
        self.velocity_scale = float(rospy.get_param('~velocity_scale', 255))
        self.latest_motor = 0
        
        self.wheel = 0
        
        rospy.Subscriber('motor', Int16, self.motor_callback)
        
        self.pub_wheel = rospy.Publisher('wheel', Int16,queue_size=10)
        
    ############################################### 

def talker_ctrl():
    global rate_value
    global currentHeading
    global windDir 
    global isTacking
    global heeling
    global spHeading

    rospy.init_node('usv_simple_ctrl', anonymous=True)
    rate = rospy.Rate(rate_value) # 10h
    # publishes to thruster and rudder topics
    #pub_sail = rospy.Publisher('angleLimits', Float64, queue_size=10)
    pub_rudder = rospy.Publisher('joint_setpoint', JointState, queue_size=10)
    pub_result = rospy.Publisher('move_usv/result', Float64, queue_size=10)
    pub_heading = rospy.Publisher('currentHeading', Float64, queue_size=10)
    pub_windDir = rospy.Publisher('windDirection', Float64, queue_size=10)
    pub_heeling = rospy.Publisher('heeling', Float64, queue_size=10)
    pub_spHeading = rospy.Publisher('spHeading', Float64, queue_size=10)
    
    # subscribe to state and targer point topics
    rospy.Subscriber("state", Odometry, get_pose)  # get usv position (add 'gps' position latter)
    rospy.Subscriber("move_usv/goal", Odometry, get_target)  # get target position

    while not rospy.is_shutdown():
        try:
            pub_rudder.publish(rudder_ctrl_msg())
            #pub_sail.publish(sail_ctrl())
            pub_result.publish(verify_result())
            pub_heading.publish(currentHeading)
            pub_windDir.publish(windDir)
            pub_heeling.publish(heeling)
            pub_spHeading.publish(spHeading)
            rate.sleep()
        except rospy.ROSInterruptException:
	    rospy.logerr("ROS Interrupt Exception! Just ignore the exception!")
        except rospy.ROSTimeMovedBackwardsException:
	    rospy.logerr("ROS Time Backwards! Just ignore the exception!") 

def __init__(self):
    ###############################################
        rospy.init_node("wheel_loopback");
        self.nodename = rospy.get_name()
        rospy.loginfo("%s started" % self.nodename)
        
        self.rate = rospy.get_param("~rate", 200)
        self.timeout_secs = rospy.get_param("~timeout_secs", 0.5)
        self.ticks_meter = float(rospy.get_param('~ticks_meter', 50))
        self.velocity_scale = float(rospy.get_param('~velocity_scale', 255))
        self.latest_motor = 0
        
        self.wheel = 0
        
        rospy.Subscriber('motor', Int16, self.motor_callback)
        
        self.pub_wheel = rospy.Publisher('wheel', Int16, queue_size=10)
        
    ############################################### 

def main():
##########################################################################
##########################################################################
    rospy.init_node('virtual_joystick')
    rospy.loginfo('virtual_joystick started')
    global x_min
    global x_max
    global r_min
    global r_max
    
    x_min = rospy.get_param("~x_min", -0.20)
    x_max = rospy.get_param("~x_max", 0.20)
    r_min = rospy.get_param("~r_min", -1.0)
    r_max = rospy.get_param("~r_max", 1.0)
    
    app = QtGui.QApplication(sys.argv)
    ex = MainWindow()
    sys.exit(app.exec_()) 

def main():
##########################################################################
##########################################################################
    rospy.init_node('virtual_joystick')
    rospy.loginfo('virtual_joystick started')
    global x_min
    global x_max
    global r_min
    global r_max
    
    x_min = rospy.get_param("~x_min", -0.20)
    x_max = rospy.get_param("~x_max", 0.20)
    r_min = rospy.get_param("~r_min", -1.0)
    r_max = rospy.get_param("~r_max", 1.0)
    
    app = QtGui.QApplication(sys.argv)
    ex = MainWindow()
    sys.exit(app.exec_()) 

def __init__(self):
		rospy.init_node('DeadReckoning')
		
		self._VelocityCommandPublisher = rospy.Publisher("cmd_vel", Twist)
		self._TransformListener = tf.TransformListener()

		# wait for the listener to get the first transform message
		self._TransformListener.waitForTransform("/odom", "/base_link", rospy.Time(), rospy.Duration(4.0)) 

def __init__(self, goalFrameId = '/map'):
		self._GoalFrameId = goalFrameId
		
		# Initializes a rospy node so that the SimpleActionClient can publish and subscribe over ROS.
		rospy.init_node('goalsSequencer')
		
		self._Client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
		# Waits until the action server has started up and started listening for goals.
		self._Client.wait_for_server() 

def __init__(self):
    #############################################################
        rospy.init_node("twist_to_motors")
        nodename = rospy.get_name()
        rospy.loginfo("%s started" % nodename)
    
        self.w = rospy.get_param("~base_width", 0.2)
    
        self.pub_lmotor = rospy.Publisher('lwheel_vtarget', Float32,queue_size=10)
        self.pub_rmotor = rospy.Publisher('rwheel_vtarget', Float32,queue_size=10)
        rospy.Subscriber('cmd_vel_mux/input/teleop', Twist, self.twistCallback)
    
    
        self.rate = rospy.get_param("~rate", 50)
        self.timeout_ticks = rospy.get_param("~timeout_ticks", 2)
        self.left = 0
        self.right = 0
        
    ############################################################# 

def simple_move(x,w):

    rospy.init_node('simple_move')

    #Simple Action Client
    sac = actionlib.SimpleActionClient('move_base', MoveBaseAction )

    #create goal
    goal = MoveBaseGoal()

    #use self?
    #set goal
 
    rospy.loginfo("Set X = "+x)
    rospy.loginfo("Set W = "+w)

    goal.target_pose.pose.position.x = float(x)
    goal.target_pose.pose.orientation.w = float(w)
    goal.target_pose.header.frame_id = 'first_move'
    goal.target_pose.header.stamp = rospy.Time.now()

    #start listner
    rospy.loginfo("Waiting for server")

    sac.wait_for_server()


    rospy.loginfo("Sending Goals")

    #send goal

    sac.send_goal(goal)
    rospy.loginfo("Waiting for server")

    #finish
    sac.wait_for_result()

    #print result
    print sac.get_result() 

def __init__(self, args):
        self.index = 0
        if len(sys.argv) > 1:
            self.index = int(sys.argv[1])
        rospy.init_node('save_img')
        self.bridge = CvBridge()

        while not rospy.is_shutdown():
            raw_input()

            data = rospy.wait_for_message('/camera1/usb_cam/image_raw', Image)

            try:
                cv_image = self.bridge.imgmsg_to_cv2(data, "passthrough")
            except CvBridgeError as e:
                print(e)

            print "Saved to: ", base_path+str(self.index)+".jpg"
            cv2.cvtColor(cv_image, cv2.COLOR_BGR2RGB, cv_image)
            cv2.imwrite(join(base_path, "frame{:06d}.jpg".format(self.index)), cv_image)#*255)
            self.index += 1
        rospy.spin() 

def __init__(self, goalFrameId = '/map'):
		self._GoalFrameId = goalFrameId
		
		# Initializes a rospy node so that the SimpleActionClient can publish and subscribe over ROS.
		rospy.init_node('goalsSequencer')
		
		self._Client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
		# Waits until the action server has started up and started listening for goals.
		self._Client.wait_for_server() 

def __init__(self):
		rospy.init_node('DeadReckoning')
		
		self._VelocityCommandPublisher = rospy.Publisher("cmd_vel", Twist)
		self._TransformListener = tf.TransformListener()

		# wait for the listener to get the first transform message
		self._TransformListener.waitForTransform("/odom", "/base_link", rospy.Time(), rospy.Duration(4.0)) 

def __init__(self, port="/dev/ttyUSB0", baudrate=9600):
		'''
		Initializes the receiver class. 
		port: The serial port to listen to.
		baudrate: Baud rate for the serial communication
		'''

		self._Counter = 0

		rospy.init_node('arduino')

		port = rospy.get_param("~port", "/dev/ttyUSB0")
		baudRate = int(rospy.get_param("~baudRate", 9600))

		rospy.loginfo("Starting with serial port: " + port + ", baud rate: " + str(baudRate))

		# subscriptions
		rospy.Subscriber("cmd_vel", Twist, self._HandleVelocityCommand)
		self._SerialPublisher = rospy.Publisher('serial', String)

		self._OdometryTransformBroadcaster = tf.TransformBroadcaster()
		self._OdometryPublisher = rospy.Publisher("odom", Odometry)

#		self._VoltageLowlimit = rospy.get_param("~batteryStateParams/voltageLowlimit", "12.0")
#		self._VoltageLowLowlimit = rospy.get_param("~batteryStateParams/voltageLowLowlimit", "11.7")

#		self._BatteryStatePublisher = rospy.Publisher("battery", BatteryState)
		
		self._SetDriveGainsService = rospy.Service('setDriveControlGains', SetDriveControlGains, self._HandleSetDriveGains)

		self._State = Arduino.CONTROLLER_RESET_REQUIRED

		self._SerialDataGateway = SerialDataGateway(port, baudRate,  self._HandleReceivedLine) 

def simple_move(x,w):

    rospy.init_node('simple_move')

    #Simple Action Client
    sac = actionlib.SimpleActionClient('move_base', MoveBaseAction )

    #create goal
    goal = MoveBaseGoal()

    #use self?
    #set goal
 
    rospy.loginfo("Set X = "+x)
    rospy.loginfo("Set W = "+w)

    goal.target_pose.pose.position.x = float(x)
    goal.target_pose.pose.orientation.w = float(w)
    goal.target_pose.header.frame_id = 'first_move'
    goal.target_pose.header.stamp = rospy.Time.now()

    #start listner
    rospy.loginfo("Waiting for server")

    sac.wait_for_server()


    rospy.loginfo("Sending Goals")

    #send goal

    sac.send_goal(goal)
    rospy.loginfo("Waiting for server")

    #finish
    sac.wait_for_result()

    #print result
    print sac.get_result() 

def __init__(self):
    #############################################################
        rospy.init_node("twist_to_motors")
        nodename = rospy.get_name()
        rospy.loginfo("%s started" % nodename)
    
        self.w = rospy.get_param("~base_width", 0.2)
    
        self.pub_lmotor = rospy.Publisher('lwheel_vtarget', Float32,queue_size=10)
        self.pub_rmotor = rospy.Publisher('rwheel_vtarget', Float32,queue_size=10)
        rospy.Subscriber('cmd_vel_mux/input/teleop', Twist, self.twistCallback)
    
    
        self.rate = rospy.get_param("~rate", 50)
        self.timeout_ticks = rospy.get_param("~timeout_ticks", 2)
        self.left = 0
        self.right = 0
        
    ############################################################# 

def main():
    rospy.init_node("inverse_kinematics_test")
    pose = get_pose_stamped(0.45, 0.16, 0.21, EXAMPLE_O)
    print(get_joint_angles(pose))

    pose = get_pose_stamped(0.45, 0.16, 0.21, EXAMPLE_O)
    print(get_joint_angles(pose, limb="left")) 

def main():
    """RSDK URDF Fragment Example:
    This example shows a proof of concept for
    adding your URDF fragment to the robot's
    onboard URDF (which is currently in use).
    """
    arg_fmt = argparse.RawDescriptionHelpFormatter
    parser = argparse.ArgumentParser(formatter_class=arg_fmt,
                                     description=main.__doc__)
    required = parser.add_argument_group('required arguments')
    required.add_argument(
        '-f', '--file', metavar='PATH', required=True,
        help='Path to URDF file to send'
    )
    required.add_argument(
        '-l', '--link', required=False, default="right_hand", #parent
        help='URDF Link already to attach fragment to (usually <left/right>_hand)'
    )
    required.add_argument(
        '-j', '--joint', required=False, default="right_gripper_base",
        help='Root joint for fragment (usually <left/right>_gripper_base)'
    )
    parser.add_argument("-d", "--duration", type=lambda t:abs(float(t)),
            default=5.0, help="[in seconds] Duration to publish fragment")
    args = parser.parse_args(rospy.myargv()[1:])

    rospy.init_node('rsdk_configure_urdf', anonymous=True)

    if not os.access(args.file, os.R_OK):
        rospy.logerr("Cannot read file at '%s'" % (args.file,))
        return 1
    send_urdf(args.link, args.joint, args.file, args.duration)
    return 0 

def main():
    rospy.init_node("inverse_kinematics_test")
    pose = get_pose_stamped(0.45, 0.16, 0.21, EXAMPLE_O)
    print(get_joint_angles(pose)) 

def listener():
    rospy.init_node('serial_adapter', anonymous=True)
    # Declare the Subscriber to motors orders
    rospy.Subscriber("arduino/servo", Int16, servoCallback, queue_size=2)
    rospy.Subscriber("arduino/motor", Int8, motorCallback, queue_size=2)
    rospy.spin() 

def main():
    rospy.init_node("inverse_kinematics_test")
    pose = get_pose_stamped(0.45, 0.16, 0.21, EXAMPLE_O)
    print(get_joint_angles(pose))

    pose = get_pose_stamped(0.45, 0.16, 0.21, EXAMPLE_O)
    print(get_joint_angles(pose, limb="left")) 

def main():
  rospy.init_node('scan_to_angle')
  CalibrateRobot()