/** 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(); } }); }
@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()); }
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); }
/** * 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); }
@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); }
@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()); } }); } }
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); } }); }
@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); } }); }
@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(); }
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); }
/** 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; } }); }