def sub_cb(self, msg):
	if (msg.control.speed <> self.last_speed):
	  JC = JointCommand();
	  JC.name = 'power_motor';
	  JC.type = 'speed';
	  JC.speed = -msg.control.speed*100; #convert m/s to deg/s for Lego NXT_steering
	  self.last_speed = msg.control.speed;
	  self.pub.publish(JC)
	if (msg.control.steering_angle <> self.last_steering_angle):
	  JC = JointCommand();
	  JC.name = 'steering_motor';
	  JC.type = 'to_angle';
	  JC.angle = msg.control.steering_angle*360/(2*math.pi)*self.ratioGearSteering;
	  JC.speed = 400;
	  self.last_steering_angle = msg.control.steering_angle;
	  self.pub.publish(JC)	  
 def sub_cb(self, msg):
     if (msg.control.speed <> self.last_speed):
         JC = JointCommand()
         JC.name = 'power_motor'
         JC.type = 'speed'
         JC.speed = -msg.control.speed * 100
         #convert m/s to deg/s for Lego NXT_steering
         self.last_speed = msg.control.speed
         self.pub.publish(JC)
     if (msg.control.steering_angle <> self.last_steering_angle):
         JC = JointCommand()
         JC.name = 'steering_motor'
         JC.type = 'to_angle'
         JC.angle = msg.control.steering_angle * 360 / (
             2 * math.pi) * self.ratioGearSteering
         JC.speed = 400
         self.last_steering_angle = msg.control.steering_angle
         self.pub.publish(JC)
  def sub_cb_js(self, msg):
	if (msg.name[0] == 'radar_motor'):
	    #control radar
	    if ((msg.position[0]*180/math.pi > self.last_angle_cmd - 5 and self.last_angle_cmd>0) or (msg.position[0]*180/math.pi < self.last_angle_cmd + 5 and self.last_angle_cmd<0) or (self.init_radar_motor == False)):
	      rospy.loginfo('Init command radar');
	      if (self.init_radar_motor == True):
		#publish LaserScan message
		self.LS_msg.angle_max = -msg.position[0]/5;
		self.LS_msg.angle_increment = (self.LS_msg.angle_max - self.LS_msg.angle_min)/self.count_ray;
		self.LS_msg.time_increment = 0.01;
		self.pub_LS.publish(self.LS_msg);
	      self.init_radar_motor = True;
	      #new LaserScan message
	      self.count_ray = 0;
	      self.LS_msg = LaserScan();
	      self.LS_msg.header.frame_id = "base_link";
	      self.LS_msg.header.stamp = rospy.Time.now();
	      self.LS_msg.angle_min = -msg.position[0]/5;
	      self.LS_msg.range_min = 0.04;
	      self.LS_msg.range_max = 2.55;
	      
	      
	      #command for run motor radar
	      JC = JointCommand();
	      JC.name = 'radar_motor';
	      JC.type = 'to_angle'; 
	      JC.speed = 300;
	      self.last_angle_cmd = -self.last_angle_cmd;
	      JC.angle = self.last_angle_cmd;
	      self.pub_JC.publish(JC);
	    self.count_ray = self.count_ray + 1;
	    angle = msg.position[0]/5;
	    #add ray to laserscan message
	    self.LS_msg.ranges.append(self.last_range);
	    
	    #publish PointCloud message
	    pnt = PointCloud()
	    pnt.header.frame_id = "base_link"
	    pnt.header.stamp = rospy.Time.now()
	    #rospy.loginfo('Range:'+str(self.last_range)+' Angle: '+str(angle)+' X: '+str(self.last_range*cos(angle))+' Y: '+str(self.last_range*sin(angle)));
	    pnt.points.append(Point32(self.last_range*cos(angle), -self.last_range*sin(angle), 0))
	    self.pub_PC.publish(pnt)