예제 #1
0
  /** Entry hook for ROS when called as stand-alone node */
  @Override
  public void onStart(Node node) {
    System.out.println("REACHED ROBOT NAVIGATION");

    motionPub = node.newPublisher("/command/PathFollowing", "rss_msgs/MotionMsg");
    localSub = node.newSubscriber("/rss/localization", "rss_msgs/OdometryMsg");
    pathSub = node.newSubscriber("/command/path", "lab6_msgs/GUIPolyMsg");

    pathSub.addMessageListener(
        new MessageListener<org.ros.message.lab6_msgs.GUIPolyMsg>() {
          @Override
          public void onNewMessage(org.ros.message.lab6_msgs.GUIPolyMsg message) {
            for (int i = 0; i < message.numVertices; i++) {
              Point2D.Double point = new Point2D.Double(message.x[i], message.y[i]);
              goalPath.add(point);
            }
          }
        });

    localSub.addMessageListener(
        new MessageListener<org.ros.message.rss_msgs.OdometryMsg>() {
          @Override
          public void onNewMessage(org.ros.message.rss_msgs.OdometryMsg message) {
            System.out.println("Getting Odometry");
            locX = message.x;
            locY = message.y;
            locTheta = message.theta;

            navigateRobot();
          }
        });
  }
예제 #2
0
 @Override
 public void onStart(ConnectedNode connectedNode) {
   // Subscribe to the laser scans.
   Subscriber<sensor_msgs.LaserScan> laserScanSubscriber =
       connectedNode.newSubscriber(laserTopic, sensor_msgs.LaserScan._TYPE);
   laserScanSubscriber.addMessageListener(this);
   // Subscribe to the command velocity. This is needed for auto adjusting the
   // zoom in ZoomMode.VELOCITY_ZOOM_MODE mode.
   Subscriber<geometry_msgs.Twist> twistSubscriber =
       connectedNode.newSubscriber("cmd_vel", geometry_msgs.Twist._TYPE);
   twistSubscriber.addMessageListener(
       new MessageListener<geometry_msgs.Twist>() {
         @Override
         public void onNewMessage(final geometry_msgs.Twist robotVelocity) {
           post(
               new Runnable() {
                 @Override
                 public void run() {
                   distanceRenderer.currentSpeed(robotVelocity.getLinear().getX());
                 }
               });
         }
       });
   setOnTouchListener(this);
   // Load the last saved setting.
   distanceRenderer.loadPreferences(this.getContext());
 }
예제 #3
0
 public void addExchangeListCallback(MessageListener<AppInstallationState> callback)
     throws RosException {
   Subscriber<AppInstallationState> s =
       node.newSubscriber(
           resolver.resolve("exchange_app_list"), "app_manager/AppInstallationState");
   s.addMessageListener(callback);
   subscriptions.add(s);
 }
예제 #4
0
  /**
   * The constructor for the StatusListener class.
   *
   * @param topicName The name of the ROS topic
   */
  public StatusListener(RobotModel robot, String topicName) {
    log = GuiNode.getLog();
    connectedNode = GuiNode.getConnectedNode();
    this.robot = robot;

    Subscriber<std_msgs.String> subscriber =
        connectedNode.newSubscriber(topicName, std_msgs.String._TYPE);
    subscriber.addMessageListener(this);
  }
