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