Skip to content

Instantly share code, notes, and snippets.

@PierrickKoch
Created June 8, 2011 08:56
Show Gist options
  • Save PierrickKoch/1014053 to your computer and use it in GitHub Desktop.
Save PierrickKoch/1014053 to your computer and use it in GitHub Desktop.
package org.ros.tutorials.pubsub;
import org.apache.commons.logging.Log;
import org.ros.MessageListener;
import org.ros.Node;
import org.ros.NodeConfiguration;
import org.ros.NodeMain;
import org.ros.Subscriber;
import org.ros.Publisher;
import org.ros.message.sensor_msgs.LaserScan;
import org.ros.message.geometry_msgs.Twist;
public class Proteus implements NodeMain {
@Override
public void main(NodeConfiguration configuration) {
Node node = null;
try {
node = new Node("test_cmd", configuration);
final Publisher<Twist> publisher =
node.createPublisher("/ATRV/Motion_Controller", Twist.class);
final Log log = node.getLog();
node.createSubscriber("/ATRV/Sick", new MessageListener<LaserScan>() {
@Override
public void onNewMessage(LaserScan msg) {
int i, mid = msg.ranges.length / 2;
Twist cmd = new Twist();
// halt if an object is less than 2m in a 30deg angle
boolean halt = false;
for (i = mid - 15; i <= mid + 15; i++) {
if (msg.ranges[i] < 2.0) {
halt = true;
break;
}
}
if (halt) {
double midA = 0, midB = 0;
// we go to the highest-range side scanned
for (i = 0; i < mid; i++)
midA += msg.ranges[i];
for (i = mid; i < msg.ranges.length; i++)
midB += msg.ranges[i];
log.info("A:" + midA + ", B:" + midB);
if (midA > midB) {
cmd.angular.z = -1;
} else {
cmd.angular.z = 1;
}
} else {
cmd.linear.x = 1;
}
publisher.publish(cmd);
}
}, LaserScan.class);
} catch (Exception e) {
if (node != null) {
node.getLog().fatal(e);
} else {
e.printStackTrace();
}
}
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment