def to_parameters(self):
        """
        Fetches a calibration from ROS parameters in the namespace of the current node into this object.

        :rtype: None
        """
        calib_dict = self.to_dict()

        root_params = ['eye_on_hand', 'tracking_base_frame']
        if calib_dict['eye_on_hand']:
            root_params.append('robot_effector_frame')
        else:
            root_params.append('robot_base_frame')

        for rp in root_params:
            rospy.set_param(rp, calib_dict[rp])

        transf_params = 'x', 'y', 'z', 'qx', 'qy', 'qz', 'qw'

        for tp in transf_params:
            rospy.set_param('transformation/'+tp, calib_dict['transformation'][tp]) 

def load_robot_description():
    sensors_config_file =   rospy.get_param('/fssim/sensors_config_file')
    car_config_file     =   rospy.get_param('/fssim/car_config_file')
    car_dimensions_file =   rospy.get_param('/fssim/car_structure_file')
    robot_name          =   rospy.get_param('/fssim/robot_name')

    model_filepath = rospy.get_param('/fssim/model_filepath')

    print sensors_config_file
    print car_config_file
    print car_dimensions_file
    print model_filepath

    try:
        command_string = "rosrun xacro xacro --inorder {} robot_name:='{}' sensors_config_file:='{}' car_config_file:='{}' car_dimensions_file:='{}".format(model_filepath, 
            robot_name, sensors_config_file, car_config_file, car_dimensions_file)
        robot_description = subprocess.check_output(command_string, shell=True, stderr=subprocess.STDOUT)   
    except subprocess.CalledProcessError as process_error:
        rospy.logfatal('Failed to run xacro command with error: \n%s', process_error.output)
        sys.exit(1)

    rospy.set_param("/robot_description", robot_description) 

def _HandleSetDriveGains(self, request):
		""" Handle the setting of the drive gains (PID). """
		
		# We persist the new values in the parameter server
		rospy.set_param("~speedController", {'velocityPParam': request.velocityPParam, 'velocityPParam': request.velocityIParam, 'turnPParam': request.turnPParam, 'turnIParam': request.turnIParam})
		
		commandTimeout = self._GetCommandTimeoutForSpeedController()
		speedControllerParams = (request.velocityPParam, request.velocityIParam, request.turnPParam, request.turnIParam, commandTimeout)
		self._WriteSpeedControllerParams(speedControllerParams)
		return SetDriveControlGainsResponse() 

def _HandleSetDriveGains(self, request):
		""" Handle the setting of the drive gains (PID). """
		
		# We persist the new values in the parameter server
		rospy.set_param("~speedController", {'velocityPParam': request.velocityPParam, 'velocityPParam': request.velocityIParam, 'turnPParam': request.turnPParam, 'turnIParam': request.turnIParam})
		
		commandTimeout = self._GetCommandTimeoutForSpeedController()
		speedControllerParams = (request.velocityPParam, request.velocityIParam, request.turnPParam, request.turnIParam, commandTimeout)
		self._WriteSpeedControllerParams(speedControllerParams)
		return SetDriveControlGainsResponse() 

def _HandleSetDriveGains(self, request):
		""" Handle the setting of the drive gains (PID). """
		
		# We persist the new values in the parameter server
		rospy.set_param("~speedController", {'velocityPParam': request.velocityPParam, 'velocityPParam': request.velocityIParam, 'turnPParam': request.turnPParam, 'turnIParam': request.turnIParam})
		
		commandTimeout = self._GetCommandTimeoutForSpeedController()
		speedControllerParams = (request.velocityPParam, request.velocityIParam, request.turnPParam, request.turnIParam, commandTimeout)
		self._WriteSpeedControllerParams(speedControllerParams)
		return SetDriveControlGainsResponse() 

def _HandleSetDriveGains(self, request):
		""" Handle the setting of the drive gains (PID). """
		
		# We persist the new values in the parameter server
		rospy.set_param("~speedController", {'velocityPParam': request.velocityPParam, 'velocityPParam': request.velocityIParam, 'turnPParam': request.turnPParam, 'turnIParam': request.turnIParam})
		
		commandTimeout = self._GetCommandTimeoutForSpeedController()
		speedControllerParams = (request.velocityPParam, request.velocityIParam, request.turnPParam, request.turnIParam, commandTimeout)
		self._WriteSpeedControllerParams(speedControllerParams)
		return SetDriveControlGainsResponse() 

def _HandleSetDriveGains(self, request):
		""" Handle the setting of the drive gains (PID). """
		
		# We persist the new values in the parameter server
		rospy.set_param("~speedController", {'velocityPParam': request.velocityPParam, 'velocityPParam': request.velocityIParam, 'turnPParam': request.turnPParam, 'turnIParam': request.turnIParam})
		
		commandTimeout = self._GetCommandTimeoutForSpeedController()
		speedControllerParams = (request.velocityPParam, request.velocityIParam, request.turnPParam, request.turnIParam, commandTimeout)
		self._WriteSpeedControllerParams(speedControllerParams)
		return SetDriveControlGainsResponse() 

def _HandleSetDriveGains(self, request):
		""" Handle the setting of the drive gains (PID). """
		
		# We persist the new values in the parameter server
		rospy.set_param("~speedController", {'velocityPParam': request.velocityPParam, 'velocityPParam': request.velocityIParam, 'turnPParam': request.turnPParam, 'turnIParam': request.turnIParam})
		
		commandTimeout = self._GetCommandTimeoutForSpeedController()
		speedControllerParams = (request.velocityPParam, request.velocityIParam, request.turnPParam, request.turnIParam, commandTimeout)
		self._WriteSpeedControllerParams(speedControllerParams)
		return SetDriveControlGainsResponse() 

def _HandleSetDriveGains(self, request):
		""" Handle the setting of the drive gains (PID). """
		
		# We persist the new values in the parameter server
		rospy.set_param("~speedController", {'velocityPParam': request.velocityPParam, 'velocityPParam': request.velocityIParam, 'turnPParam': request.turnPParam, 'turnIParam': request.turnIParam})
		
		commandTimeout = self._GetCommandTimeoutForSpeedController()
		speedControllerParams = (request.velocityPParam, request.velocityIParam, request.turnPParam, request.turnIParam, commandTimeout)
		self._WriteSpeedControllerParams(speedControllerParams)
		return SetDriveControlGainsResponse() 

def _HandleSetDriveGains(self, request):
		""" Handle the setting of the drive gains (PID). """
		
		# We persist the new values in the parameter server
		rospy.set_param("~speedController", {'velocityPParam': request.velocityPParam, 'velocityPParam': request.velocityIParam, 'turnPParam': request.turnPParam, 'turnIParam': request.turnIParam})
		
		commandTimeout = self._GetCommandTimeoutForSpeedController()
		speedControllerParams = (request.velocityPParam, request.velocityIParam, request.turnPParam, request.turnIParam, commandTimeout)
		self._WriteSpeedControllerParams(speedControllerParams)
		return SetDriveControlGainsResponse() 

def __init__(self, action_space, observation_space, state_space, sensor_names, dt=None):
        super(Pr2Env, self).__init__(action_space, observation_space, state_space, sensor_names)
        self._dt = 0.2 if dt is None else dt
        self.pr2 = PR2.PR2()
        self.pr2.larm.goto_posture('side')
        self.pr2.rarm.goto_posture('side')
        self.pr2.torso.go_down()
        gains = {'head_pan_joint': {'d': 2.0, 'i': 12.0, 'i_clamp': 0.5, 'p': 50.0},
                 'head_tilt_joint': {'d': 3.0, 'i': 4.0, 'i_clamp': 0.2, 'p': 1000.0}}
        rospy.set_param('/head_traj_controller/gains', gains)
        self.pr2.head.set_pan_tilt(*((self.state_space.low + self.state_space.high) / 2.0))

        self.msg_and_camera_sensor = camera_sensor.MessageAndCameraSensor()
        rospy.sleep(5.0) 

def camera_data(self, data):
      # set values on the parameter server
      rospy.set_param('camera_link', data.header.frame_id)  # kinect2_ir_optical_frame
      rospy.set_param('camera_height', data.height)         # sd height is 424 / qhd height is 540
      rospy.set_param('camera_width', data.width)           # sd width is 512 / qhd width is 960

      # set values for local variables
      self.cam_height = data.height
      self.cam_width = data.width


   # This callback function handles processing Kinect color image, looking for the green ball
   #  on the Crazyflie. 

def init_gazebo(self, st):
        rospy.set_param('gazebo/use_sim_time', True)
        try:
            ssm = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
        except rospy.ServiceException as e:
            print('Service call failed: %s' % (e))
        for k in range(self._n_robots):
            pose0, twist0 = Pose(), Twist()
            pose0.position.x = st.init_pose[k].pose[0]
            pose0.position.y = st.init_pose[k].pose[1]
            x, y, z, w = tf.transformations.quaternion_from_euler(0, 0, st.init_pose[k].pose[2])
            pose0.orientation.x = x
            pose0.orientation.y = y
            pose0.orientation.z = z
            pose0.orientation.w = w
            twist0.linear.x = 0.
            twist0.angular.z = 0.
            mod0 = ModelState('p3dx'+str(k), pose0, twist0, 'world')
            ssm(mod0)
        for l, k in enumerate(st.robobst):
            pose0, twist0 = Pose(), Twist()
            pose0.position.x = st.obstacles[k].pose[0]
            pose0.position.y = st.obstacles[k].pose[1]
            x, y, z, w = tf.transformations.quaternion_from_euler(0, 0, st.obstacles[k].pose[2])
            pose0.orientation.x = x
            pose0.orientation.y = y
            pose0.orientation.z = z
            pose0.orientation.w = w
            twist0.linear.x = 0.
            twist0.angular.z = 0.
            mod0 = ModelState('p3dx_obs'+str(l), pose0, twist0, 'world')
            ssm(mod0) 

