def __init__(self, move_group, robot_name=""):
		'''
		Constructor
		'''
		super(GetJointsFromSrdfGroup, self).__init__(outcomes=['retrieved', 'param_error'],
												output_keys=['joint_names'])

		self._move_group = move_group
		self._robot_name = robot_name

		# Check existence of SRDF parameter.
		# Anyways, values will only be read during runtime to allow modifications.
		self._srdf_param = None
		if rospy.has_param("/robot_description_semantic"):
			self._srdf_param = rospy.get_param("/robot_description_semantic")
		else:
			Logger.logerr('Unable to get parameter: /robot_description_semantic')

		self._file_error = False
		self._srdf = None 

def _read_unit_offsets(self):
        if not rospy.has_param('~num_of_units'):
            rospy.logwarn("No unit offset parameters found!")
        num_of_units = rospy.get_param('~num_of_units', 0)
        self._unit_offsets = np.zeros((num_of_units, 3))
        self._unit_coefficients = np.zeros((num_of_units, 2))
        for i in xrange(num_of_units):
            unit_params = rospy.get_param('~unit_{}'.format(i))
            x = unit_params['x']
            y = unit_params['y']
            z = unit_params['z']
            self._unit_offsets[i, :] = [x, y, z]
            p0 = unit_params['p0']
            p1 = unit_params['p1']
            self._unit_coefficients[i, :] = [p0, p1]
        rospy.loginfo("Unit offsets: {}".format(self._unit_offsets))
        rospy.loginfo("Unit coefficients: {}".format(self._unit_coefficients)) 

def batterySim():
  battery_level = 100
  result = battery_simResult()
  while not rospy.is_shutdown():
	if rospy.has_param("/MyRobot/BatteryStatus"):
		time.sleep(1)
		param = rospy.get_param("/MyRobot/BatteryStatus")
		if param == 1:
			if battery_level == 100:
				result.battery_status = "Full"
				server.set_succeeded(result)
				break
			else:								
				battery_level += 1
				rospy.loginfo("Charging...currently, %s", battery_level)
				time.sleep(4)	
		elif param == 0:
			battery_level -= 1
			rospy.logwarn("Discharging...currently, %s", battery_level)
			time.sleep(2) 

def __init__(self):

		# The OpenCog API. This is used to send sound localization
		# data to OpenCog.
		self.atomo = AtomicMsgs()

		# Sound localization
		parameter_name = "sound_localization/mapping_matrix"
		if rospy.has_param(parameter_name):
			self.sl_matrix = rospy.get_param(parameter_name)
			rospy.Subscriber("/manyears/source_pose", PoseStamped, \
				self.sound_cb)
			print "Sound localization is enabled"
		else :
			print "Sound localization is disabled"

	# ---------------------------------------------------------------
	# Store the location of the strongest sound-source in the
	# OpenCog space server.  This data arrives at a rate of about
	# 30 Hz, currently, from ManyEars. 

def read_parameter(name, default):
  """
  Get a parameter from the ROS parameter server. If it's not found, a 
  warn is printed.
  @type name: string
  @param name: Parameter name
  @param default: Default value for the parameter. The type should be 
  the same as the one expected for the parameter.
  @return: The restulting parameter
  """
  if not rospy.has_param(name):
    rospy.logwarn('Parameter [%s] not found, using default: %s' % (name, default))
  return rospy.get_param(name, default) 

def is_real_robot():
    if rospy.has_param("/use_sim_time"):
        return not rospy.get_param("/use_sim_time")
    else:
        return False 

def __init__(self, config_name, move_group="", duration=1.0, wait_for_execution=True, action_topic='/execute_kinematic_path', robot_name=""):
        '''
                Constructor
                '''
        super(SrdfStateToMoveitExecute, self).__init__(
            outcomes=['reached', 'request_failed', 'moveit_failed', 'param_error'])

        self._config_name  = config_name
        self._robot_name   = robot_name
        self._move_group   = move_group
        self._duration     = duration
        self._wait_for_execution = wait_for_execution
        self._action_topic = action_topic
        self._client       = ProxyServiceCaller({self._action_topic: ExecuteKnownTrajectory})

        # self._action_topic = action_topic
        # self._client       =  ProxyActionServer({self._action_topic: ExecuteTrajectoryAction})

        self._request_failed = False
        self._moveit_failed  = False
        self._success        = False

        self._srdf_param = None
        if rospy.has_param("/robot_description_semantic"):
            self._srdf_param = rospy.get_param("/robot_description_semantic")
        else:
            Logger.logerr('Unable to get parameter: /robot_description_semantic')

        self._param_error = False
        self._srdf = None
        self._response = None 