예제 #5
0
  @Override
  public void onStart(ConnectedNode connectedNode) {
    this.connectedNode = connectedNode;
    super.onStart(connectedNode);

    odoComputer = new OdoComputer(1.7, 14.3, connectedNode.getCurrentTime());

    ultasonicPublisher = connectedNode.newPublisher(namespace.join("distance"), Float32._TYPE);
    voltagePublisher = connectedNode.newPublisher(namespace.join("voltage"), Float32._TYPE);
    odometryPublisher = connectedNode.newPublisher(namespace.join("odom"), Odometry._TYPE);
    tfPublisher = connectedNode.newPublisher(namespace.join("tf"), TransformStamped._TYPE);

    Subscriber<Twist> motorSpeedSubscriber =
        connectedNode.newSubscriber(namespace.join("cmd_vel"), Twist._TYPE);
    motorSpeedSubscriber.addMessageListener(
        new MessageListener<Twist>() {
          @Override
          public void onNewMessage(Twist msg) {
            double x = msg.getLinear().getX();
            double y = msg.getAngular().getZ();
            if (Math.abs(x) < 0.02) x = 0;
            if (Math.abs(y) < 0.02) y = 0;
            double s = x < 0 ? -1 : 1;
            double speed = Math.sqrt(x * x + y * y) / Math.sqrt(2);
            double twist = -200 * y / speed;
            speed *= 100 * s;
            try {
              brick.MOTOR.steerMotors(NXT.Motor.A, NXT.Motor.B, (int) speed, (int) twist, 10000);
            } catch (IOException e) {
              NXTNode.this.connectedNode.getLog().error("Motor error", e);
            }
          }
        });

    Subscriber<Int16> toneSubscriber =
        connectedNode.newSubscriber(namespace.join("tone"), Int16._TYPE);
    toneSubscriber.addMessageListener(
        new MessageListener<Int16>() {
          @Override
          public void onNewMessage(Int16 freq) {
            try {
              brick.SYSTEM.playTone(freq.getData(), 500);
            } catch (IOException e) {
              NXTNode.this.connectedNode.getLog().error("Tone error", e);
            }
          }
        });

    connectedNode
        .getScheduledExecutorService()
        .scheduleAtFixedRate(new SensorSample(), loopMs, loopMs, TimeUnit.MILLISECONDS);
  }
예제 #6
0
  @Override
  public void onStart(ConnectedNode connectedNode) {

    frames = new HashMap<String, Frame>();
    node = connectedNode;

    Subscriber<tf.tfMessage> subscriber = connectedNode.newSubscriber("/tf", tf.tfMessage._TYPE);
    subscriber.addMessageListener(
        new MessageListener<tf.tfMessage>() {
          @Override
          public void onNewMessage(tfMessage tfm) {
            if (tfm != null) {
              for (TransformStamped tf : tfm.getTransforms()) {
                setTransform(tf);
              }
            }
          }
        });
  }
 private void setUpListeners() {
   if (jointSubscriber != null) {
     jointSubscriber.addMessageListener(
         new MessageListener<JointState>() {
           public void onNewMessage(JointState message) {
             receivedClockMessage(message.getHeader().getStamp().totalNsecs());
           }
         });
   }
 }
예제 #8
0
 protected void setupSubscriber(ConnectedNode connectedNode) {
   subscriber = connectedNode.newSubscriber(subscriberTopic, std_msgs.String._TYPE);
   subscriber.addMessageListener(
       new MessageListener<std_msgs.String>() {
         @Override
         public void onNewMessage(std_msgs.String string) {
           subcriber_string = string.getData();
           hasReceivedMsg = true;
         }
       });
 }
 private void scalerProcessor(
     final Publisher<Int16> wheelPublisher, Subscriber<Int16> wheelSubscriber) {
   wheelSubscriber.addMessageListener(
       new MessageListener<Int16>() {
         @Override
         public void onNewMessage(Int16 message) {
           Int16 value = wheelPublisher.newMessage();
           System.out.println("Wheel scaler received " + message.getData());
           value.setData((short) (message.getData() * scale));
           wheelPublisher.publish(value);
         }
       });
 }
예제 #10
0
 @Override
 public void onStart(Node node) {
   System.out.println("KINECT NODE STARTED");
   kinSub = node.newSubscriber("/camera/depth_registered/points", "sensor_msgs/PointCloud2");
   kinSub.addMessageListener(
       new MessageListener<org.ros.message.sensor_msgs.PointCloud2>() {
         @Override
         public synchronized void onNewMessage(org.ros.message.sensor_msgs.PointCloud2 message) {
           unpackPointCloudData(
               (int) message.width,
               (int) message.height,
               (int) message.point_step,
               (int) message.row_step,
               message.data);
         }
       });
 }