def emit(n, v):
    print datetime.datetime.now().strftime('%c'), v
    rospy.set_param(o.param, v) 

def goalFun(goal):
  rate = rospy.Rate(2)
  process = Process(target = batterySim)
  process.start()
  time.sleep(1)
  if goal.charge_state == 0:
	rospy.set_param("/MyRobot/BatteryStatus",goal.charge_state)
  elif goal.charge_state == 1:
	rospy.set_param("/MyRobot/BatteryStatus",goal.charge_state) 

def readCurrentCoords():
	cc = uarm.get_position()
	print 'Current location is x: %2.2fcm, y: %2.2fcm, z: %2.2fcm.' %(float(cc[0]), float(cc[1]), float(cc[2]))

	rospy.set_param('current_x', cc[0])
	rospy.set_param('current_y', float(cc[1]))
	rospy.set_param('current_z', float(cc[2]))

# Read current Angles function 

def readCurrentAngles():
	ra = {}
	ra['s1'] = uarm.get_servo_angle(0)
	ra['s2'] = uarm.get_servo_angle(1)
	ra['s3'] = uarm.get_servo_angle(2)
	ra['s4'] = uarm.get_servo_angle(3)

	print 'Four Servo Angles: %2.2f, %2.2f, %2.2f and %2.2f degrees.' %(ra['s1'], ra['s2'],ra['s3'],ra['s4'])

	rospy.set_param('servo_1',ra['s1'])
	rospy.set_param('servo_2',ra['s2'])
	rospy.set_param('servo_3',ra['s3'])
	rospy.set_param('servo_4',ra['s4'])

# Read stopper function 

def readStopperStatus():
	val = uarm.get_tip_sensor()
	if val == True:
		print 'Stopper is actived'
		rospy.set_param('stopper_status','HIGH')
	else:
		print 'Stopper is not actived'
		rospy.set_param('stopper_status','LOW')


# Write single angle 

def setParam(self, name, value):
        """Changes the value of the given parameter.

        See :meth:`getParam()` docs for overview of the parameter system.

        Args:
            name (str): The parameter's name.
            value (Any): The parameter's value.
        """
        rospy.set_param(self.prefix + "/" + name, value)
        self.updateParamsService([name]) 

def setParams(self, params):
        """Changes the value of several parameters at once.

        See :meth:`getParam()` docs for overview of the parameter system.

        Args:
            params (Dict[str, Any]): Dict of parameter names/values.
        """
        for name, value in params.iteritems():
            rospy.set_param(self.prefix + "/" + name, value)
        self.updateParamsService(params.keys()) 

def setParam(self, name, value):
        """Broadcasted setParam. See Crazyflie.setParam() for details."""
        rospy.set_param("/allcfs/" + name, value)
        self.updateParamsService([name]) 