def __init__(self, config_name, move_group="", action_topic = '/move_group', robot_name=""):
                '''
                Constructor
                '''
                super(SrdfStateToMoveit, self).__init__(outcomes=['reached', 'planning_failed', 'control_failed', 'param_error'],
                                                        output_keys=['config_name', 'move_group', 'robot_name',  'action_topic', 'joint_values', 'joint_names'])


                self._config_name  = config_name
                self._move_group   = move_group
                self._robot_name   = robot_name
                self._action_topic = action_topic
                self._client       = ProxyActionClient({self._action_topic: MoveGroupAction})

                self._planning_failed = False
                self._control_failed = False
                self._success = False

                self._srdf_param = None
                if rospy.has_param("/robot_description_semantic"):
                        self._srdf_param = rospy.get_param("/robot_description_semantic")
                else:
                        Logger.logerr('Unable to get parameter: /robot_description_semantic')

                self._param_error = False
                self._srdf = None 

def init_config( self ):
        # mandatory configurations to be set
        self.config['frame_rate'] = rospy.get_param('~frame_rate')
        self.config['source'] = rospy.get_param('~source')
        self.config['resolution'] = rospy.get_param('~resolution')
        self.config['color_space'] = rospy.get_param('~color_space')

        # optional for camera frames
        # top camera with default
        if rospy.has_param('~camera_top_frame'):
            self.config['camera_top_frame'] = rospy.get_param('~camera_top_frame')
        else:
            self.config['camera_top_frame'] = "/CameraTop_optical_frame"
        # bottom camera with default
        if rospy.has_param('~camera_bottom_frame'):
            self.config['camera_bottom_frame'] = rospy.get_param('~camera_bottom_frame')
        else:
            self.config['camera_bottom_frame'] = "/CameraBottom_optical_frame"
        # depth camera with default
        if rospy.has_param('~camera_depth_frame'):
            self.config['camera_depth_frame'] = rospy.get_param('~camera_depth_frame')
        else:
            self.config['camera_depth_frame'] = "/CameraDepth_optical_frame"

        #load calibration files
        if rospy.has_param('~calibration_file_top'):
            self.config['calibration_file_top'] = rospy.get_param('~calibration_file_top')
        else:
            rospy.loginfo('no camera calibration for top camera found')

        if rospy.has_param('~calibration_file_bottom'):
            self.config['calibration_file_bottom'] = rospy.get_param('~calibration_file_bottom')
        else:
            rospy.loginfo('no camera calibration for bottom camera found')

        # set time reference
        if rospy.has_param('~use_ros_time'):
            self.config['use_ros_time'] = rospy.get_param('~use_ros_time')
        else:
            self.config['use_ros_time'] = False 

def fetch_param(name, default):
  if rospy.has_param(name):
    return rospy.get_param(name)
  else:
    print "parameter [%s] not defined. Defaulting to %.3f" % (name, default)
    return default 

def __init__(self, dxl_io, controller_namespace, port_namespace):
        JointController.__init__(self, dxl_io, controller_namespace, port_namespace)
        
        self.motor_id = rospy.get_param(self.controller_namespace + '/motor/id')
        self.initial_position_raw = rospy.get_param(self.controller_namespace + '/motor/init')
        self.min_angle_raw = rospy.get_param(self.controller_namespace + '/motor/min')
        self.max_angle_raw = rospy.get_param(self.controller_namespace + '/motor/max')
        if rospy.has_param(self.controller_namespace + '/motor/acceleration'):
            self.acceleration = rospy.get_param(self.controller_namespace + '/motor/acceleration')
        else:
            self.acceleration = None
        
        self.flipped = self.min_angle_raw > self.max_angle_raw
        
        self.joint_state = JointState(name=self.joint_name, motor_ids=[self.motor_id]) 

def get_param(name, value=None):
    private = "~%s" % name
    if rospy.has_param(private):
        return rospy.get_param(private)
    elif rospy.has_param(name):
        return rospy.get_param(name)
    else:
        return value 