예제 #11
0
  @Override
  public void onStart(Node node) {
    ParameterTree paramTree = node.newParameterTree();
    mapFileName = paramTree.getString(node.resolveName("~/mapFileName"));

    last_localization = System.currentTimeMillis();

    try {
      polymap = new PolygonMap(mapFileName);
    } catch (Exception e) {
      e.printStackTrace();
    }

    Point2D.Double robotStart = polymap.getRobotStart();

    Rectangle2D.Double cSpaceWorld =
        CSpace.CSpaceWorldRect(polymap.getWorldRect(), polymap.getRobotStart(), RADIUS);
    List<PolygonObstacle> cSpaceObstacles =
        CSpace.computeCSpace(polymap, getRobot(), polymap.getRobotStart());

    Map<int[], Point2D.Double> fiducialPairs = new HashMap<int[], Point2D.Double>();

    // Finish adding to fiducial pairs here from map

    localization = new Localization(cSpaceObstacles, cSpaceWorld, robotStart, fiducialPairs);

    blobTrack = new BlobTracking(width, height);
    final boolean reverseRGB = node.newParameterTree().getBoolean("reverse_rgb", false);

    localPub = node.newPublisher("/rss/localization", "rss_msgs/OdometryMsg");
    stopPub = node.newPublisher("/rss/stop", "rss_msgs/ResetMsg");

    vidSub = node.newSubscriber("/rss/video", "sensor_msgs/Image");
    vidSub.addMessageListener(
        new MessageListener<org.ros.message.sensor_msgs.Image>() {
          @Override
          public void onNewMessage(org.ros.message.sensor_msgs.Image message) {
            byte[] rgbData;
            if (reverseRGB) {
              rgbData = Image.RGB2BGR(message.data, (int) message.width, (int) message.height);
            } else {
              rgbData = message.data;
            }
            assert ((int) message.width == width);
            assert ((int) message.height == height);
            handle(rgbData);
          }
        });

    depthSub = node.newSubscriber("/rss/depth", "sensor_msgs/Image");
    depthSub.addMessageListener(
        new MessageListener<org.ros.message.sensor_msgs.Image>() {
          @Override
          public void onNewMessage(org.ros.message.sensor_msgs.Image message) {
            byte[] rgbData;
            if (reverseRGB) {
              rgbData = Image.RGB2BGR(message.data, (int) message.width, (int) message.height);
            } else {
              rgbData = message.data;
            }
            assert ((int) message.width == width);
            assert ((int) message.height == height);
            handle(rgbData);
          }
        });

    odoSub = node.newSubscriber("/rss/odometry", "rss_msgs/OdometryMsg");
    odoSub.addMessageListener(
        new MessageListener<org.ros.message.rss_msgs.OdometryMsg>() {
          @Override
          public void onNewMessage(org.ros.message.rss_msgs.OdometryMsg message) {
            odo_x = message.x;
            odo_y = message.y;
            odo_theta = message.theta;

            Point2D.Double curr_point;
            OdometryMsg msg = new OdometryMsg();
            if (LOCALIZE) {
              synchronized (localization) {
                curr_point = localization.encoderUpdate(odo_x, odo_y);
              }
              msg.x = curr_point.x;
              msg.y = curr_point.y;
            } else {
              msg.x = odo_x;
              msg.y = odo_y;
            }
            msg.theta = odo_theta;
            localPub.publish(msg);
          }
        });

    Thread visionThread =
        new Thread(
            new Runnable() {
              @Override
              public void run() {
                handleImage();
                try {
                  Thread.sleep(10);
                } catch (Exception exc) {
                  exc.printStackTrace();
                }
              }
            });
    visionThread.start();
  }
예제 #12
0
 public void addAppListCallback(MessageListener<AppList> callback) throws RosException {
   Subscriber<AppList> s = node.newSubscriber(resolver.resolve("app_list"), "app_manager/AppList");
   s.addMessageListener(callback);
   subscriptions.add(s);
 }