def __fill_motor_parameters(self, motor_id, model_number):
        """
        Stores some extra information about each motor on the parameter server.
        Some of these paramters are used in joint controller implementation.
        """
        angles = self.dxl_io.get_angle_limits(motor_id)
        voltage = self.dxl_io.get_voltage(motor_id)
        voltages = self.dxl_io.get_voltage_limits(motor_id)
        
        rospy.set_param('dynamixel/%s/%d/model_number' %(self.port_namespace, motor_id), model_number)
        rospy.set_param('dynamixel/%s/%d/model_name' %(self.port_namespace, motor_id), DXL_MODEL_TO_PARAMS[model_number]['name'])
        rospy.set_param('dynamixel/%s/%d/min_angle' %(self.port_namespace, motor_id), angles['min'])
        rospy.set_param('dynamixel/%s/%d/max_angle' %(self.port_namespace, motor_id), angles['max'])
        
        torque_per_volt = DXL_MODEL_TO_PARAMS[model_number]['torque_per_volt']
        rospy.set_param('dynamixel/%s/%d/torque_per_volt' %(self.port_namespace, motor_id), torque_per_volt)
        rospy.set_param('dynamixel/%s/%d/max_torque' %(self.port_namespace, motor_id), torque_per_volt * voltage)
        
        velocity_per_volt = DXL_MODEL_TO_PARAMS[model_number]['velocity_per_volt']
        rpm_per_tick = DXL_MODEL_TO_PARAMS[model_number]['rpm_per_tick']
        rospy.set_param('dynamixel/%s/%d/velocity_per_volt' %(self.port_namespace, motor_id), velocity_per_volt)
        rospy.set_param('dynamixel/%s/%d/max_velocity' %(self.port_namespace, motor_id), velocity_per_volt * voltage)
        rospy.set_param('dynamixel/%s/%d/radians_second_per_encoder_tick' %(self.port_namespace, motor_id), rpm_per_tick * RPM_TO_RADSEC)
        
        encoder_resolution = DXL_MODEL_TO_PARAMS[model_number]['encoder_resolution']
        range_degrees = DXL_MODEL_TO_PARAMS[model_number]['range_degrees']
        range_radians = math.radians(range_degrees)
        rospy.set_param('dynamixel/%s/%d/encoder_resolution' %(self.port_namespace, motor_id), encoder_resolution)
        rospy.set_param('dynamixel/%s/%d/range_degrees' %(self.port_namespace, motor_id), range_degrees)
        rospy.set_param('dynamixel/%s/%d/range_radians' %(self.port_namespace, motor_id), range_radians)
        rospy.set_param('dynamixel/%s/%d/encoder_ticks_per_degree' %(self.port_namespace, motor_id), encoder_resolution / range_degrees)
        rospy.set_param('dynamixel/%s/%d/encoder_ticks_per_radian' %(self.port_namespace, motor_id), encoder_resolution / range_radians)
        rospy.set_param('dynamixel/%s/%d/degrees_per_encoder_tick' %(self.port_namespace, motor_id), range_degrees / encoder_resolution)
        rospy.set_param('dynamixel/%s/%d/radians_per_encoder_tick' %(self.port_namespace, motor_id), range_radians / encoder_resolution)
        
        # keep some parameters around for diagnostics
        self.motor_static_info[motor_id] = {}
        self.motor_static_info[motor_id]['model'] = DXL_MODEL_TO_PARAMS[model_number]['name']
        self.motor_static_info[motor_id]['firmware'] = self.dxl_io.get_firmware_version(motor_id)
        self.motor_static_info[motor_id]['delay'] = self.dxl_io.get_return_delay_time(motor_id)
        self.motor_static_info[motor_id]['min_angle'] = angles['min']
        self.motor_static_info[motor_id]['max_angle'] = angles['max']
        self.motor_static_info[motor_id]['min_voltage'] = voltages['min']
        self.motor_static_info[motor_id]['max_voltage'] = voltages['max'] 

def __find_motors(self):
        rospy.loginfo('%s: Pinging motor IDs %d through %d...' % (self.port_namespace, self.min_motor_id, self.max_motor_id))
        self.motors = []
        self.motor_static_info = {}
        
        for motor_id in range(self.min_motor_id, self.max_motor_id + 1):
            for trial in range(self.num_ping_retries):
                try:
                    result = self.dxl_io.ping(motor_id)
                except Exception as ex:
                    rospy.logerr('Exception thrown while pinging motor %d - %s' % (motor_id, ex))
                    continue
                    
                if result:
                    self.motors.append(motor_id)
                    break
                    
        if not self.motors:
            rospy.logfatal('%s: No motors found.' % self.port_namespace)
            sys.exit(1)
            
        counts = defaultdict(int)
        
        to_delete_if_error = []
        for motor_id in self.motors:
            for trial in range(self.num_ping_retries):
                try:
                    model_number = self.dxl_io.get_model_number(motor_id)
                    self.__fill_motor_parameters(motor_id, model_number)
                except Exception as ex:
                    rospy.logerr('Exception thrown while getting attributes for motor %d - %s' % (motor_id, ex))
                    if trial == self.num_ping_retries - 1: to_delete_if_error.append(motor_id)
                    continue
                    
                counts[model_number] += 1
                break
                
        for motor_id in to_delete_if_error:
            self.motors.remove(motor_id)
            
        rospy.set_param('dynamixel/%s/connected_ids' % self.port_namespace, self.motors)
        
        status_str = '%s: Found %d motors - ' % (self.port_namespace, len(self.motors))
        for model_number,count in counts.items():
            if count:
                model_name = DXL_MODEL_TO_PARAMS[model_number]['name']
                status_str += '%d %s [' % (count, model_name)
                
                for motor_id in self.motors:
                    if self.motor_static_info[motor_id]['model'] == model_name:
                        status_str += '%d, ' % motor_id
                        
                status_str = status_str[:-2] + '], '
                
        rospy.loginfo('%s, initialization complete.' % status_str[:-2])