def __init__(self):		

		# Load Global Trajectory
		if rospy.has_param('mat_waypoints'):
			mat_name = rospy.get_param('mat_waypoints')
		else:
			raise ValueError("No Matfile of waypoints were provided!")

		if not (rospy.has_param('lat0') and rospy.has_param('lon0') and rospy.has_param('yaw0')):
			raise ValueError('Invalid rosparam global origin provided!')

		lat0 = rospy.get_param('lat0')
		lon0 = rospy.get_param('lon0')
		yaw0 = rospy.get_param('yaw0')

		grt = r.GPSRefTrajectory(mat_filename=mat_name, LAT0=lat0, LON0=lon0, YAW0=yaw0) # only 1 should be valid.

		# Set up Data
		self.x_global_traj = grt.get_Xs()
		self.y_global_traj = grt.get_Ys()
		self.x_ref_traj = self.x_global_traj[0]; self.y_ref_traj = self.y_global_traj[0]
		self.x_mpc_traj = self.x_global_traj[0]; self.y_mpc_traj = self.y_global_traj[0]
		self.x_vehicle = self.x_global_traj[0];  self.y_vehicle = self.y_global_traj[0]; self.psi_vehicle = 0.0


		# Set up Plot: includes full ("global") trajectory, target trajectory, MPC prediction trajectory, and vehicle position.
		self.f = plt.figure()		
		plt.ion()
		l1, = plt.plot(self.x_global_traj, self.y_global_traj, 'k') 			
		l2, = plt.plot(self.x_ref_traj,    self.y_ref_traj, 'rx')	
		l3, = plt.plot(self.x_vehicle, self.y_vehicle, 'bo')	
		l4, = plt.plot(self.x_mpc_traj, self.y_mpc_traj, 'g*')

		plt.xlabel('X (m)'); plt.ylabel('Y (m)')
		self.l_arr = [l1,l2,l3,l4]
		plt.axis('equal')

		rospy.init_node('vehicle_plotter', anonymous=True)
		rospy.Subscriber('state_est', state_est, self.update_state, queue_size=1)
		rospy.Subscriber('mpc_path', mpc_path, self.update_mpc_trajectory, queue_size=1)
		self.loop() 

def pub_loop():
	rospy.init_node('state_publisher', anonymous=True)	
	rospy.Subscriber('/gps/fix', NavSatFix, parse_gps_fix, queue_size=1)
	rospy.Subscriber('/gps/vel', TwistWithCovarianceStamped, parse_gps_vel, queue_size=1)
	rospy.Subscriber('/imu/data', Imu, parse_imu_data, queue_size=1)
	rospy.Subscriber('/vehicle/steering', SteeringReport, parse_steering_angle, queue_size=1)

	if not (rospy.has_param('lat0') and rospy.has_param('lon0') and rospy.has_param('yaw0')):
		raise ValueError('Invalid rosparam global origin provided!')

	if not rospy.has_param('time_check_on'):
		raise ValueError('Did not specify if time validity should be checked!')

	LAT0 = rospy.get_param('lat0')
	LON0 = rospy.get_param('lon0')
	YAW0 = rospy.get_param('yaw0')
	time_check_on = rospy.get_param('time_check_on')
	
	state_pub = rospy.Publisher('state_est', state_est, queue_size=1)

	r = rospy.Rate(100)
	while not rospy.is_shutdown():		
		
		if None in (lat, lon, psi, vel, acc_filt, df): 
			r.sleep() # If the vehicle state info has not been received.
			continue

		curr_state = state_est()
		curr_state.header.stamp = rospy.Time.now()
		
		# TODO: time validity check, only publish if data is fresh
		#if time_check_on and not time_valid(curr_state.header.stamp,[tm_vel, tm_df, tm_imu, tm_gps]):
		#	r.sleep()
		#	continue

		curr_state.lat = lat
		curr_state.lon = lon

		X,Y = latlon_to_XY(LAT0, LON0, lat, lon)

		curr_state.x   = X
		curr_state.y   = Y
		curr_state.psi = psi
		curr_state.v   = vel
		
		curr_state.a   = acc_filt
		curr_state.df  = df

		state_pub.publish(curr_state)
		r.sleep()