예제 #13
0
  /** Initialize communication on ROS topic */
  void init(String agentName) {
    logger1.info("ArtBaseScan >> init(agentName)");
    logger1.info("ArtBaseScan >> init(agentName): agent name=" + agentName);
    logger1.info("ArtBaseScan >> init(agentName): Defining artifact property " + propertyName);

    defineObsProperty(propertyName, 50000f, 50000f, 50000f, 50000f, 50000f);

    if (agentName != null) {
      // Update topic name with agent name
      topicName = "/" + agentName + topicName;
    }

    cmd = new ReadCmd();

    subscriberScan = (Subscriber<LaserScan>) createSubscriber(topicName, topicType);
    subscriberScan.addMessageListener(
        new MessageListener<sensor_msgs.LaserScan>() {
          @Override
          public void onNewMessage(LaserScan message) {
            // logger1.info("I heard from ArtScan: " + message);
            float[] localScan = message.getRanges();
            // logger1.info("ArtScan >> init(agentName): RANGE SIZE " + localScan.length);
            // logger1.info("----ZERO------------" + localScan[0] + "-----------ZERO---------");
            // logger1.info("----512------------" + localScan[511] + "-----------512---------");
            // logger1.info("----256------------" + localScan[255] + "-----------256---------");
            // logger1.info(message.toString());
            if (currentScan == null) {
              currentScan = localScan;
            } else if (!Arrays.equals(currentScan, localScan) && refreshRate()) {
              // Do not send the message if the current value is the same of the previous one
              currentScan = localScan;
              /// *
              /*

              for(int i = 0 ;i<localScan.length-1;i=i+128)
              {
              	if(localScan[i] <= 0.300 && localScan[i] > 0.200)
              {
              		logger1.info("LASER: "+i+ "Keep out :  "+localScan[i]);
              		//laser < 30 cm
              		} else if(localScan[i] <= 0.200 && localScan[i] > 0.100 )
              				{
              		logger1.info("LASER: "+i+ "I'm speaking seriously KEEP OUT!!");
              		//laser < 30 cm
              	}else if(localScan[i] <= 0.100 )
              				{
              		logger1.info("LASER: "+i+ " I'll kill you BITch");
              		//laser < 30 cm
              		}
              		else
              		{
              		logger1.info("Laser"+i+":"+localScan[i]);
                  	//print laser
              		}

              	}
              	int i = 0;
              	*/
              // logger1.info("X: " +m_odometry.getX());
              // logger1.info("Y: " +m_odometry.getY());
              // logger1.info("Z: " +m_odometry.getZ());
              execInternalOp("receiving");
            }
            /* primeira tentativa com 726 posiçoes
             *
            for(int i = 182 ;i<726;i=i+181)
            {
            	if(localScan[i] <= 0.300 && localScan[i] > 0.200 )
            	{
            	logger1.info("LASER: "+i+ " Keep out");
            	//laser < 30 cm
            	} else if(localScan[i] <= 0.200 && localScan[i] > 0.100 )
            			{
            	logger1.info("LASER: "+i+ "I'm speaking seriously KEEP OUT!!");
            	//laser < 30 cm
            	}else if(localScan[i] <= 0.100 )
            			{
            	logger1.info("LASER: "+i+ " I'll kill you BITch");
            	//laser < 30 cm
            	}
            	else
            	{
            	logger1.info("Laser"+i+":"+localScan[i]);
               	//print laser
            	}

            }
            int i = 0;
            // */
            // logger1.info("X: " +m_odometry.getX());
            // logger1.info("Y: " +m_odometry.getY());
            // logger1.info("Z: " +m_odometry.getZ());
            // execInternalOp("receiving");
            // }

            // currentScan = message.getRanges();
            // execInternalOp("receiving");
          }

          private boolean refreshRate() {
            boolean result = false;
            if (refreshRateAux % refreshRateValue == 0) {
              // logger1.info("----------- Artifact should UPDATE current value. [" + refreshRateAux
              // + "/" + refreshRateValue + "]");
              refreshRateAux = refreshRateValue;
              result = true;
            }
            // else
            // {
            //	logger1.info("----------- Artifact should NOT update current value. [" +
            // refreshRateAux + "/" + refreshRateValue + "]");
            // }

            refreshRateAux++;
            // return true;
            return result;
          }
        });

    logger1.info("ArtScan >> init(agentName): end of method");
  }
  @Override
  public void onStart(ConnectedNode connectedNode) {
    // log = connectedNode.getLog();

    final Publisher<Float32> pub_motor = Topic.MOTOR_CMD.newPublisher(connectedNode, nameSpace);
    final Publisher<Float32> pub_vel = Topic.WHEEL_VEL.newPublisher(connectedNode, nameSpace);

    // This CancellableLoop will be canceled automatically when the node
    // shuts
    // down.
    connectedNode.executeCancellableLoop(
        new CancellableLoop() {

          @Override
          protected void setup() {
            ticks_since_target = timeout_ticks;
            wheel_prev = wheel_latest;
            then = System.currentTimeMillis();
            wheel_prev = wheel_latest;
          }

          @Override
          protected void loop() throws InterruptedException {

            spinOnce();
            Thread.sleep(rate);
          }

          private void spinOnce() {

            previous_error = 0.0f;
            prev_vel.clear();
            prev_vel.add(0.0d);
            prev_vel.add(0.0d);
            integral = 0.0f;
            error = 0;
            derivative = 0.0f;
            vel = 0.0f;

            while (ticks_since_target < timeout_ticks) {
              calcVelocity();
              doPid();
              Float32 mes = pub_motor.newMessage();
              mes.setData((float) motor);
              pub_motor.publish(mes);
              try {
                Thread.sleep(rate);
              } catch (InterruptedException e) {
                // TODO Auto-generated catch block
                e.printStackTrace();
              }
              ticks_since_target++;
              if (ticks_since_target == timeout_ticks) {
                Float32 mes2 = pub_motor.newMessage();
                mes2.setData(0);
                pub_motor.publish(mes2);
              }
            }
          }

          private void doPid() {
            long pid_dt_duration = System.currentTimeMillis() - prev_pid_time;
            double pid_dt = pid_dt_duration / 1000.0d;
            pid_dt = Math.max(1, pid_dt);
            prev_pid_time = System.currentTimeMillis();
            error = target - vel;
            integral = integral + (error * pid_dt);
            derivative = (error - previous_error) / pid_dt;
            previous_error = error;

            motor = (Kp * error) + (Ki * integral) + (Kd * derivative);
            if (motor > out_max) {
              motor = out_max;
              integral = integral - (error * pid_dt);
            }
            if (motor < out_min) {
              motor = out_min;
              integral = integral - (error * pid_dt);
            }
            if (target == 0) {
              motor = 0;
            }
          }

          private void calcVelocity() {
            dt = (System.currentTimeMillis() - then) / 1000;
            dt = Math.max(dt, 1);
            if (wheel_latest == wheel_prev) {
              double cur_vel = ((1.0d / ticks_per_meter) / dt);
              if (Math.abs(cur_vel) < vel_threshold) {
                appendVel(0);
                calcRollingVel();
              } else {
                if (Math.abs(cur_vel) < vel) {
                  appendVel(cur_vel);
                  calcRollingVel();
                }
              }
            } else {
              double cur_vel = (wheel_latest - wheel_prev) / dt;
              appendVel(cur_vel);
              calcRollingVel();
              wheel_prev = wheel_latest;
              then = System.currentTimeMillis();
            }
            Float32 msg = pub_vel.newMessage();
            msg.setData(vel);
            pub_vel.publish(msg);
          }

          private void calcRollingVel() {
            double size = prev_vel.size();
            double total = 0;
            for (Double val : prev_vel) {
              total += val;
            }
            vel = 0;
            if (size > 0) {
              vel = (float) (total / size);
            }
          }

          private void appendVel(double cur_vel) {
            prev_vel.add(cur_vel);
            prev_vel.remove(0);
          }
        });

    Subscriber<Int16> wsubscriber = Topic.WHEEL.newSubscriber(connectedNode, nameSpace);
    wsubscriber.addMessageListener(
        new MessageListener<Int16>() {
          @Override
          public void onNewMessage(Int16 message) {
            short enc = message.getData();
            if (enc < encoder_low_wrap && prev_encoder > encoder_high_wrap) {
              wheel_mult = wheel_mult + 1;
            }
            if (enc > encoder_high_wrap && prev_encoder < encoder_low_wrap) {
              wheel_mult = wheel_mult - 1;
            }
            wheel_latest =
                1.0d * (enc + wheel_mult * (encoder_max - encoder_min)) / ticks_per_meter;
            prev_encoder = enc;
          }
        });

    Subscriber<Float32> wtsubscriber = Topic.WHEEL_VTARGET.newSubscriber(connectedNode, nameSpace);
    wtsubscriber.addMessageListener(
        new MessageListener<Float32>() {
          @Override
          public void onNewMessage(Float32 message) {
            target = message.getData();
            ticks_since_target = 0;
          }
        });
  }