Merge pull request '更新代码' (#19) from dev into master
commit
7a0594c15e
File diff suppressed because it is too large
Load Diff
@ -0,0 +1,178 @@
|
||||
package com.jilk.ros.model;
|
||||
|
||||
import java.util.List;
|
||||
|
||||
/**
|
||||
* Created by super_yu on 2018/1/8.
|
||||
*/
|
||||
|
||||
public class FootPrintResult {
|
||||
/**
|
||||
* header : {"seq":4297,"frame_id":"odom","stamp":{"secs":1515378187,"nsecs":125325857}}
|
||||
* polygon : {"points":[{"z":0,"x":0.31369608640670776,"y":-0.5355392694473267},{"z":0,"x":0.35115259885787964,"y":-0.016890067607164383},{"z":0,"x":-0.30713292956352234,"y":0.03065088950097561},{"z":0,"x":-0.3445894420146942,"y":-0.4879983365535736}]}
|
||||
*/
|
||||
|
||||
private HeaderBean header;
|
||||
private PolygonBean polygon;
|
||||
|
||||
public HeaderBean getHeader() {
|
||||
return header;
|
||||
}
|
||||
|
||||
public void setHeader(HeaderBean header) {
|
||||
this.header = header;
|
||||
}
|
||||
|
||||
public PolygonBean getPolygon() {
|
||||
return polygon;
|
||||
}
|
||||
|
||||
public void setPolygon(PolygonBean polygon) {
|
||||
this.polygon = polygon;
|
||||
}
|
||||
|
||||
public static class HeaderBean {
|
||||
/**
|
||||
* seq : 4297
|
||||
* frame_id : odom
|
||||
* stamp : {"secs":1515378187,"nsecs":125325857}
|
||||
*/
|
||||
|
||||
private int seq;
|
||||
private String frame_id;
|
||||
private StampBean stamp;
|
||||
|
||||
public int getSeq() {
|
||||
return seq;
|
||||
}
|
||||
|
||||
public void setSeq(int seq) {
|
||||
this.seq = seq;
|
||||
}
|
||||
|
||||
public String getFrame_id() {
|
||||
return frame_id;
|
||||
}
|
||||
|
||||
public void setFrame_id(String frame_id) {
|
||||
this.frame_id = frame_id;
|
||||
}
|
||||
|
||||
public StampBean getStamp() {
|
||||
return stamp;
|
||||
}
|
||||
|
||||
public void setStamp(StampBean stamp) {
|
||||
this.stamp = stamp;
|
||||
}
|
||||
|
||||
public static class StampBean {
|
||||
/**
|
||||
* secs : 1515378187
|
||||
* nsecs : 125325857
|
||||
*/
|
||||
|
||||
private int secs;
|
||||
private int nsecs;
|
||||
|
||||
public int getSecs() {
|
||||
return secs;
|
||||
}
|
||||
|
||||
public void setSecs(int secs) {
|
||||
this.secs = secs;
|
||||
}
|
||||
|
||||
public int getNsecs() {
|
||||
return nsecs;
|
||||
}
|
||||
|
||||
public void setNsecs(int nsecs) {
|
||||
this.nsecs = nsecs;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public static class PolygonBean {
|
||||
private List<PointsBean> points;
|
||||
|
||||
public List<PointsBean> getPoints() {
|
||||
return points;
|
||||
}
|
||||
|
||||
public void setPoints(List<PointsBean> points) {
|
||||
this.points = points;
|
||||
}
|
||||
|
||||
public static class PointsBean {
|
||||
/**
|
||||
* z : 0
|
||||
* x : 0.31369608640670776
|
||||
* y : -0.5355392694473267
|
||||
*/
|
||||
|
||||
private int z;
|
||||
private double x;
|
||||
private double y;
|
||||
|
||||
public int getZ() {
|
||||
return z;
|
||||
}
|
||||
|
||||
public void setZ(int z) {
|
||||
this.z = z;
|
||||
}
|
||||
|
||||
public double getX() {
|
||||
return x;
|
||||
}
|
||||
|
||||
public void setX(double x) {
|
||||
this.x = x;
|
||||
}
|
||||
|
||||
public double getY() {
|
||||
return y;
|
||||
}
|
||||
|
||||
public void setY(double y) {
|
||||
this.y = y;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// {
|
||||
// "header": {
|
||||
// "seq": 4297,
|
||||
// "frame_id": "odom",
|
||||
// "stamp": {
|
||||
// "secs": 1515378187,
|
||||
// "nsecs": 125325857
|
||||
// }
|
||||
// },
|
||||
// "polygon": {
|
||||
// "points": [
|
||||
// {
|
||||
// "z": 0,
|
||||
// "x": 0.31369608640670776,
|
||||
// "y": -0.5355392694473267
|
||||
// },
|
||||
// {
|
||||
// "z": 0,
|
||||
// "x": 0.35115259885787964,
|
||||
// "y": -0.016890067607164383
|
||||
// },
|
||||
// {
|
||||
// "z": 0,
|
||||
// "x": -0.30713292956352234,
|
||||
// "y": 0.03065088950097561
|
||||
// },
|
||||
// {
|
||||
// "z": 0,
|
||||
// "x": -0.3445894420146942,
|
||||
// "y": -0.4879983365535736
|
||||
// }
|
||||
// ]
|
||||
// }
|
||||
// }
|
||||
}
|
||||
@ -0,0 +1,274 @@
|
||||
package com.jilk.ros.model;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
|
||||
/**
|
||||
* Created by super_yu on 2018/2/5.
|
||||
*/
|
||||
|
||||
public class InitPoseResult {
|
||||
|
||||
/**
|
||||
* op : publish
|
||||
* topic : /initialpose
|
||||
* msg : {"header":{"frame_id":"map"},"pose":{"pose":{"position":{"x":0,"y":0,"z":0},"orientation":{"x":0,"y":0,"z":0,"w":1}},"covariance":[0.25,0,0,0,0,0,0,0.25,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.06853891945200942]}}
|
||||
*/
|
||||
|
||||
private String op = "publish";
|
||||
private String topic = "/initialpose";
|
||||
private MsgBean msg;
|
||||
|
||||
|
||||
public MsgBean getMsg() {
|
||||
return msg;
|
||||
}
|
||||
|
||||
public void setMsg(MsgBean msg) {
|
||||
this.msg = msg;
|
||||
}
|
||||
|
||||
public static class MsgBean {
|
||||
/**
|
||||
* header : {"frame_id":"map"}
|
||||
* pose : {"pose":{"position":{"x":0,"y":0,"z":0},"orientation":{"x":0,"y":0,"z":0,"w":1}},"covariance":[0.25,0,0,0,0,0,0,0.25,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0.06853891945200942]}
|
||||
*/
|
||||
|
||||
private HeaderBean header;
|
||||
private PoseBeanX pose;
|
||||
|
||||
public HeaderBean getHeader() {
|
||||
return header;
|
||||
}
|
||||
|
||||
public void setHeader(HeaderBean header) {
|
||||
this.header = header;
|
||||
}
|
||||
|
||||
public PoseBeanX getPose() {
|
||||
return pose;
|
||||
}
|
||||
|
||||
public void setPose(PoseBeanX pose) {
|
||||
this.pose = pose;
|
||||
}
|
||||
|
||||
public static class HeaderBean {
|
||||
/**
|
||||
* frame_id : map
|
||||
*/
|
||||
|
||||
private String frame_id = "map";
|
||||
}
|
||||
|
||||
public static class PoseBeanX {
|
||||
/**
|
||||
* pose : {"position":{"x":0,"y":0,"z":0},"orientation":{"x":0,"y":0,"z":0,"w":1}}
|
||||
* covariance :
|
||||
* 0.25,0,0,0,0,0,0,0.25,
|
||||
* 0,0,0,0,0,0,0,0,0,0,0,
|
||||
* 0,0,0,0,0,0,0,0,0,0,0,
|
||||
* 0,0,0,0,0,0.06853891945200942
|
||||
*/
|
||||
|
||||
private PoseBean pose;
|
||||
private List<Double> covariance;
|
||||
|
||||
public PoseBean getPose() {
|
||||
return pose;
|
||||
}
|
||||
|
||||
public void setPose(PoseBean pose) {
|
||||
this.pose = pose;
|
||||
}
|
||||
|
||||
public void setCovariance() {
|
||||
covariance = new ArrayList<Double>();
|
||||
for (int i = 0; i < 36; i++) {
|
||||
if (i == 0) {
|
||||
covariance.add(0.25);
|
||||
} else if (i == 7) {
|
||||
covariance.add(0.25);
|
||||
} else if (i == 35) {
|
||||
covariance.add(0.06853891945200942);
|
||||
|
||||
} else {
|
||||
covariance.add((double) 0);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public static class PoseBean {
|
||||
/**
|
||||
* position : {"x":0,"y":0,"z":0}
|
||||
* orientation : {"x":0,"y":0,"z":0,"w":1}
|
||||
*/
|
||||
|
||||
private PositionBean position;
|
||||
private OrientationBean orientation;
|
||||
|
||||
public PositionBean getPosition() {
|
||||
return position;
|
||||
}
|
||||
|
||||
public void setPosition(PositionBean position) {
|
||||
this.position = position;
|
||||
}
|
||||
|
||||
public OrientationBean getOrientation() {
|
||||
return orientation;
|
||||
}
|
||||
|
||||
public void setOrientation(OrientationBean orientation) {
|
||||
this.orientation = orientation;
|
||||
}
|
||||
|
||||
public static class PositionBean {
|
||||
/**
|
||||
* x : 0
|
||||
* y : 0
|
||||
* z : 0
|
||||
*/
|
||||
|
||||
private double x;
|
||||
private double y;
|
||||
private double z;
|
||||
|
||||
public double getX() {
|
||||
return x;
|
||||
}
|
||||
|
||||
public void setX(double x) {
|
||||
this.x = x;
|
||||
}
|
||||
|
||||
public double getY() {
|
||||
return y;
|
||||
}
|
||||
|
||||
public void setY(double y) {
|
||||
this.y = y;
|
||||
}
|
||||
|
||||
public double getZ() {
|
||||
return z;
|
||||
}
|
||||
|
||||
public void setZ(double z) {
|
||||
this.z = z;
|
||||
}
|
||||
}
|
||||
|
||||
public static class OrientationBean {
|
||||
/**
|
||||
* x : 0
|
||||
* y : 0
|
||||
* z : 0
|
||||
* w : 1
|
||||
*/
|
||||
|
||||
private double x;
|
||||
private double y;
|
||||
private double z;
|
||||
private double w;
|
||||
|
||||
public double getX() {
|
||||
return x;
|
||||
}
|
||||
|
||||
public void setX(double x) {
|
||||
this.x = x;
|
||||
}
|
||||
|
||||
public double getY() {
|
||||
return y;
|
||||
}
|
||||
|
||||
public void setY(double y) {
|
||||
this.y = y;
|
||||
}
|
||||
|
||||
public double getZ() {
|
||||
return z;
|
||||
}
|
||||
|
||||
public void setZ(double z) {
|
||||
this.z = z;
|
||||
}
|
||||
|
||||
public double getW() {
|
||||
return w;
|
||||
}
|
||||
|
||||
public void setW(double w) {
|
||||
this.w = w;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// {
|
||||
// "op": "publish",
|
||||
// "topic": "/initialpose",
|
||||
// "msg": {
|
||||
// "header": {
|
||||
// "frame_id": "map"
|
||||
// },
|
||||
// "pose": {
|
||||
// "pose": {
|
||||
// "position": {
|
||||
// "x": 0,
|
||||
// "y": 0,
|
||||
// "z": 0
|
||||
// },
|
||||
// "orientation": {
|
||||
// "x": 0,
|
||||
// "y": 0,
|
||||
// "z": 0,
|
||||
// "w": 1
|
||||
// }
|
||||
// },
|
||||
// "covariance": [
|
||||
// 0.25,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0.25,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0.06853891945200942
|
||||
// ]
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
}
|
||||
@ -0,0 +1,501 @@
|
||||
package com.jilk.ros.model;
|
||||
|
||||
/**
|
||||
* Created by super_yu on 2018/1/5.
|
||||
*/
|
||||
|
||||
import java.util.List;
|
||||
|
||||
/**
|
||||
*对位置移动有数据变化的是
|
||||
* orientation z、w
|
||||
* position x、y
|
||||
* twist x
|
||||
* angular z
|
||||
* Created by super_yu on 2017/9/29.
|
||||
*/
|
||||
|
||||
public class PoiResult {
|
||||
|
||||
/**
|
||||
* twist : {"twist":{"angular":{"z":0,"y":0,"x":0},"linear":{"z":0,"y":0,"x":0}},"covariance":[0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]}
|
||||
* child_frame_id : base_footprint
|
||||
* pose : {"covariance":[0.1,0,0,0,0,0,0,0.1,0,0,0,0,0,0,1.7976931348623157E308,0,0,0,0,0,0,1.7976931348623157E308,0,0,0,0,0,0,1.7976931348623157E308,0,0,0,0,0,0,0.2],"pose":{"position":{"z":0,"y":-0.16137039724419636,"x":0.001470031525333453},"orientation":{"w":-0.08271030624895466,"z":-0.9965736326234024,"y":"-0","x":0}}}
|
||||
* header : {"seq":69712,"stamp":{"secs":1506650141,"nsecs":455670567},"frame_id":"odom"}
|
||||
*/
|
||||
|
||||
private TwistBeanX twist;
|
||||
private String child_frame_id;
|
||||
private PoseBeanX pose;
|
||||
private HeaderBean header;
|
||||
|
||||
public TwistBeanX getTwist() {
|
||||
return twist;
|
||||
}
|
||||
|
||||
public void setTwist(TwistBeanX twist) {
|
||||
this.twist = twist;
|
||||
}
|
||||
|
||||
public String getChild_frame_id() {
|
||||
return child_frame_id;
|
||||
}
|
||||
|
||||
public void setChild_frame_id(String child_frame_id) {
|
||||
this.child_frame_id = child_frame_id;
|
||||
}
|
||||
|
||||
public PoseBeanX getPose() {
|
||||
return pose;
|
||||
}
|
||||
|
||||
public void setPose(PoseBeanX pose) {
|
||||
this.pose = pose;
|
||||
}
|
||||
|
||||
public HeaderBean getHeader() {
|
||||
return header;
|
||||
}
|
||||
|
||||
public void setHeader(HeaderBean header) {
|
||||
this.header = header;
|
||||
}
|
||||
|
||||
public static class TwistBeanX {
|
||||
/**
|
||||
* twist : {"angular":{"z":0,"y":0,"x":0},"linear":{"z":0,"y":0,"x":0}}
|
||||
* covariance : [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]
|
||||
*/
|
||||
|
||||
private TwistBean twist;
|
||||
private List<Integer> covariance;
|
||||
|
||||
public TwistBean getTwist() {
|
||||
return twist;
|
||||
}
|
||||
|
||||
public void setTwist(TwistBean twist) {
|
||||
this.twist = twist;
|
||||
}
|
||||
|
||||
public List<Integer> getCovariance() {
|
||||
return covariance;
|
||||
}
|
||||
|
||||
public void setCovariance(List<Integer> covariance) {
|
||||
this.covariance = covariance;
|
||||
}
|
||||
|
||||
public static class TwistBean {
|
||||
/**
|
||||
* angular : {"z":0,"y":0,"x":0}
|
||||
* linear : {"z":0,"y":0,"x":0}
|
||||
*/
|
||||
|
||||
private AngularBean angular;
|
||||
private LinearBean linear;
|
||||
|
||||
public AngularBean getAngular() {
|
||||
return angular;
|
||||
}
|
||||
|
||||
public void setAngular(AngularBean angular) {
|
||||
this.angular = angular;
|
||||
}
|
||||
|
||||
public LinearBean getLinear() {
|
||||
return linear;
|
||||
}
|
||||
|
||||
public void setLinear(LinearBean linear) {
|
||||
this.linear = linear;
|
||||
}
|
||||
|
||||
public static class AngularBean {
|
||||
/**
|
||||
* z : 0
|
||||
* y : 0
|
||||
* x : 0
|
||||
*/
|
||||
|
||||
private int z;
|
||||
private int y;
|
||||
private int x;
|
||||
|
||||
public int getZ() {
|
||||
return z;
|
||||
}
|
||||
|
||||
public void setZ(int z) {
|
||||
this.z = z;
|
||||
}
|
||||
|
||||
public int getY() {
|
||||
return y;
|
||||
}
|
||||
|
||||
public void setY(int y) {
|
||||
this.y = y;
|
||||
}
|
||||
|
||||
public int getX() {
|
||||
return x;
|
||||
}
|
||||
|
||||
public void setX(int x) {
|
||||
this.x = x;
|
||||
}
|
||||
}
|
||||
|
||||
public static class LinearBean {
|
||||
/**
|
||||
* z : 0
|
||||
* y : 0
|
||||
* x : 0
|
||||
*/
|
||||
|
||||
private int z;
|
||||
private int y;
|
||||
private int x;
|
||||
|
||||
public int getZ() {
|
||||
return z;
|
||||
}
|
||||
|
||||
public void setZ(int z) {
|
||||
this.z = z;
|
||||
}
|
||||
|
||||
public int getY() {
|
||||
return y;
|
||||
}
|
||||
|
||||
public void setY(int y) {
|
||||
this.y = y;
|
||||
}
|
||||
|
||||
public int getX() {
|
||||
return x;
|
||||
}
|
||||
|
||||
public void setX(int x) {
|
||||
this.x = x;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public static class PoseBeanX {
|
||||
/**
|
||||
* covariance : [0.1,0,0,0,0,0,0,0.1,0,0,0,0,0,0,1.7976931348623157E308,0,0,0,0,0,0,1.7976931348623157E308,0,0,0,0,0,0,1.7976931348623157E308,0,0,0,0,0,0,0.2]
|
||||
* pose : {"position":{"z":0,"y":-0.16137039724419636,"x":0.001470031525333453},"orientation":{"w":-0.08271030624895466,"z":-0.9965736326234024,"y":"-0","x":0}}
|
||||
*/
|
||||
|
||||
private PoseBean pose;
|
||||
private List<Double> covariance;
|
||||
|
||||
public PoseBean getPose() {
|
||||
return pose;
|
||||
}
|
||||
|
||||
public void setPose(PoseBean pose) {
|
||||
this.pose = pose;
|
||||
}
|
||||
|
||||
public List<Double> getCovariance() {
|
||||
return covariance;
|
||||
}
|
||||
|
||||
public void setCovariance(List<Double> covariance) {
|
||||
this.covariance = covariance;
|
||||
}
|
||||
|
||||
public static class PoseBean {
|
||||
/**
|
||||
* position : {"z":0,"y":-0.16137039724419636,"x":0.001470031525333453}
|
||||
* orientation : {"w":-0.08271030624895466,"z":-0.9965736326234024,"y":"-0","x":0}
|
||||
*/
|
||||
|
||||
private PositionBean position;
|
||||
private OrientationBean orientation;
|
||||
|
||||
public PositionBean getPosition() {
|
||||
return position;
|
||||
}
|
||||
|
||||
public void setPosition(PositionBean position) {
|
||||
this.position = position;
|
||||
}
|
||||
|
||||
public OrientationBean getOrientation() {
|
||||
return orientation;
|
||||
}
|
||||
|
||||
public void setOrientation(OrientationBean orientation) {
|
||||
this.orientation = orientation;
|
||||
}
|
||||
|
||||
public static class PositionBean {
|
||||
/**
|
||||
* z : 0
|
||||
* y : -0.16137039724419636
|
||||
* x : 0.001470031525333453
|
||||
*/
|
||||
|
||||
private int z;
|
||||
private double y;
|
||||
private double x;
|
||||
|
||||
public int getZ() {
|
||||
return z;
|
||||
}
|
||||
|
||||
public void setZ(int z) {
|
||||
this.z = z;
|
||||
}
|
||||
|
||||
public double getY() {
|
||||
return y;
|
||||
}
|
||||
|
||||
public void setY(double y) {
|
||||
this.y = y;
|
||||
}
|
||||
|
||||
public double getX() {
|
||||
return x;
|
||||
}
|
||||
|
||||
public void setX(double x) {
|
||||
this.x = x;
|
||||
}
|
||||
}
|
||||
|
||||
public static class OrientationBean {
|
||||
/**
|
||||
* w : -0.08271030624895466
|
||||
* z : -0.9965736326234024
|
||||
* y : -0
|
||||
* x : 0
|
||||
*/
|
||||
|
||||
private double w;
|
||||
private double z;
|
||||
private String y;
|
||||
private int x;
|
||||
|
||||
public double getW() {
|
||||
return w;
|
||||
}
|
||||
|
||||
public void setW(double w) {
|
||||
this.w = w;
|
||||
}
|
||||
|
||||
public double getZ() {
|
||||
return z;
|
||||
}
|
||||
|
||||
public void setZ(double z) {
|
||||
this.z = z;
|
||||
}
|
||||
|
||||
public String getY() {
|
||||
return y;
|
||||
}
|
||||
|
||||
public void setY(String y) {
|
||||
this.y = y;
|
||||
}
|
||||
|
||||
public int getX() {
|
||||
return x;
|
||||
}
|
||||
|
||||
public void setX(int x) {
|
||||
this.x = x;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public static class HeaderBean {
|
||||
/**
|
||||
* seq : 69712
|
||||
* stamp : {"secs":1506650141,"nsecs":455670567}
|
||||
* frame_id : odom
|
||||
*/
|
||||
|
||||
private int seq;
|
||||
private StampBean stamp;
|
||||
private String frame_id;
|
||||
|
||||
public int getSeq() {
|
||||
return seq;
|
||||
}
|
||||
|
||||
public void setSeq(int seq) {
|
||||
this.seq = seq;
|
||||
}
|
||||
|
||||
public StampBean getStamp() {
|
||||
return stamp;
|
||||
}
|
||||
|
||||
public void setStamp(StampBean stamp) {
|
||||
this.stamp = stamp;
|
||||
}
|
||||
|
||||
public String getFrame_id() {
|
||||
return frame_id;
|
||||
}
|
||||
|
||||
public void setFrame_id(String frame_id) {
|
||||
this.frame_id = frame_id;
|
||||
}
|
||||
|
||||
public static class StampBean {
|
||||
/**
|
||||
* secs : 1506650141
|
||||
* nsecs : 455670567
|
||||
*/
|
||||
|
||||
private int secs;
|
||||
private int nsecs;
|
||||
|
||||
public int getSecs() {
|
||||
return secs;
|
||||
}
|
||||
|
||||
public void setSecs(int secs) {
|
||||
this.secs = secs;
|
||||
}
|
||||
|
||||
public int getNsecs() {
|
||||
return nsecs;
|
||||
}
|
||||
|
||||
public void setNsecs(int nsecs) {
|
||||
this.nsecs = nsecs;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
// {
|
||||
// "twist": {
|
||||
// "twist": {
|
||||
// "angular": {
|
||||
// "z": 0,
|
||||
// "y": 0,
|
||||
// "x": 0
|
||||
// },
|
||||
// "linear": {
|
||||
// "z": 0,
|
||||
// "y": 0,
|
||||
// "x": 0
|
||||
// }
|
||||
// },
|
||||
// "covariance": [
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0
|
||||
// ]
|
||||
// },
|
||||
// "child_frame_id": "base_footprint",
|
||||
// "pose": {
|
||||
// "covariance": [
|
||||
// 0.1,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0.1,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 1.7976931348623157E308,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 1.7976931348623157E308,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 1.7976931348623157E308,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0,
|
||||
// 0.2
|
||||
// ],
|
||||
// "pose": {
|
||||
// "position": {
|
||||
// "z": 0,
|
||||
// "y": -0.16137039724419636,
|
||||
// "x": 0.001470031525333453
|
||||
// },
|
||||
// "orientation": {
|
||||
// "w": -0.08271030624895466,
|
||||
// "z": -0.9965736326234024,
|
||||
// "y": -0,
|
||||
// "x": 0
|
||||
// }
|
||||
// }
|
||||
// },
|
||||
// "header": {
|
||||
// "seq": 69712,
|
||||
// "stamp": {
|
||||
// "secs": 1506650141,
|
||||
// "nsecs": 455670567
|
||||
// },
|
||||
// "frame_id": "odom"
|
||||
// }
|
||||
// }
|
||||
}
|
||||
@ -0,0 +1,246 @@
|
||||
package com.jilk.ros.nav;
|
||||
|
||||
/**
|
||||
* Created by super_yu on 27/10/2017.
|
||||
*/
|
||||
|
||||
public class Move_Base_Goal {
|
||||
/**
|
||||
* msg : {"target_pose":{"header":{"frame_id":"/map"},"pose":{"w":0.72508,"x":0.00455891,"y":0.0110448,"z":-0.688561}},"base_position":{"header":{"frame_id":"/map"},"pose":{"x":-5.56034,"y":0.543951,"z":2.96803E-4}}}
|
||||
* op : publish
|
||||
* topic : /move_base/goal
|
||||
*/
|
||||
|
||||
private MsgBean msg;
|
||||
private String op = "publish";
|
||||
private String topic = "/move_base/goal";
|
||||
|
||||
public MsgBean getMsg() {
|
||||
return msg;
|
||||
}
|
||||
|
||||
public void setMsg(MsgBean msg) {
|
||||
this.msg = msg;
|
||||
}
|
||||
|
||||
public String getOp() {
|
||||
return op;
|
||||
}
|
||||
|
||||
public String getTopic() {
|
||||
return topic;
|
||||
}
|
||||
|
||||
public static class MsgBean {
|
||||
/**
|
||||
* target_pose : {"header":{"frame_id":"/map"},"pose":{"w":0.72508,"x":0.00455891,"y":0.0110448,"z":-0.688561}}
|
||||
* base_position : {"header":{"frame_id":"/map"},"pose":{"x":-5.56034,"y":0.543951,"z":2.96803E-4}}
|
||||
*/
|
||||
|
||||
private TargetPoseBean target_pose;
|
||||
private BasePositionBean base_position;
|
||||
|
||||
public TargetPoseBean getTarget_pose() {
|
||||
return target_pose;
|
||||
}
|
||||
|
||||
public void setTarget_pose(TargetPoseBean target_pose) {
|
||||
this.target_pose = target_pose;
|
||||
}
|
||||
|
||||
public BasePositionBean getBase_position() {
|
||||
return base_position;
|
||||
}
|
||||
|
||||
public void setBase_position(BasePositionBean base_position) {
|
||||
this.base_position = base_position;
|
||||
}
|
||||
|
||||
public static class TargetPoseBean {
|
||||
/**
|
||||
* header : {"frame_id":"/map"}
|
||||
* pose : {"w":0.72508,"x":0.00455891,"y":0.0110448,"z":-0.688561}
|
||||
*/
|
||||
|
||||
private HeaderBean header;
|
||||
private PoseBean pose;
|
||||
|
||||
public HeaderBean getHeader() {
|
||||
return header;
|
||||
}
|
||||
|
||||
public void setHeader(HeaderBean header) {
|
||||
this.header = header;
|
||||
}
|
||||
|
||||
public PoseBean getPose() {
|
||||
return pose;
|
||||
}
|
||||
|
||||
public void setPose(PoseBean pose) {
|
||||
this.pose = pose;
|
||||
}
|
||||
|
||||
public static class HeaderBean {
|
||||
/**
|
||||
* frame_id : /map
|
||||
*/
|
||||
|
||||
private String frame_id = "/map";
|
||||
|
||||
public String getFrame_id() {
|
||||
return frame_id;
|
||||
}
|
||||
}
|
||||
|
||||
public static class PoseBean {
|
||||
/**
|
||||
* w : 0.72508
|
||||
* x : 0.00455891
|
||||
* y : 0.0110448
|
||||
* z : -0.688561
|
||||
*/
|
||||
|
||||
private double w;
|
||||
private double x;
|
||||
private double y;
|
||||
private double z;
|
||||
|
||||
public double getW() {
|
||||
return w;
|
||||
}
|
||||
|
||||
public void setW(double w) {
|
||||
this.w = w;
|
||||
}
|
||||
|
||||
public double getX() {
|
||||
return x;
|
||||
}
|
||||
|
||||
public void setX(double x) {
|
||||
this.x = x;
|
||||
}
|
||||
|
||||
public double getY() {
|
||||
return y;
|
||||
}
|
||||
|
||||
public void setY(double y) {
|
||||
this.y = y;
|
||||
}
|
||||
|
||||
public double getZ() {
|
||||
return z;
|
||||
}
|
||||
|
||||
public void setZ(double z) {
|
||||
this.z = z;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public static class BasePositionBean {
|
||||
/**
|
||||
* header : {"frame_id":"/map"}
|
||||
* pose : {"x":-5.56034,"y":0.543951,"z":2.96803E-4}
|
||||
*/
|
||||
|
||||
private HeaderBeanX header;
|
||||
private PoseBeanX pose;
|
||||
|
||||
public HeaderBeanX getHeader() {
|
||||
return header;
|
||||
}
|
||||
|
||||
public void setHeader(HeaderBeanX header) {
|
||||
this.header = header;
|
||||
}
|
||||
|
||||
public PoseBeanX getPose() {
|
||||
return pose;
|
||||
}
|
||||
|
||||
public void setPose(PoseBeanX pose) {
|
||||
this.pose = pose;
|
||||
}
|
||||
|
||||
public static class HeaderBeanX {
|
||||
/**
|
||||
* frame_id : /map
|
||||
*/
|
||||
|
||||
private String frame_id = "/map";
|
||||
|
||||
public String getFrame_id() {
|
||||
return frame_id;
|
||||
}
|
||||
}
|
||||
|
||||
public static class PoseBeanX {
|
||||
/**
|
||||
* x : -5.56034
|
||||
* y : 0.543951
|
||||
* z : 2.96803E-4
|
||||
*/
|
||||
|
||||
private double x;
|
||||
private double y;
|
||||
private double z;
|
||||
|
||||
public double getX() {
|
||||
return x;
|
||||
}
|
||||
|
||||
public void setX(double x) {
|
||||
this.x = x;
|
||||
}
|
||||
|
||||
public double getY() {
|
||||
return y;
|
||||
}
|
||||
|
||||
public void setY(double y) {
|
||||
this.y = y;
|
||||
}
|
||||
|
||||
public double getZ() {
|
||||
return z;
|
||||
}
|
||||
|
||||
public void setZ(double z) {
|
||||
this.z = z;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// {
|
||||
// "op": "publish",
|
||||
// "topic": "/move_base/goal",
|
||||
// "msg": {
|
||||
// "target_pose": {
|
||||
// "header": {
|
||||
// "frame_id": "/map"
|
||||
// },
|
||||
// "pose": {
|
||||
// "z": -0.695688,
|
||||
// "x": 0.000728705,
|
||||
// "w": 0.71834,
|
||||
// "y": 0.00213251
|
||||
// }
|
||||
// },
|
||||
// "base_position": {
|
||||
// "header": {
|
||||
// "frame_id": "/map"
|
||||
// },
|
||||
// "pose": {
|
||||
// "z": 0.00984454,
|
||||
// "x": -0.0483838,
|
||||
// "y": -0.00480753
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
}
|
||||
@ -0,0 +1,219 @@
|
||||
package com.jilk.ros.nav;
|
||||
|
||||
import java.util.List;
|
||||
|
||||
/**
|
||||
* Created by super_yu on 30/10/2017.
|
||||
*/
|
||||
|
||||
public class Move_Base_Status {
|
||||
/**
|
||||
* status_list : [{"goal_id":{"id":"map_6_A_603","stamp":{"secs":1509348841,"nsecs":490135242}},"text":"Goal reached.","status":3},{"goal_id":{"id":"map_6_A_601","stamp":{"secs":1509348875,"nsecs":650002258}},"text":"This goal has been accepted by the simple action server","status":1}]
|
||||
* header : {"seq":13516,"frame_id":"","stamp":{"secs":1509348876,"nsecs":156902566}}
|
||||
*/
|
||||
|
||||
private HeaderBean header;
|
||||
private List<StatusListBean> status_list;
|
||||
|
||||
public HeaderBean getHeader() {
|
||||
return header;
|
||||
}
|
||||
|
||||
public void setHeader(HeaderBean header) {
|
||||
this.header = header;
|
||||
}
|
||||
|
||||
public List<StatusListBean> getStatus_list() {
|
||||
return status_list;
|
||||
}
|
||||
|
||||
public void setStatus_list(List<StatusListBean> status_list) {
|
||||
this.status_list = status_list;
|
||||
}
|
||||
|
||||
public static class HeaderBean {
|
||||
/**
|
||||
* seq : 13516
|
||||
* frame_id :
|
||||
* stamp : {"secs":1509348876,"nsecs":156902566}
|
||||
*/
|
||||
|
||||
private int seq;
|
||||
private String frame_id;
|
||||
private StampBean stamp;
|
||||
|
||||
public int getSeq() {
|
||||
return seq;
|
||||
}
|
||||
|
||||
public void setSeq(int seq) {
|
||||
this.seq = seq;
|
||||
}
|
||||
|
||||
public String getFrame_id() {
|
||||
return frame_id;
|
||||
}
|
||||
|
||||
public void setFrame_id(String frame_id) {
|
||||
this.frame_id = frame_id;
|
||||
}
|
||||
|
||||
public StampBean getStamp() {
|
||||
return stamp;
|
||||
}
|
||||
|
||||
public void setStamp(StampBean stamp) {
|
||||
this.stamp = stamp;
|
||||
}
|
||||
|
||||
public static class StampBean {
|
||||
/**
|
||||
* secs : 1509348876
|
||||
* nsecs : 156902566
|
||||
*/
|
||||
|
||||
private int secs;
|
||||
private int nsecs;
|
||||
|
||||
public int getSecs() {
|
||||
return secs;
|
||||
}
|
||||
|
||||
public void setSecs(int secs) {
|
||||
this.secs = secs;
|
||||
}
|
||||
|
||||
public int getNsecs() {
|
||||
return nsecs;
|
||||
}
|
||||
|
||||
public void setNsecs(int nsecs) {
|
||||
this.nsecs = nsecs;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public static class StatusListBean {
|
||||
/**
|
||||
* goal_id : {"id":"map_6_A_603","stamp":{"secs":1509348841,"nsecs":490135242}}
|
||||
* text : Goal reached.
|
||||
* status : 3
|
||||
*/
|
||||
|
||||
private GoalIdBean goal_id;
|
||||
private String text;
|
||||
private int status;
|
||||
|
||||
public GoalIdBean getGoal_id() {
|
||||
return goal_id;
|
||||
}
|
||||
|
||||
public void setGoal_id(GoalIdBean goal_id) {
|
||||
this.goal_id = goal_id;
|
||||
}
|
||||
|
||||
public String getText() {
|
||||
return text;
|
||||
}
|
||||
|
||||
public void setText(String text) {
|
||||
this.text = text;
|
||||
}
|
||||
|
||||
public int getStatus() {
|
||||
return status;
|
||||
}
|
||||
|
||||
public void setStatus(int status) {
|
||||
this.status = status;
|
||||
}
|
||||
|
||||
public static class GoalIdBean {
|
||||
/**
|
||||
* id : map_6_A_603
|
||||
* stamp : {"secs":1509348841,"nsecs":490135242}
|
||||
*/
|
||||
|
||||
private String id;
|
||||
private StampBeanX stamp;
|
||||
|
||||
public String getId() {
|
||||
return id;
|
||||
}
|
||||
|
||||
public void setId(String id) {
|
||||
this.id = id;
|
||||
}
|
||||
|
||||
public StampBeanX getStamp() {
|
||||
return stamp;
|
||||
}
|
||||
|
||||
public void setStamp(StampBeanX stamp) {
|
||||
this.stamp = stamp;
|
||||
}
|
||||
|
||||
public static class StampBeanX {
|
||||
/**
|
||||
* secs : 1509348841
|
||||
* nsecs : 490135242
|
||||
*/
|
||||
|
||||
private int secs;
|
||||
private int nsecs;
|
||||
|
||||
public int getSecs() {
|
||||
return secs;
|
||||
}
|
||||
|
||||
public void setSecs(int secs) {
|
||||
this.secs = secs;
|
||||
}
|
||||
|
||||
public int getNsecs() {
|
||||
return nsecs;
|
||||
}
|
||||
|
||||
public void setNsecs(int nsecs) {
|
||||
this.nsecs = nsecs;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// {
|
||||
// "status_list": [
|
||||
// {
|
||||
// "goal_id": {
|
||||
// "id": "map_6_A_603",
|
||||
// "stamp": {
|
||||
// "secs": 1509348841,
|
||||
// "nsecs": 490135242
|
||||
// }
|
||||
// },
|
||||
// "text": "Goal reached.",
|
||||
// "status": 3
|
||||
// },
|
||||
// {
|
||||
// "goal_id": {
|
||||
// "id": "map_6_A_601",
|
||||
// "stamp": {
|
||||
// "secs": 1509348875,
|
||||
// "nsecs": 650002258
|
||||
// }
|
||||
// },
|
||||
// "text": "This goal has been accepted by the simple action server",
|
||||
// "status": 1
|
||||
// }
|
||||
// ],
|
||||
// "header": {
|
||||
// "seq": 13516,
|
||||
// "frame_id": "",
|
||||
// "stamp": {
|
||||
// "secs": 1509348876,
|
||||
// "nsecs": 156902566
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
}
|
||||
@ -0,0 +1,38 @@
|
||||
package com.jilk.ros.nav;
|
||||
|
||||
import java.util.HashMap;
|
||||
import java.util.List;
|
||||
import java.util.Map;
|
||||
|
||||
/**
|
||||
* 存储导航对象
|
||||
* Created by super_yu on 27/10/2017.
|
||||
*/
|
||||
|
||||
public class NavPublich {
|
||||
|
||||
private List<String> wayPointsNames;
|
||||
|
||||
private HashMap<String, TMove_Base_Goal> navPublishHashMap;
|
||||
|
||||
public List<String> getWayPointsNames() {
|
||||
return wayPointsNames;
|
||||
}
|
||||
|
||||
public void setWayPointsNames(List<String> wayPointsNames) {
|
||||
this.wayPointsNames = wayPointsNames;
|
||||
}
|
||||
|
||||
public HashMap<String, TMove_Base_Goal> getNavPublishHashMap() {
|
||||
return navPublishHashMap;
|
||||
}
|
||||
|
||||
public void setNavPublishHashMap(HashMap<String, TMove_Base_Goal> navPublishHashMap) {
|
||||
this.navPublishHashMap = navPublishHashMap;
|
||||
}
|
||||
|
||||
public void clear() {
|
||||
if (wayPointsNames != null) wayPointsNames.clear();
|
||||
if (navPublishHashMap != null) navPublishHashMap.clear();
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,402 @@
|
||||
package com.jilk.ros.nav;
|
||||
|
||||
/**
|
||||
* Created by super_yu on 30/10/2017.
|
||||
*/
|
||||
|
||||
public class TMove_Base_Goal {
|
||||
/**
|
||||
* op : publish
|
||||
* topic : /move_base/goal
|
||||
* msg : {"header":{"seq":0,"stamp":{"secs":0,"nsecs":0},"frame_id":"map"},"goal_id":{"stamp":{"secs":0,"nsecs":0},"id":"map_6_A_601"},"goal":{"target_pose":{"header":{"seq":0,"stamp":{"secs":0,"nsecs":0},"frame_id":"map"},"pose":{"position":{"z":2.96803E-4,"x":-5.56034,"y":0.543951},"orientation":{"z":-0.688561,"x":0.00455891,"w":0.72508,"y":0.0110448}}}}}
|
||||
*/
|
||||
private String op = "publish";
|
||||
private String topic = "/move_base/goal";
|
||||
private MsgBean msg;
|
||||
|
||||
public String getOp() {
|
||||
return op;
|
||||
}
|
||||
|
||||
public String getTopic() {
|
||||
return topic;
|
||||
}
|
||||
|
||||
public MsgBean getMsg() {
|
||||
return msg;
|
||||
}
|
||||
|
||||
public void setMsg(MsgBean msg) {
|
||||
this.msg = msg;
|
||||
}
|
||||
|
||||
public static class MsgBean {
|
||||
/**
|
||||
* header : {"seq":0,"stamp":{"secs":0,"nsecs":0},"frame_id":"map"}
|
||||
* goal_id : {"stamp":{"secs":0,"nsecs":0},"id":"map_6_A_601"}
|
||||
* goal : {"target_pose":{"header":{"seq":0,"stamp":{"secs":0,"nsecs":0},"frame_id":"map"},"pose":{"position":{"z":2.96803E-4,"x":-5.56034,"y":0.543951},"orientation":{"z":-0.688561,"x":0.00455891,"w":0.72508,"y":0.0110448}}}}
|
||||
*/
|
||||
|
||||
private HeaderBean header;
|
||||
private GoalIdBean goal_id;
|
||||
private GoalBean goal;
|
||||
|
||||
public HeaderBean getHeader() {
|
||||
return header;
|
||||
}
|
||||
|
||||
public void setHeader(HeaderBean header) {
|
||||
this.header = header;
|
||||
}
|
||||
|
||||
public GoalIdBean getGoal_id() {
|
||||
return goal_id;
|
||||
}
|
||||
|
||||
public void setGoal_id(GoalIdBean goal_id) {
|
||||
this.goal_id = goal_id;
|
||||
}
|
||||
|
||||
public GoalBean getGoal() {
|
||||
return goal;
|
||||
}
|
||||
|
||||
public void setGoal(GoalBean goal) {
|
||||
this.goal = goal;
|
||||
}
|
||||
|
||||
public static class HeaderBean {
|
||||
/**
|
||||
* seq : 0
|
||||
* stamp : {"secs":0,"nsecs":0}
|
||||
* frame_id : map
|
||||
*/
|
||||
|
||||
private int seq = 0;
|
||||
private StampBean stamp;
|
||||
private String frame_id = "map";
|
||||
|
||||
public int getSeq() {
|
||||
return seq;
|
||||
}
|
||||
|
||||
public StampBean getStamp() {
|
||||
return stamp;
|
||||
}
|
||||
|
||||
public void setStamp(StampBean stamp) {
|
||||
this.stamp = stamp;
|
||||
}
|
||||
|
||||
public String getFrame_id() {
|
||||
return frame_id;
|
||||
}
|
||||
|
||||
public static class StampBean {
|
||||
/**
|
||||
* secs : 0
|
||||
* nsecs : 0
|
||||
*/
|
||||
|
||||
private int secs = 0;
|
||||
private int nsecs = 0;
|
||||
|
||||
public int getSecs() {
|
||||
return secs;
|
||||
}
|
||||
|
||||
public int getNsecs() {
|
||||
return nsecs;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public static class GoalIdBean {
|
||||
/**
|
||||
* stamp : {"secs":0,"nsecs":0}
|
||||
* id : map_6_A_601
|
||||
*/
|
||||
|
||||
private StampBeanX stamp;
|
||||
|
||||
private String id;
|
||||
|
||||
public StampBeanX getStamp() {
|
||||
return stamp;
|
||||
}
|
||||
|
||||
public void setStamp(StampBeanX stamp) {
|
||||
this.stamp = stamp;
|
||||
}
|
||||
|
||||
public void setId(String id) {
|
||||
this.id = id;
|
||||
}
|
||||
|
||||
public String getId() {
|
||||
return id;
|
||||
}
|
||||
|
||||
public static class StampBeanX {
|
||||
/**
|
||||
* secs : 0
|
||||
* nsecs : 0
|
||||
*/
|
||||
|
||||
private int secs = 0;
|
||||
private int nsecs = 0;
|
||||
|
||||
public int getSecs() {
|
||||
return secs;
|
||||
}
|
||||
|
||||
public int getNsecs() {
|
||||
return nsecs;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public static class GoalBean {
|
||||
/**
|
||||
* target_pose : {"header":{"seq":0,"stamp":{"secs":0,"nsecs":0},"frame_id":"map"},"pose":{"position":{"z":2.96803E-4,"x":-5.56034,"y":0.543951},"orientation":{"z":-0.688561,"x":0.00455891,"w":0.72508,"y":0.0110448}}}
|
||||
*/
|
||||
|
||||
private TargetPoseBean target_pose;
|
||||
|
||||
public TargetPoseBean getTarget_pose() {
|
||||
return target_pose;
|
||||
}
|
||||
|
||||
public void setTarget_pose(TargetPoseBean target_pose) {
|
||||
this.target_pose = target_pose;
|
||||
}
|
||||
|
||||
public static class TargetPoseBean {
|
||||
/**
|
||||
* header : {"seq":0,"stamp":{"secs":0,"nsecs":0},"frame_id":"map"}
|
||||
* pose : {"position":{"z":2.96803E-4,"x":-5.56034,"y":0.543951},"orientation":{"z":-0.688561,"x":0.00455891,"w":0.72508,"y":0.0110448}}
|
||||
*/
|
||||
|
||||
private HeaderBeanX header;
|
||||
private PoseBean pose;
|
||||
|
||||
public HeaderBeanX getHeader() {
|
||||
return header;
|
||||
}
|
||||
|
||||
public void setHeader(HeaderBeanX header) {
|
||||
this.header = header;
|
||||
}
|
||||
|
||||
public PoseBean getPose() {
|
||||
return pose;
|
||||
}
|
||||
|
||||
public void setPose(PoseBean pose) {
|
||||
this.pose = pose;
|
||||
}
|
||||
|
||||
public static class HeaderBeanX {
|
||||
/**
|
||||
* seq : 0
|
||||
* stamp : {"secs":0,"nsecs":0}
|
||||
* frame_id : map
|
||||
*/
|
||||
|
||||
private int seq = 0;
|
||||
private StampBeanXX stamp;
|
||||
private String frame_id = "map";
|
||||
|
||||
public int getSeq() {
|
||||
return seq;
|
||||
}
|
||||
|
||||
public StampBeanXX getStamp() {
|
||||
return stamp;
|
||||
}
|
||||
|
||||
public void setStamp(StampBeanXX stamp) {
|
||||
this.stamp = stamp;
|
||||
}
|
||||
|
||||
public String getFrame_id() {
|
||||
return frame_id;
|
||||
}
|
||||
|
||||
public static class StampBeanXX {
|
||||
/**
|
||||
* secs : 0
|
||||
* nsecs : 0
|
||||
*/
|
||||
|
||||
private int secs = 0;
|
||||
private int nsecs = 0;
|
||||
|
||||
public int getSecs() {
|
||||
return secs;
|
||||
}
|
||||
|
||||
public int getNsecs() {
|
||||
return nsecs;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public static class PoseBean {
|
||||
/**
|
||||
* position : {"z":2.96803E-4,"x":-5.56034,"y":0.543951}
|
||||
* orientation : {"z":-0.688561,"x":0.00455891,"w":0.72508,"y":0.0110448}
|
||||
*/
|
||||
|
||||
private PositionBean position;
|
||||
private OrientationBean orientation;
|
||||
|
||||
public PositionBean getPosition() {
|
||||
return position;
|
||||
}
|
||||
|
||||
public void setPosition(PositionBean position) {
|
||||
this.position = position;
|
||||
}
|
||||
|
||||
public OrientationBean getOrientation() {
|
||||
return orientation;
|
||||
}
|
||||
|
||||
public void setOrientation(OrientationBean orientation) {
|
||||
this.orientation = orientation;
|
||||
}
|
||||
|
||||
public static class PositionBean {
|
||||
/**
|
||||
* z : 2.96803E-4
|
||||
* x : -5.56034
|
||||
* y : 0.543951
|
||||
*/
|
||||
|
||||
private double z;
|
||||
private double x;
|
||||
private double y;
|
||||
|
||||
public double getZ() {
|
||||
return z;
|
||||
}
|
||||
|
||||
public void setZ(double z) {
|
||||
this.z = z;
|
||||
}
|
||||
|
||||
public double getX() {
|
||||
return x;
|
||||
}
|
||||
|
||||
public void setX(double x) {
|
||||
this.x = x;
|
||||
}
|
||||
|
||||
public double getY() {
|
||||
return y;
|
||||
}
|
||||
|
||||
public void setY(double y) {
|
||||
this.y = y;
|
||||
}
|
||||
}
|
||||
|
||||
public static class OrientationBean {
|
||||
/**
|
||||
* z : -0.688561
|
||||
* x : 0.00455891
|
||||
* w : 0.72508
|
||||
* y : 0.0110448
|
||||
*/
|
||||
|
||||
private double z;
|
||||
private double x;
|
||||
private double w;
|
||||
private double y;
|
||||
|
||||
public double getZ() {
|
||||
return z;
|
||||
}
|
||||
|
||||
public void setZ(double z) {
|
||||
this.z = z;
|
||||
}
|
||||
|
||||
public double getX() {
|
||||
return x;
|
||||
}
|
||||
|
||||
public void setX(double x) {
|
||||
this.x = x;
|
||||
}
|
||||
|
||||
public double getW() {
|
||||
return w;
|
||||
}
|
||||
|
||||
public void setW(double w) {
|
||||
this.w = w;
|
||||
}
|
||||
|
||||
public double getY() {
|
||||
return y;
|
||||
}
|
||||
|
||||
public void setY(double y) {
|
||||
this.y = y;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// {
|
||||
// "op": "publish",
|
||||
// "topic": "/move_base/goal",
|
||||
// "msg": {
|
||||
// "header": {
|
||||
// "seq": 0,
|
||||
// "stamp": {
|
||||
// "secs": 0,
|
||||
// "nsecs": 0
|
||||
// },
|
||||
// "frame_id": "map"
|
||||
// },
|
||||
// "goal_id": {
|
||||
// "stamp": {
|
||||
// "secs": 0,
|
||||
// "nsecs": 0
|
||||
// },
|
||||
// "id": "map_6_A_601"
|
||||
// },
|
||||
// "goal": {
|
||||
// "target_pose": {
|
||||
// "header": {
|
||||
// "seq": 0,
|
||||
// "stamp": {
|
||||
// "secs": 0,
|
||||
// "nsecs": 0
|
||||
// },
|
||||
// "frame_id": "map"
|
||||
// },
|
||||
// "pose": {
|
||||
// "position": {
|
||||
// "z": 0.000296803,
|
||||
// "x": -5.56034,
|
||||
// "y": 0.543951
|
||||
// },
|
||||
// "orientation": {
|
||||
// "z": -0.688561,
|
||||
// "x": 0.00455891,
|
||||
// "w": 0.72508,
|
||||
// "y": 0.0110448
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
// }
|
||||
}
|
||||
@ -0,0 +1,22 @@
|
||||
package com.jilk.ros.net;
|
||||
|
||||
import android.content.BroadcastReceiver;
|
||||
import android.content.Context;
|
||||
import android.content.Intent;
|
||||
import android.net.ConnectivityManager;
|
||||
|
||||
|
||||
/**
|
||||
* Created by Administrator on 2017/5/22 0022.
|
||||
*/
|
||||
|
||||
public class NetBroadcastReceiver extends BroadcastReceiver {
|
||||
@Override
|
||||
public void onReceive(Context context, Intent intent) {
|
||||
// TODO Auto-generated method stub
|
||||
// 如果相等的话就说明网络状态发生了变化
|
||||
if (intent.getAction().equals(ConnectivityManager.CONNECTIVITY_ACTION)) {
|
||||
int netWorkState = NetUtil.getNetWorkState(context);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,49 @@
|
||||
package com.jilk.ros.net;
|
||||
|
||||
import android.content.Context;
|
||||
import android.net.ConnectivityManager;
|
||||
import android.net.NetworkInfo;
|
||||
|
||||
/**
|
||||
* Created by Administrator on 2017/5/22 0022.
|
||||
*/
|
||||
|
||||
public class NetUtil {
|
||||
|
||||
/**
|
||||
* 没有连接网络
|
||||
*/
|
||||
private static final int NETWORK_NONE = -1;
|
||||
/**
|
||||
* 移动网络
|
||||
*/
|
||||
private static final int NETWORK_MOBILE = 0;
|
||||
/**
|
||||
* 无线网络
|
||||
*/
|
||||
private static final int NETWORK_WIFI = 1;
|
||||
|
||||
public static int getNetWorkState(Context context) {
|
||||
// 得到连接管理器对象
|
||||
ConnectivityManager connectivityManager = (ConnectivityManager) context
|
||||
.getSystemService(Context.CONNECTIVITY_SERVICE);
|
||||
|
||||
NetworkInfo activeNetworkInfo = connectivityManager
|
||||
.getActiveNetworkInfo();
|
||||
if (activeNetworkInfo != null && activeNetworkInfo.isConnected()) {
|
||||
|
||||
if (activeNetworkInfo.getType() == (ConnectivityManager.TYPE_WIFI)) {
|
||||
return NETWORK_WIFI;
|
||||
} else if (activeNetworkInfo.getType() == (ConnectivityManager.TYPE_MOBILE)) {
|
||||
return NETWORK_MOBILE;
|
||||
}
|
||||
} else {
|
||||
return NETWORK_NONE;
|
||||
}
|
||||
return NETWORK_NONE;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
File diff suppressed because one or more lines are too long
@ -0,0 +1,18 @@
|
||||
package com.example.testdemo;
|
||||
|
||||
import org.springframework.boot.SpringApplication;
|
||||
import org.springframework.boot.autoconfigure.SpringBootApplication;
|
||||
|
||||
|
||||
@SpringBootApplication
|
||||
public class ServerApplication {
|
||||
|
||||
public static void main(String[] args) {
|
||||
|
||||
SpringApplication.run(ServerApplication.class, args);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -1,42 +0,0 @@
|
||||
package com.example.testdemo;
|
||||
|
||||
import com.alibaba.fastjson.JSON;
|
||||
import com.alibaba.fastjson.JSONObject;
|
||||
import com.sun.javafx.webkit.EventLoopImpl;
|
||||
import org.openqa.selenium.*;
|
||||
import org.openqa.selenium.chrome.ChromeDriver;
|
||||
import org.openqa.selenium.chrome.ChromeOptions;
|
||||
import org.springframework.boot.SpringApplication;
|
||||
import org.springframework.boot.autoconfigure.SpringBootApplication;
|
||||
import org.springframework.scheduling.annotation.EnableScheduling;
|
||||
import org.springframework.scheduling.annotation.Scheduled;
|
||||
import org.springframework.stereotype.Controller;
|
||||
import org.springframework.web.bind.annotation.GetMapping;
|
||||
import org.springframework.web.bind.annotation.RestController;
|
||||
|
||||
import javax.script.ScriptEngine;
|
||||
import javax.script.ScriptEngineManager;
|
||||
import javax.script.ScriptException;
|
||||
import java.io.BufferedReader;
|
||||
import java.io.File;
|
||||
import java.io.FileNotFoundException;
|
||||
import java.io.FileReader;
|
||||
import java.sql.Driver;
|
||||
import java.util.Arrays;
|
||||
import java.util.List;
|
||||
import java.util.Random;
|
||||
import java.util.concurrent.ConcurrentLinkedQueue;
|
||||
|
||||
|
||||
@SpringBootApplication
|
||||
public class TestdemoApplication {
|
||||
|
||||
public static void main(String[] args) {
|
||||
|
||||
SpringApplication.run(TestdemoApplication.class, args);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -0,0 +1,57 @@
|
||||
package com.example.testdemo.ros;
|
||||
|
||||
|
||||
public class Publisher {
|
||||
|
||||
protected String topic;
|
||||
protected String msgType;
|
||||
protected int rosBridge;
|
||||
|
||||
|
||||
public Publisher(String topic, String msgType, int rosBridge){
|
||||
this.topic = topic;
|
||||
this.msgType = msgType;
|
||||
this.rosBridge = rosBridge;
|
||||
|
||||
//this.rosBridge.advertise(this.topic, this.msgType);
|
||||
|
||||
}
|
||||
|
||||
|
||||
public Publisher(String topic, String msgType, int rosBridge, boolean advertiseNow){
|
||||
this.topic = topic;
|
||||
this.msgType = msgType;
|
||||
this.rosBridge = rosBridge;
|
||||
|
||||
if(advertiseNow) {
|
||||
//this.rosBridge.advertise(this.topic, this.msgType);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
public String getTopic() {
|
||||
return topic;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Returns the ROS message type of the topic to which this object publishes.
|
||||
* @return the ROS message type of the topic to which this object publishes.
|
||||
*/
|
||||
public String getMsgType() {
|
||||
return msgType;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Returns the {@link ros.RosBridge} object that manages the connection to the ROS Bridge server.
|
||||
* @return the {@link ros.RosBridge} object that manages the connection to the ROS Bridge server.
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* Unadvertises that you are publishing to the topic.
|
||||
*/
|
||||
}
|
||||
@ -0,0 +1,85 @@
|
||||
package com.example.testdemo.ros;
|
||||
|
||||
import com.fasterxml.jackson.core.JsonFactory;
|
||||
import com.fasterxml.jackson.core.JsonParseException;
|
||||
import com.fasterxml.jackson.core.type.TypeReference;
|
||||
import com.fasterxml.jackson.databind.JsonNode;
|
||||
import com.fasterxml.jackson.databind.ObjectMapper;
|
||||
|
||||
import java.io.IOException;
|
||||
import java.util.HashMap;
|
||||
import java.util.Map;
|
||||
|
||||
/**
|
||||
* This is a delegate interface for handling ros topic subscriptions. The {@link #receive(JsonNode, String)}
|
||||
* is called every time the topic with which this delegate is associated has a new message published.
|
||||
* The JSON data, given as a {@link JsonNode}, has four top-level fields:<p>
|
||||
* op: what kind of operation it was; should always be "publish"<p>
|
||||
* topic: to which topic the message was published<p>
|
||||
* type: the ROS message type of the topic<p>
|
||||
* msg: the provided ros message in JSON format. This the primary field you will work with.<p>
|
||||
* There are generally two ways you can parse the message into a more usable Java object. The first involves manually
|
||||
* iterating through the JSON fields of the msg. For example,
|
||||
* for a geometry_msgs/Twist message, you can get out the linear x value as follows:<p>
|
||||
* <code>double x = data.get("msg").get("linear").get("x").asDouble();</code><p>
|
||||
* (If an element is na array, JSON methods exist for handling it such as {@link JsonNode#get(int)}
|
||||
* and {@link JsonNode#size()}). The the other way is to let the Jackson library unpack
|
||||
* it into a JavaBean. The {@link ros.tools.MessageUnpacker} class further streamlines this process. See its documentation
|
||||
* for more information.
|
||||
* @author James MacGlashan.
|
||||
*/
|
||||
public interface RosListenDelegate {
|
||||
|
||||
/**
|
||||
* Receives a new published message to a subscribed topic. The JSON data, given as a {@link JsonNode}, has four top-level fields:<p>
|
||||
* op: what kind of operation it was; should always be "publish"<p>
|
||||
* topic: to which topic the message was published<p>
|
||||
* type: the ROS message type of the topic<p>
|
||||
* msg: the provided ros message in JSON format. This the primary field you will work with.<p>
|
||||
* This method also receives the full string representation of the received JSON message from ROSBridge.
|
||||
* @param data the {@link JsonNode} containing the JSON data received.
|
||||
* @param stringRep the string representation of the JSON object.
|
||||
*/
|
||||
void receive(JsonNode data, String stringRep);
|
||||
|
||||
|
||||
/**
|
||||
* A class for easy conversion to the legacy java_rosbridge {@link #receive(JsonNode, String)}
|
||||
* message format that presented the JSON data
|
||||
* in a {@link Map} from {@link String} to {@link Object} instances
|
||||
* in which the values were ether primitives, {@link Map} objects themselves, or {@link java.util.List}
|
||||
* objects.
|
||||
*/
|
||||
public static class LegacyFormat{
|
||||
|
||||
/**
|
||||
* A method for easy conversion to the legacy java_rosbridge {@link #receive(JsonNode, String)}
|
||||
* message format that presented the JSON data
|
||||
* in a {@link Map} from {@link String} to {@link Object} instances
|
||||
* in which the values were ether primitives, {@link Map} objects themselves, or {@link java.util.List}
|
||||
* objects.
|
||||
* @param jsonString the source JSON string message that was received
|
||||
* @return a {@link Map} data structure of the JSON data.
|
||||
*/
|
||||
public static Map<String, Object> legacyFormat(String jsonString){
|
||||
|
||||
JsonFactory jsonFactory = new JsonFactory();
|
||||
Map<String, Object> messageData = new HashMap<String, Object>();
|
||||
try {
|
||||
ObjectMapper objectMapper = new ObjectMapper(jsonFactory);
|
||||
TypeReference<Map<String, Object>> listTypeRef =
|
||||
new TypeReference<Map<String, Object>>() {};
|
||||
messageData = objectMapper.readValue(jsonString, listTypeRef);
|
||||
} catch (JsonParseException e) {
|
||||
e.printStackTrace();
|
||||
} catch (IOException e) {
|
||||
e.printStackTrace();
|
||||
}
|
||||
|
||||
return messageData;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
@ -0,0 +1,118 @@
|
||||
package com.example.testdemo.ros;
|
||||
|
||||
import com.fasterxml.jackson.core.JsonProcessingException;
|
||||
import com.fasterxml.jackson.databind.ObjectMapper;
|
||||
|
||||
import java.util.HashMap;
|
||||
import java.util.Map;
|
||||
|
||||
/**
|
||||
* A Subscription request builder. Supports all Rosbridge protocol fields for
|
||||
* a subscription request except png compression. Requires a topic to be set,
|
||||
* with the rest of the optional values set with setter methods that return
|
||||
* the this object so that you can chain settters on a single line. Use
|
||||
* the static {@link #generate(String)} to start the sequence. If a value is set
|
||||
* to null, it will be removed from the message.
|
||||
* <p>
|
||||
* When everything is set, the JSON message can be retrieved with the {@link #generateJsonString()}
|
||||
* @author James MacGlashan.
|
||||
*/
|
||||
public class SubscriptionRequestMsg {
|
||||
|
||||
protected Map<String, Object> keyValues = new HashMap<String, Object>(7);
|
||||
|
||||
public static SubscriptionRequestMsg generate(String topic){
|
||||
return new SubscriptionRequestMsg(topic);
|
||||
}
|
||||
|
||||
public SubscriptionRequestMsg(String topic){
|
||||
if(topic == null){
|
||||
throw new RuntimeException("ROS topic cannot be null in subscription request.");
|
||||
}
|
||||
keyValues.put("op", "subscribe");
|
||||
keyValues.put("topic", topic);
|
||||
}
|
||||
|
||||
public SubscriptionRequestMsg setTopic(String topic){
|
||||
this.setKeyValue("topic", topic);
|
||||
return this;
|
||||
}
|
||||
|
||||
public SubscriptionRequestMsg setType(String type){
|
||||
this.setKeyValue("type", type);
|
||||
return this;
|
||||
}
|
||||
|
||||
public SubscriptionRequestMsg setThrottleRate(Integer throttleRate){
|
||||
this.setKeyValue("throttle_rate", throttleRate);
|
||||
return this;
|
||||
}
|
||||
|
||||
public SubscriptionRequestMsg setQueueLength(Integer queueLength){
|
||||
this.setKeyValue("queue_length", queueLength);
|
||||
return this;
|
||||
}
|
||||
|
||||
public SubscriptionRequestMsg setFragmentSize(Integer fragmentSize){
|
||||
this.setKeyValue("fragment_size", fragmentSize);
|
||||
return this;
|
||||
}
|
||||
|
||||
|
||||
public SubscriptionRequestMsg setId(String id){
|
||||
this.setKeyValue("id", id);
|
||||
return this;
|
||||
}
|
||||
|
||||
public String getTopic(){
|
||||
return (String)this.keyValues.get("topic");
|
||||
}
|
||||
|
||||
public String getType(){
|
||||
return (String)this.keyValues.get("type");
|
||||
}
|
||||
|
||||
public Integer getThrottleRate(){
|
||||
return (Integer)this.keyValues.get("throttle_rate");
|
||||
}
|
||||
|
||||
public Integer getQueueLength(){
|
||||
return (Integer)this.keyValues.get("queue_length");
|
||||
}
|
||||
|
||||
public Integer getFragmentSize(){
|
||||
return (Integer)this.keyValues.get("fragment_size");
|
||||
}
|
||||
|
||||
|
||||
public String getId(){
|
||||
return (String)this.keyValues.get("id");
|
||||
}
|
||||
|
||||
/**
|
||||
* Generates the JSON string for this subscription request.
|
||||
* @return the JSON string for this subscription request.
|
||||
*/
|
||||
public String generateJsonString(){
|
||||
ObjectMapper mapper = new ObjectMapper();
|
||||
String jsonString = null;
|
||||
try {
|
||||
jsonString = mapper.writeValueAsString(this.keyValues);
|
||||
} catch(JsonProcessingException e) {
|
||||
e.printStackTrace();
|
||||
}
|
||||
|
||||
return jsonString;
|
||||
}
|
||||
|
||||
|
||||
protected void setKeyValue(String key, Object value){
|
||||
if(value == null){
|
||||
this.keyValues.remove(key);
|
||||
}
|
||||
else{
|
||||
this.keyValues.put(key, value);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
@ -0,0 +1,19 @@
|
||||
package com.example.testdemo.ros.msgs.geometry_msgs;
|
||||
|
||||
/**
|
||||
* A Java Bean for the Vector3 ROS geometry_msgs/Twist message type. This can be used both for publishing Twist messages to
|
||||
* {@link ros.RosBridge} and unpacking Twist messages received from {@link ros.RosBridge} (see the {@link ros.tools.MessageUnpacker}
|
||||
* documentation for how to easily unpack a ROS Bridge message into a Java object).
|
||||
* @author James MacGlashan.
|
||||
*/
|
||||
public class Twist {
|
||||
public Vector3 linear = new Vector3();
|
||||
public Vector3 angular = new Vector3();
|
||||
|
||||
public Twist(){}
|
||||
|
||||
public Twist(Vector3 linear, Vector3 angular) {
|
||||
this.linear = linear;
|
||||
this.angular = angular;
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,21 @@
|
||||
package com.example.testdemo.ros.msgs.geometry_msgs;
|
||||
|
||||
/**
|
||||
* A Java Bean for the Vector3 ROS geometry_msgs/Vector3 message type. This can be used both for publishing Vector3 messages to
|
||||
* {@link ros.RosBridge} and unpacking Vector3 messages received from {@link ros.RosBridge} (see the {@link ros.tools.MessageUnpacker}
|
||||
* documentation for how to easily unpack a ROS Bridge message into a Java object).
|
||||
* @author James MacGlashan.
|
||||
*/
|
||||
public class Vector3 {
|
||||
public double x;
|
||||
public double y;
|
||||
public double z;
|
||||
|
||||
public Vector3(){}
|
||||
|
||||
public Vector3(double x, double y, double z) {
|
||||
this.x = x;
|
||||
this.y = y;
|
||||
this.z = z;
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,125 @@
|
||||
package com.example.testdemo.ros.msgs.sensor_msgs;
|
||||
|
||||
import com.example.testdemo.ros.message.Header;
|
||||
|
||||
import java.awt.image.BufferedImage;
|
||||
|
||||
/**
|
||||
* Implementation of ROS sensor_msgs/Image.msg:
|
||||
* <a href="http://docs.ros.org/api/sensor_msgs/html/msg/Image.html">http://docs.ros.org/api/sensor_msgs/html/msg/Image.html</a>.
|
||||
* This class can also decode the ROS Image into a Java Buffered Image for images that are encoded in either
|
||||
* bgr8, rgb8, or mono8, by using the {@link #toBufferedImage()} method.
|
||||
* @author James MacGlashan.
|
||||
*/
|
||||
public class Image {
|
||||
|
||||
public Header header;
|
||||
public int height;
|
||||
public int width;
|
||||
public String encoding;
|
||||
public int is_bigendian;
|
||||
public int step;
|
||||
public byte[] data;
|
||||
|
||||
public Image() {
|
||||
}
|
||||
|
||||
public Image(Header header, int height, int width, String encoding, int is_bigendian, int step, byte[] data) {
|
||||
this.header = header;
|
||||
this.height = height;
|
||||
this.width = width;
|
||||
this.encoding = encoding;
|
||||
this.is_bigendian = is_bigendian;
|
||||
this.step = step;
|
||||
this.data = data;
|
||||
}
|
||||
|
||||
/**
|
||||
* Constructs a {@link BufferedImage} from this ROS Image, provided the encoding is either rgb8, bgr8, or mono8.
|
||||
* If it is not one of those encodings, then a runtime exception will be thrown.
|
||||
* @return a {@link BufferedImage} representation of this image.
|
||||
*/
|
||||
public BufferedImage toBufferedImage(){
|
||||
|
||||
if(this.encoding.equals("bgr8") || this.encoding.equals("rgb8")){
|
||||
return this.toBufferedImageFromRGB8();
|
||||
}
|
||||
else if(this.encoding.equals("mono8")){
|
||||
return this.toBufferedImageFromMono8();
|
||||
}
|
||||
|
||||
|
||||
throw new RuntimeException("ROS Image does not currently decode " + this.encoding + ". See Java doc for support types.");
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Constructs a {@link BufferedImage} from this ROS Image assuming the encoding is mono8
|
||||
* @return a {@link BufferedImage} representation of this image.
|
||||
*/
|
||||
protected BufferedImage toBufferedImageFromMono8(){
|
||||
int w = this.width;
|
||||
int h = this.height;
|
||||
|
||||
BufferedImage i = new BufferedImage(w, h, BufferedImage.TYPE_INT_RGB);
|
||||
for (int y = 0; y < h; ++y) {
|
||||
for(int x = 0; x < w; ++x) {
|
||||
|
||||
//row major
|
||||
int index = (y * w) + x;
|
||||
// combine to RGB format
|
||||
int anded = data[index++] & 0xFF;
|
||||
int rgb = anded |
|
||||
(anded << 8) |
|
||||
(anded << 16) |
|
||||
0xFF000000;
|
||||
|
||||
i.setRGB(x, y, rgb);
|
||||
}
|
||||
}
|
||||
|
||||
return i;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Constructs a {@link BufferedImage} representation from this ROS Image assuming the encoding is either rgb8 or bgr8.
|
||||
* @return a {@link BufferedImage} representation of this image.
|
||||
*/
|
||||
protected BufferedImage toBufferedImageFromRGB8(){
|
||||
|
||||
int w = this.width;
|
||||
int h = this.height;
|
||||
|
||||
BufferedImage i = new BufferedImage(w, h, BufferedImage.TYPE_INT_RGB);
|
||||
for (int y = 0; y < h; ++y) {
|
||||
for (int x = 0; x < w; ++x) {
|
||||
|
||||
//row major, consecutive channels
|
||||
int index = (y * w * 3) + (x * 3);
|
||||
// combine to RGB format
|
||||
int rgb;
|
||||
if(this.encoding.equals("bgr8")){
|
||||
rgb = ((data[index++] & 0xFF)) |
|
||||
((data[index++] & 0xFF) << 8) |
|
||||
((data[index++] & 0xFF) << 16) |
|
||||
0xFF000000;
|
||||
}
|
||||
else if(this.encoding.equals("rgb8")){
|
||||
rgb = ((data[index++] & 0xFF) << 16) |
|
||||
((data[index++] & 0xFF) << 8) |
|
||||
((data[index++] & 0xFF)) |
|
||||
0xFF000000;
|
||||
}
|
||||
else{
|
||||
throw new RuntimeException("ROS Image toBufferedImageFromRGB8 does not decode " + this.encoding);
|
||||
}
|
||||
i.setRGB(x, y, rgb);
|
||||
}
|
||||
}
|
||||
|
||||
return i;
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
@ -0,0 +1,21 @@
|
||||
package com.example.testdemo.ros.msgs.std_msgs;
|
||||
|
||||
/**
|
||||
* @author James MacGlashan.
|
||||
*/
|
||||
public class Header {
|
||||
|
||||
public int seq;
|
||||
public Time stamp;
|
||||
public String frame_id;
|
||||
|
||||
public Header() {
|
||||
}
|
||||
|
||||
public Header(int seq, Time stamp, String frame_id) {
|
||||
this.seq = seq;
|
||||
this.stamp = stamp;
|
||||
this.frame_id = frame_id;
|
||||
}
|
||||
|
||||
}
|
||||
@ -0,0 +1,12 @@
|
||||
package com.example.testdemo.ros.msgs.std_msgs;
|
||||
|
||||
/**
|
||||
* A generic specified Java Bean for capturing many of the primitive data-type messages used by ROS in the std_msgs
|
||||
* package. The class has a single public data member called "data" that belongs to the specified primitive type.
|
||||
* @author James MacGlashan.
|
||||
*/
|
||||
public class PrimitiveMsg <T> {
|
||||
public T data;
|
||||
public PrimitiveMsg(){}
|
||||
public PrimitiveMsg(T data){this.data = data;}
|
||||
}
|
||||
@ -0,0 +1,17 @@
|
||||
package com.example.testdemo.ros.msgs.std_msgs;
|
||||
|
||||
/**
|
||||
* @author James MacGlashan.
|
||||
*/
|
||||
public class Time {
|
||||
public int secs;
|
||||
public int nsecs;
|
||||
|
||||
public Time() {
|
||||
}
|
||||
|
||||
public Time(int secs, int nsecs) {
|
||||
this.secs = secs;
|
||||
this.nsecs = nsecs;
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,61 @@
|
||||
package com.example.testdemo.ros.tools;
|
||||
|
||||
import com.fasterxml.jackson.core.JsonProcessingException;
|
||||
import com.fasterxml.jackson.databind.JsonNode;
|
||||
import com.fasterxml.jackson.databind.ObjectMapper;
|
||||
|
||||
/**
|
||||
* This is a helper class that may be used for taking a JSON message returned from Ros bridge, and unpacking
|
||||
* the actual ros message inside it into a Java bean object. (A Java bean is a Java object that has a default
|
||||
* constructor and getter and has either public data members or getter and setter methods for the data members
|
||||
* using standard Java naming conventions.)
|
||||
* <p>
|
||||
* To use this class, type the generic to the Java Bean class to which the message will be unpacked and provide
|
||||
* the constructor the class of it as well. For example:<p>
|
||||
* <code>MessageUnpacker<Twist> unpacker = new MessageUnpacker<Twist>(Twist.class);</code>. Then provide the
|
||||
* {@link #unpackRosMessage(JsonNode)} method the {@link JsonNode}
|
||||
* provided to a {@link ros.RosListenDelegate} to unpack the "msg" field into the Java Bean. This will also work
|
||||
* with Java Beans with generics. For example, you can do
|
||||
* <code>MessageUnpacker<PrimitiveMsg<String>> unpacker = new MessageUnpacker<PrimitiveMsg<String>>(PrimitiveMsg.class);</code>.
|
||||
*
|
||||
* @author James MacGlashan.
|
||||
*/
|
||||
public class MessageUnpacker <T> {
|
||||
protected ObjectMapper mapper = new ObjectMapper();
|
||||
protected Class<?> javaClass;
|
||||
|
||||
/**
|
||||
* Constructor.
|
||||
* @param javaClass a {@link Class} specifying the class of the Java bean into which
|
||||
* a ros message will be unpacked.
|
||||
*/
|
||||
public MessageUnpacker(Class<?> javaClass) {
|
||||
this.javaClass = javaClass;
|
||||
}
|
||||
|
||||
/**
|
||||
* Unpacks a ros message into the appropriate Java bean from the JSON message returned by
|
||||
* ros bridge. Note that the Ros bridge JSON message contains header information and the actual
|
||||
* ROS message is stored in the 'msg' field in the JSON message. Therefore, this method should be provided the
|
||||
* {@link JsonNode} that is given to a {@link ros.RosListenDelegate}
|
||||
* {@link ros.RosListenDelegate#receive(JsonNode, String)} method.
|
||||
* If the provided {@link JsonNode} argument of this method
|
||||
* does not have a "msg" field, then it will attempt to unpack from the whole JSON node.
|
||||
* @param rosBridgeMessage the {@link JsonNode} from RosBridge.
|
||||
* @return the unpacked Java Bean of the ROS message.
|
||||
*/
|
||||
@SuppressWarnings("unchecked")
|
||||
public T unpackRosMessage(JsonNode rosBridgeMessage){
|
||||
JsonNode rosMsgNode = rosBridgeMessage.get("msg");
|
||||
if(rosMsgNode == null){
|
||||
rosMsgNode = rosBridgeMessage;
|
||||
}
|
||||
T rosMsg = null;
|
||||
try {
|
||||
rosMsg = (T)mapper.treeToValue(rosMsgNode, javaClass);
|
||||
} catch(JsonProcessingException e) {
|
||||
e.printStackTrace();
|
||||
}
|
||||
return rosMsg;
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,178 @@
|
||||
package com.example.testdemo.ros.tools;
|
||||
|
||||
import com.example.testdemo.ros.Publisher;
|
||||
|
||||
import java.util.Timer;
|
||||
import java.util.TimerTask;
|
||||
|
||||
/**
|
||||
* This class is a tool for causing a ROS message to be periodically published at a given rate. To start periodic publishing
|
||||
* use the {@link #beginPublishing(int)} or {@link #beginPublishing(Object, int)} methods. Note that subsequent calls
|
||||
* to these method will automatically cancel the previous periodic publishing and start again at the newly specified rate.
|
||||
* To cancel periodic publishing, use the {@link #cancelPublishing()} method. You can also change the message that is
|
||||
* being published without stopping or restarting the publishing by using the {@link #setMsg(Object)} method.
|
||||
* @author James MacGlashan.
|
||||
*/
|
||||
public class PeriodicPublisher {
|
||||
|
||||
|
||||
/**
|
||||
* The {@link Publisher} to which publish calls are made
|
||||
*/
|
||||
protected Publisher pub;
|
||||
|
||||
/**
|
||||
* The ROS message that will be periodically published
|
||||
*/
|
||||
protected volatile Object msg = null;
|
||||
|
||||
/**
|
||||
* The period at which this object is publishing; -1 if it is not currently publishing.
|
||||
*/
|
||||
protected int period = -1;
|
||||
|
||||
/**
|
||||
* The timer task that calls the publish. Null if this object is not periodically publishing.
|
||||
*/
|
||||
protected PublisherTimerTask timerTask;
|
||||
|
||||
/**
|
||||
* Initializes with a {@link Publisher} for this action using the specified topic, message type, and {@link RosBridge} connection.
|
||||
* @param topic the ros topic to which messages will be published
|
||||
* @param msgType the ros message type of th target topic
|
||||
* @param rosBridge the {@link RosBridge} connection to use.
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* Initializes with a {@link Publisher} for this action using the specified topic, message type, and {@link RosBridge} connection.
|
||||
* @param topic the ros topic to which messages will be published
|
||||
* @param msgType the ros message type of th target topic
|
||||
* @param rosBridge the {@link RosBridge} connection to use.
|
||||
* @param msg the ROS message to periodically publish when publishing begins
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* Initializes with a {@link Publisher} for publishing action messages to ROS.
|
||||
* @param pub the {@link Publisher} to which to publish
|
||||
*/
|
||||
public PeriodicPublisher(Publisher pub){
|
||||
this.pub = pub;
|
||||
}
|
||||
|
||||
/**
|
||||
* Initializes with a {@link Publisher} for publishing action messages to ROS.
|
||||
* @param pub the {@link Publisher} to which to publish
|
||||
* @param msg the ROS message to periodically publish when publishing begins
|
||||
*/
|
||||
public PeriodicPublisher(Publisher pub, Object msg) {
|
||||
this.pub = pub;
|
||||
this.msg = msg;
|
||||
}
|
||||
|
||||
public Publisher getPub() {
|
||||
return pub;
|
||||
}
|
||||
|
||||
public void setPub(Publisher pub) {
|
||||
this.pub = pub;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the ROS message that is being or will be periodically published.
|
||||
* @return the ROS message that is being or will be periodically published.
|
||||
*/
|
||||
public Object getMsg() {
|
||||
return msg;
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the ROS message that will be periodically published. Note that if the provided message is null and this
|
||||
* object is already periodically publishing, a runtime error will be thrown.
|
||||
* @param msg the ROS message that will be published.
|
||||
*/
|
||||
public void setMsg(Object msg) {
|
||||
if(msg == null && this.timerTask != null){
|
||||
System.out.println("PeriodicPublisher is not setting message because new message is null and publisher is currently publishing.");
|
||||
}
|
||||
this.msg = msg;
|
||||
}
|
||||
|
||||
/**
|
||||
* The delay in milliseconds between subsequent periodic publishes. Returns -1 if this object is not currently periodically publishing.
|
||||
* @return The delay in milliseconds between subsequent publishes. Returns -1 if this object is not currently periodically publishing.
|
||||
*/
|
||||
public int getPublishingPeriod(){
|
||||
return this.period;
|
||||
}
|
||||
|
||||
/**
|
||||
* Stops this object from periodically publishing, or does nothing if it is not currently periodically publishing.
|
||||
*/
|
||||
public void cancelPublishing(){
|
||||
if(this.timerTask != null) {
|
||||
this.timerTask.cancel();
|
||||
this.timerTask = null;
|
||||
this.period = -1;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Indicates whether this object is currently periodically publishing.
|
||||
* @return true if this object is periodically publishing; false if it is not.
|
||||
*/
|
||||
public boolean isPublishing(){
|
||||
return this.timerTask != null;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Causes this object to begin periodically publishing at the specified rate. If this object is
|
||||
* already publishing then the previous publishing rate is canceled and started at the new rate.
|
||||
* If the current message to
|
||||
* publish is not set, then a runtime exception will be thrown. The message can be set either with the
|
||||
* {@link #setMsg(Object)} method or by using the alternative {@link #beginPublishing(Object, int)}
|
||||
* method that takes the message to publish.
|
||||
* @param period the time in milliseconds between publishes
|
||||
*/
|
||||
public void beginPublishing(int period){
|
||||
if(this.msg == null){
|
||||
throw new RuntimeException("Cannot begin publishing because the message to publish is unset. " +
|
||||
"Use the setMsg method or beginPublishing(Object msg, int period)");
|
||||
}
|
||||
if(this.timerTask != null){
|
||||
this.timerTask.cancel();
|
||||
}
|
||||
this.timerTask = new PublisherTimerTask();
|
||||
Timer timer = new Timer();
|
||||
timer.schedule(this.timerTask, 0, period);
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Causes this object to begin periodically publishing the specified message at the specified rate.
|
||||
* If this object is already publishing then the previous publishing rate is canceled and started at the new rate.
|
||||
* Note that if the provided message is null then a runtime exception will be thrown.
|
||||
* @param msg the ROS message to publish
|
||||
* @param period the time in milliseconds between publishes
|
||||
*/
|
||||
public void beginPublishing(Object msg, int period){
|
||||
this.msg = msg;
|
||||
this.beginPublishing(period);
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* The class that is called by Java's {@link Timer} and invokes the actual
|
||||
* publish call.
|
||||
*/
|
||||
protected class PublisherTimerTask extends TimerTask{
|
||||
|
||||
@Override
|
||||
public void run() {
|
||||
//pub.publish(msg);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
@ -0,0 +1,181 @@
|
||||
package edu.wpi.rail.jrosbridge;
|
||||
|
||||
/**
|
||||
* The JRosbridge class contains constant definitions used in the rosbridge
|
||||
* protocol itself (e.g., op code types).
|
||||
*
|
||||
* @author Russell Toris - russell.toris@gmail.com
|
||||
* @version April 1, 2014
|
||||
*/
|
||||
public class JRosbridge {
|
||||
|
||||
/**
|
||||
* The args field for the rosbridge protocol.
|
||||
*/
|
||||
public static final String FIELD_ARGS = "args";
|
||||
|
||||
/**
|
||||
* The client field for the rosbridge protocol.
|
||||
*/
|
||||
public static final String FIELD_CLIENT = "client";
|
||||
|
||||
/**
|
||||
* The compression field for the rosbridge protocol.
|
||||
*/
|
||||
public static final String FIELD_COMPRESSION = "compression";
|
||||
|
||||
/**
|
||||
* The data field for the rosbridge protocol.
|
||||
*/
|
||||
public static final String FIELD_DATA = "data";
|
||||
|
||||
/**
|
||||
* The destination field for the rosbridge protocol.
|
||||
*/
|
||||
public static final String FIELD_DESTINATION = "dest";
|
||||
|
||||
/**
|
||||
* The end time field for the rosbridge protocol.
|
||||
*/
|
||||
public static final String FIELD_END_TIME = "end";
|
||||
|
||||
/**
|
||||
* The ID field for the rosbridge protocol.
|
||||
*/
|
||||
public static final String FIELD_ID = "id";
|
||||
|
||||
/**
|
||||
* The user level field for the rosbridge protocol.
|
||||
*/
|
||||
public static final String FIELD_LEVEL = "level";
|
||||
|
||||
/**
|
||||
* The MAC field for the rosbridge protocol.
|
||||
*/
|
||||
public static final String FIELD_MAC = "mac";
|
||||
|
||||
/**
|
||||
* The message data field for the rosbridge protocol.
|
||||
*/
|
||||
public static final String FIELD_MESSAGE = "msg";
|
||||
|
||||
/**
|
||||
* The op code field for the rosbridge protocol.
|
||||
*/
|
||||
public static final String FIELD_OP = "op";
|
||||
|
||||
/**
|
||||
* The random field for the rosbridge protocol.
|
||||
*/
|
||||
public static final String FIELD_RAND = "rand";
|
||||
|
||||
/**
|
||||
* The result field for the rosbridge protocol.
|
||||
*/
|
||||
public static final String FIELD_RESULT = "result";
|
||||
|
||||
/**
|
||||
* The service field for the rosbridge protocol.
|
||||
*/
|
||||
public static final String FIELD_SERVICE = "service";
|
||||
|
||||
/**
|
||||
* The throttle rate field for the rosbridge protocol.
|
||||
*/
|
||||
public static final String FIELD_THROTTLE_RATE = "throttle_rate";
|
||||
|
||||
/**
|
||||
* The time field for the rosbridge protocol.
|
||||
*/
|
||||
public static final String FIELD_TIME = "t";
|
||||
|
||||
/**
|
||||
* The topic field for the rosbridge protocol.
|
||||
*/
|
||||
public static final String FIELD_TOPIC = "topic";
|
||||
|
||||
/**
|
||||
* The message/service type field for the rosbridge protocol.
|
||||
*/
|
||||
public static final String FIELD_TYPE = "type";
|
||||
|
||||
/**
|
||||
* The values field for the rosbridge protocol.
|
||||
*/
|
||||
public static final String FIELD_VALUES = "values";
|
||||
|
||||
/**
|
||||
* The advertise op code for the rosbridge protocol.
|
||||
*/
|
||||
public static final String OP_CODE_ADVERTISE = "advertise";
|
||||
|
||||
/**
|
||||
* The advertise service op code for the rosbridge protocol.
|
||||
*/
|
||||
public static final String OP_CODE_ADVERTISE_SERVICE = "advertise_service";
|
||||
|
||||
/**
|
||||
* The unadvertise service op code for the rosbridge protocol.
|
||||
*/
|
||||
public static final String OP_CODE_UNADVERTISE_SERVICE = "unadvertise_service";
|
||||
|
||||
/**
|
||||
* The authenticate op code for the rosbridge protocol.
|
||||
*/
|
||||
public static final String OP_CODE_AUTH = "auth";
|
||||
|
||||
/**
|
||||
* The call service op code for the rosbridge protocol.
|
||||
*/
|
||||
public static final String OP_CODE_CALL_SERVICE = "call_service";
|
||||
|
||||
/**
|
||||
* The png compression op code for the rosbridge protocol.
|
||||
*/
|
||||
public static final String OP_CODE_PNG = "png";
|
||||
|
||||
/**
|
||||
* The publish op code for the rosbridge protocol.
|
||||
*/
|
||||
public static final String OP_CODE_PUBLISH = "publish";
|
||||
|
||||
/**
|
||||
* The service response op code for the rosbridge protocol.
|
||||
*/
|
||||
public static final String OP_CODE_SERVICE_RESPONSE = "service_response";
|
||||
|
||||
/**
|
||||
* The subscribe op code for the rosbridge protocol.
|
||||
*/
|
||||
public static final String OP_CODE_SUBSCRIBE = "subscribe";
|
||||
|
||||
/**
|
||||
* The unadvertise op code for the rosbridge protocol.
|
||||
*/
|
||||
public static final String OP_CODE_UNADVERTISE = "unadvertise";
|
||||
|
||||
/**
|
||||
* The unsubscribe op code for the rosbridge protocol.
|
||||
*/
|
||||
public static final String OP_CODE_UNSUBSCRIBE = "unsubscribe";
|
||||
|
||||
/**
|
||||
* The types of websocket protocols supported by jrosbridge and rosbridge.
|
||||
*
|
||||
* @author Russell Toris - russell.toris@gmail.com
|
||||
* @version April 1, 2014
|
||||
*/
|
||||
public enum WebSocketType {
|
||||
ws, wss
|
||||
}
|
||||
|
||||
/**
|
||||
* The types of compression supported by jrosbridge and rosbridge.
|
||||
*
|
||||
* @author Russell Toris - russell.toris@gmail.com
|
||||
* @version April 1, 2014
|
||||
*/
|
||||
public enum CompressionType {
|
||||
png, none
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,106 @@
|
||||
package edu.wpi.rail.jrosbridge;
|
||||
|
||||
import java.io.StringReader;
|
||||
|
||||
import javax.json.Json;
|
||||
import javax.json.JsonObject;
|
||||
|
||||
/**
|
||||
* JsonWrapper objects are used as a wrapper around JSON objects. That is, they
|
||||
* are a reusable convince for messages and services. This objects are immutable
|
||||
* and declared abstract as they should not be used directly.
|
||||
*
|
||||
* @author Russell Toris - russell.toris@gmail.com
|
||||
* @version April 1, 2014
|
||||
*/
|
||||
public abstract class JsonWrapper {
|
||||
|
||||
/**
|
||||
* The String representation of an empty message in JSON.
|
||||
*/
|
||||
public static final String EMPTY_JSON = "{}";
|
||||
|
||||
private final JsonObject jsonObject;
|
||||
private final String jsonString;
|
||||
|
||||
/**
|
||||
* Create a new, empty JSON object.
|
||||
*/
|
||||
public JsonWrapper() {
|
||||
this(JsonWrapper.EMPTY_JSON);
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a JSON object based on the given String representation of a JSON
|
||||
* object.
|
||||
*
|
||||
* @param json
|
||||
* The JSON String to parse.
|
||||
*/
|
||||
public JsonWrapper(String jsonString) {
|
||||
// parse and pass it to the JSON constructor
|
||||
this(Json.createReader(new StringReader(jsonString)).readObject());
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a Message based on the given JSON object.
|
||||
*
|
||||
* @param jsonObject
|
||||
* The JSON object containing the message data.
|
||||
*/
|
||||
public JsonWrapper(JsonObject jsonObject) {
|
||||
this.jsonObject = jsonObject;
|
||||
// only need to do this once
|
||||
this.jsonString = this.jsonObject.toString();
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the JSON object.
|
||||
*
|
||||
* @return The JSON object.
|
||||
*/
|
||||
public JsonObject toJsonObject() {
|
||||
return this.jsonObject;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the String representation of this JSON object in JSON format.
|
||||
*
|
||||
* @return The String representation of this JSON object in JSON format.
|
||||
*/
|
||||
@Override
|
||||
public String toString() {
|
||||
return this.jsonString;
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a clone of this JSON object.
|
||||
*/
|
||||
public abstract JsonWrapper clone();
|
||||
|
||||
/**
|
||||
* Return the hash code of this JSON object, which is the hash code of the
|
||||
* JSON string.
|
||||
*
|
||||
* @return The hash code of the message.
|
||||
*/
|
||||
@Override
|
||||
public int hashCode() {
|
||||
return this.jsonString.hashCode();
|
||||
}
|
||||
|
||||
/**
|
||||
* Test if the given Object is equal to this JsonWrapper. Two JsonWrappers
|
||||
* are equal if and only if their JSON strings match.
|
||||
*
|
||||
* @param o
|
||||
* The Object to test equality with.
|
||||
* @return If the given Object is equal to this JsonWrapper.
|
||||
*/
|
||||
@Override
|
||||
public boolean equals(Object o) {
|
||||
return o == this
|
||||
|| (o instanceof JsonWrapper && this.jsonString.equals(o
|
||||
.toString()));
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,535 @@
|
||||
package edu.wpi.rail.jrosbridge;
|
||||
|
||||
import java.awt.image.Raster;
|
||||
import java.io.ByteArrayInputStream;
|
||||
import java.io.IOException;
|
||||
import java.io.StringReader;
|
||||
import java.net.URI;
|
||||
import java.net.URISyntaxException;
|
||||
import java.util.ArrayList;
|
||||
import java.util.HashMap;
|
||||
|
||||
import javax.imageio.ImageIO;
|
||||
import javax.json.Json;
|
||||
import javax.json.JsonObject;
|
||||
import javax.json.stream.JsonParsingException;
|
||||
import javax.websocket.ClientEndpoint;
|
||||
import javax.websocket.ContainerProvider;
|
||||
import javax.websocket.DeploymentException;
|
||||
import javax.websocket.OnClose;
|
||||
import javax.websocket.OnError;
|
||||
import javax.websocket.OnMessage;
|
||||
import javax.websocket.OnOpen;
|
||||
import javax.websocket.Session;
|
||||
|
||||
import com.example.testdemo.ros.message.Message;
|
||||
import edu.wpi.rail.jrosbridge.callback.CallServiceCallback;
|
||||
import edu.wpi.rail.jrosbridge.services.ServiceRequest;
|
||||
|
||||
import edu.wpi.rail.jrosbridge.callback.ServiceCallback;
|
||||
import edu.wpi.rail.jrosbridge.callback.TopicCallback;
|
||||
import edu.wpi.rail.jrosbridge.handler.RosHandler;
|
||||
import edu.wpi.rail.jrosbridge.services.ServiceResponse;
|
||||
import org.springframework.util.Base64Utils;
|
||||
|
||||
@ClientEndpoint
|
||||
public class Ros {
|
||||
|
||||
/**
|
||||
* The default hostname used if none is provided.
|
||||
*/
|
||||
public static final String DEFAULT_HOSTNAME = "localhost";
|
||||
|
||||
/**
|
||||
* The default port used if none is provided.
|
||||
*/
|
||||
public static final int DEFAULT_PORT = 9090;
|
||||
|
||||
private final String hostname;
|
||||
private final int port;
|
||||
private final JRosbridge.WebSocketType protocol;
|
||||
|
||||
// active session (stored upon connection)
|
||||
private Session session;
|
||||
|
||||
// used throughout the library to create unique IDs for requests.
|
||||
private long idCounter;
|
||||
|
||||
// keeps track of callback functions for a given topic
|
||||
private final HashMap<String, ArrayList<TopicCallback>> topicCallbacks;
|
||||
|
||||
// keeps track of callback functions for a given service request
|
||||
private final HashMap<String, ServiceCallback> serviceCallbacks;
|
||||
|
||||
// keeps track of callback functions for a given advertised service
|
||||
private final HashMap<String, CallServiceCallback> callServiceCallbacks;
|
||||
|
||||
// keeps track of handlers for this connection
|
||||
private final ArrayList<RosHandler> handlers;
|
||||
|
||||
/**
|
||||
* Create a connection to ROS with the default hostname and port. A call to
|
||||
* connect must be made to establish a connection.
|
||||
*/
|
||||
public Ros() {
|
||||
this(Ros.DEFAULT_HOSTNAME);
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a connection to ROS with the given hostname and default port. A
|
||||
* call to connect must be made to establish a connection. By default,
|
||||
* WebSockets is used (as opposed to WSS).
|
||||
*
|
||||
* @param hostname
|
||||
* The hostname to connect to.
|
||||
*/
|
||||
public Ros(String hostname) {
|
||||
this(hostname, Ros.DEFAULT_PORT);
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a connection to ROS with the given hostname and port. A call to
|
||||
* connect must be made to establish a connection. By default, WebSockets is
|
||||
* used (as opposed to WSS).
|
||||
*
|
||||
* @param hostname
|
||||
* The hostname to connect to.
|
||||
* @param port
|
||||
* The port to connect to.
|
||||
*/
|
||||
public Ros(String hostname, int port) {
|
||||
this(hostname, port, JRosbridge.WebSocketType.ws);
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a connection to ROS with the given hostname and port. A call to
|
||||
* connect must be made to establish a connection.
|
||||
*
|
||||
* @param hostname
|
||||
* The hostname to connect to.
|
||||
* @param port
|
||||
* The port to connect to.
|
||||
* @param protocol
|
||||
* The WebSocket protocol to use.
|
||||
*/
|
||||
public Ros(String hostname, int port, JRosbridge.WebSocketType protocol) {
|
||||
this.hostname = hostname;
|
||||
this.port = port;
|
||||
this.protocol = protocol;
|
||||
this.session = null;
|
||||
this.idCounter = 0;
|
||||
this.topicCallbacks = new HashMap<String, ArrayList<TopicCallback>>();
|
||||
this.serviceCallbacks = new HashMap<String, ServiceCallback>();
|
||||
this.callServiceCallbacks = new HashMap<String, CallServiceCallback>();
|
||||
this.handlers = new ArrayList<RosHandler>();
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the hostname associated with this connection.
|
||||
*
|
||||
* @return The hostname associated with this connection.
|
||||
*/
|
||||
public String getHostname() {
|
||||
return this.hostname;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the port associated with this connection.
|
||||
*
|
||||
* @return The port associated with this connection.
|
||||
*/
|
||||
public int getPort() {
|
||||
return this.port;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the type of WebSocket protocol being used.
|
||||
*
|
||||
* @return The type of WebSocket protocol being used.
|
||||
*/
|
||||
public JRosbridge.WebSocketType getProtocol() {
|
||||
return this.protocol;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the full URL this client is connecting to.
|
||||
*
|
||||
* @return
|
||||
*/
|
||||
public String getURL() {
|
||||
return this.protocol.toString() + "://" + this.hostname + ":"
|
||||
+ this.port;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the next unique ID number for this connection.
|
||||
*
|
||||
* @return The next unique ID number for this connection.
|
||||
*/
|
||||
public long nextId() {
|
||||
return this.idCounter++;
|
||||
}
|
||||
|
||||
/**
|
||||
* Add a handler to this connection. This handler is called when the
|
||||
* associated events occur.
|
||||
*
|
||||
* @param handler
|
||||
* The handler to add.
|
||||
*/
|
||||
public void addRosHandler(RosHandler handler) {
|
||||
this.handlers.add(handler);
|
||||
}
|
||||
|
||||
/**
|
||||
* Attempt to establish a connection to rosbridge. Errors are printed to the
|
||||
* error output stream.
|
||||
*
|
||||
* @return Returns true if the connection was established successfully and
|
||||
* false otherwise.
|
||||
*/
|
||||
public boolean connect() {
|
||||
try {
|
||||
// create a WebSocket connection here
|
||||
URI uri = new URI(this.getURL());
|
||||
ContainerProvider.getWebSocketContainer()
|
||||
.connectToServer(this, uri);
|
||||
return true;
|
||||
} catch (DeploymentException | URISyntaxException | IOException e) {
|
||||
// failed connection, return false
|
||||
System.err.println("[ERROR]: Could not create WebSocket: "
|
||||
+ e.getMessage());
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Disconnect the connection to rosbridge. Errors are printed to the error
|
||||
* output stream.
|
||||
*
|
||||
* @return Returns true if the disconnection was successful and false
|
||||
* otherwise.
|
||||
*/
|
||||
public boolean disconnect() {
|
||||
if (this.isConnected()) {
|
||||
try {
|
||||
this.session.close();
|
||||
return true;
|
||||
} catch (IOException e) {
|
||||
System.err.println("[ERROR]: Could not disconnect: "
|
||||
+ e.getMessage());
|
||||
}
|
||||
}
|
||||
// could not disconnect cleanly
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* Check if there is a connection to rosbridge.
|
||||
*
|
||||
* @return If there is a connection to rosbridge.
|
||||
*/
|
||||
public boolean isConnected() {
|
||||
return this.session != null && this.session.isOpen();
|
||||
}
|
||||
|
||||
/**
|
||||
* This function is called once a successful connection is made.
|
||||
*
|
||||
* @param session
|
||||
* The session associated with the connection.
|
||||
*/
|
||||
@OnOpen
|
||||
public void onOpen(Session session) {
|
||||
// store the session
|
||||
this.session = session;
|
||||
|
||||
// call the handlers
|
||||
for (RosHandler handler : this.handlers) {
|
||||
handler.handleConnection(session);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* This function is called once a successful disconnection is made.
|
||||
*
|
||||
* @param session
|
||||
* The session associated with the disconnection.
|
||||
*/
|
||||
@OnClose
|
||||
public void onClose(Session session) {
|
||||
// remove the session
|
||||
this.session = null;
|
||||
|
||||
// call the handlers
|
||||
for (RosHandler handler : this.handlers) {
|
||||
handler.handleDisconnection(session);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* This function is called if an error occurs.
|
||||
*
|
||||
* @param session
|
||||
* The session for the error.
|
||||
* @param session
|
||||
* The session for the error.
|
||||
*/
|
||||
@OnError
|
||||
public void onError(Session session, Throwable t) {
|
||||
// call the handlers
|
||||
for (RosHandler handler : this.handlers) {
|
||||
handler.handleError(session, t);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* This method is called once an entire message has been read in by the
|
||||
* connection from rosbridge. It will parse the incoming JSON and attempt to
|
||||
* handle the request appropriately.
|
||||
*
|
||||
* @param message
|
||||
* The incoming JSON message from rosbridge.
|
||||
*/
|
||||
@OnMessage
|
||||
public void onMessage(String message) {
|
||||
try {
|
||||
// parse the JSON
|
||||
JsonObject jsonObject = Json
|
||||
.createReader(new StringReader(message)).readObject();
|
||||
|
||||
// check for compression
|
||||
String op = jsonObject.getString(JRosbridge.FIELD_OP);
|
||||
if (op.equals(JRosbridge.OP_CODE_PNG)) {
|
||||
String data = jsonObject.getString(JRosbridge.FIELD_DATA);
|
||||
// decompress the PNG data
|
||||
byte[] bytes = Base64Utils.decode(data.getBytes());
|
||||
Raster imageData = ImageIO
|
||||
.read(new ByteArrayInputStream(bytes)).getRaster();
|
||||
|
||||
// read the RGB data
|
||||
int[] rawData = null;
|
||||
rawData = imageData.getPixels(0, 0, imageData.getWidth(),
|
||||
imageData.getHeight(), rawData);
|
||||
StringBuffer buffer = new StringBuffer();
|
||||
for (int i = 0; i < rawData.length; i++) {
|
||||
buffer.append(Character.toString((char) rawData[i]));
|
||||
}
|
||||
|
||||
// reparse the JSON
|
||||
JsonObject newJsonObject = Json.createReader(
|
||||
new StringReader(buffer.toString())).readObject();
|
||||
handleMessage(newJsonObject);
|
||||
} else {
|
||||
handleMessage(jsonObject);
|
||||
}
|
||||
} catch (NullPointerException | IOException | JsonParsingException e) {
|
||||
// only occurs if there was an error with the JSON
|
||||
System.err.println("[WARN]: Invalid incoming rosbridge protocol: "
|
||||
+ message);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Handle the incoming rosbridge message by calling the appropriate
|
||||
* callbacks.
|
||||
*
|
||||
* @param jsonObject
|
||||
* The JSON object from the incoming rosbridge message.
|
||||
*/
|
||||
private void handleMessage(JsonObject jsonObject) {
|
||||
// check for the correct fields
|
||||
String op = jsonObject.getString(JRosbridge.FIELD_OP);
|
||||
if (op.equals(JRosbridge.OP_CODE_PUBLISH)) {
|
||||
// check for the topic name
|
||||
String topic = jsonObject.getString(JRosbridge.FIELD_TOPIC);
|
||||
|
||||
// call each callback with the message
|
||||
ArrayList<TopicCallback> callbacks = topicCallbacks.get(topic);
|
||||
if (callbacks != null) {
|
||||
Message msg = new Message() {
|
||||
@Override
|
||||
public void print() {
|
||||
super.print();
|
||||
}
|
||||
};
|
||||
for (TopicCallback cb : callbacks) {
|
||||
cb.handleMessage(msg);
|
||||
}
|
||||
}
|
||||
} else if (op.equals(JRosbridge.OP_CODE_SERVICE_RESPONSE)) {
|
||||
// check for the request ID
|
||||
String id = jsonObject.getString(JRosbridge.FIELD_ID);
|
||||
|
||||
// call the callback for the request
|
||||
ServiceCallback cb = serviceCallbacks.get(id);
|
||||
if (cb != null) {
|
||||
// check if a success code was given
|
||||
boolean success = jsonObject
|
||||
.containsKey(JRosbridge.FIELD_RESULT) ? jsonObject
|
||||
.getBoolean(JRosbridge.FIELD_RESULT) : true;
|
||||
// get the response
|
||||
JsonObject values = jsonObject
|
||||
.getJsonObject(JRosbridge.FIELD_VALUES);
|
||||
ServiceResponse response = new ServiceResponse(values, success);
|
||||
cb.handleServiceResponse(response);
|
||||
}
|
||||
} else if (op.equals(JRosbridge.OP_CODE_CALL_SERVICE)) {
|
||||
// check for the request ID
|
||||
String id = jsonObject.getString("id");
|
||||
String service = jsonObject.getString("service");
|
||||
|
||||
// call the callback for the request
|
||||
CallServiceCallback cb = callServiceCallbacks.get(service);
|
||||
if (cb != null) {
|
||||
// get the response
|
||||
JsonObject args = jsonObject
|
||||
.getJsonObject(JRosbridge.FIELD_ARGS);
|
||||
ServiceRequest request = new ServiceRequest(args);
|
||||
request.setId(id);
|
||||
cb.handleServiceCall(request);
|
||||
}
|
||||
} else {
|
||||
System.err.println("[WARN]: Unrecognized op code: "
|
||||
+ jsonObject.toString());
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* Send the given JSON object to rosbridge.
|
||||
*
|
||||
* @param jsonObject
|
||||
* The JSON object to send to rosbridge.
|
||||
* @return If the sending of the message was successful.
|
||||
*/
|
||||
public boolean send(JsonObject jsonObject) {
|
||||
// check the connection
|
||||
if (this.isConnected()) {
|
||||
try {
|
||||
// send it as text
|
||||
this.session.getBasicRemote().sendText(jsonObject.toString());
|
||||
return true;
|
||||
} catch (IOException e) {
|
||||
System.err.println("[ERROR]: Could not send message: "
|
||||
+ e.getMessage());
|
||||
}
|
||||
}
|
||||
// message send failed
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* Sends an authorization request to the server.
|
||||
*
|
||||
* @param mac
|
||||
* The MAC (hash) string given by the trusted source.
|
||||
* @param client
|
||||
* The IP of the client.
|
||||
* @param dest
|
||||
* The IP of the destination.
|
||||
* @param rand
|
||||
* The random string given by the trusted source.
|
||||
* @param t
|
||||
* The time of the authorization request.
|
||||
* @param level
|
||||
* The user level as a string given by the client.
|
||||
* @param end
|
||||
* The end time of the client's session.
|
||||
*/
|
||||
public void authenticate(String mac, String client, String dest,
|
||||
String rand, int t, String level, int end) {
|
||||
// build and send the rosbridge call
|
||||
JsonObject call = Json.createObjectBuilder()
|
||||
.add(JRosbridge.FIELD_OP, JRosbridge.OP_CODE_AUTH)
|
||||
.add(JRosbridge.FIELD_MAC, mac)
|
||||
.add(JRosbridge.FIELD_CLIENT, client)
|
||||
.add(JRosbridge.FIELD_DESTINATION, dest)
|
||||
.add(JRosbridge.FIELD_RAND, rand).add(JRosbridge.FIELD_TIME, t)
|
||||
.add(JRosbridge.FIELD_LEVEL, level)
|
||||
.add(JRosbridge.FIELD_END_TIME, end).build();
|
||||
this.send(call);
|
||||
}
|
||||
|
||||
/**
|
||||
* Register a callback for a given topic.
|
||||
*
|
||||
* @param topic
|
||||
* The topic to register this callback with.
|
||||
* @param cb
|
||||
* The callback that will be called when messages come in for the
|
||||
* associated topic.
|
||||
*/
|
||||
public void registerTopicCallback(String topic, TopicCallback cb) {
|
||||
// check if any callbacks exist yet
|
||||
if (!this.topicCallbacks.containsKey(topic)) {
|
||||
this.topicCallbacks.put(topic, new ArrayList<TopicCallback>());
|
||||
}
|
||||
|
||||
// add the callback
|
||||
this.topicCallbacks.get(topic).add(cb);
|
||||
}
|
||||
|
||||
/**
|
||||
* Deregister a callback for a given topic.
|
||||
*
|
||||
* @param topic
|
||||
* The topic associated with the callback.
|
||||
* @param cb
|
||||
* The callback to remove.
|
||||
*/
|
||||
public void deregisterTopicCallback(String topic, TopicCallback cb) {
|
||||
// check if any exist for this topic
|
||||
if (this.topicCallbacks.containsKey(topic)) {
|
||||
// remove the callback if it exists
|
||||
ArrayList<TopicCallback> callbacks = this.topicCallbacks.get(topic);
|
||||
if (callbacks.contains(cb)) {
|
||||
callbacks.remove(cb);
|
||||
}
|
||||
|
||||
// remove the list if it is empty
|
||||
if (callbacks.size() == 0) {
|
||||
this.topicCallbacks.remove(topic);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Register a callback for a given outgoing service call.
|
||||
*
|
||||
* @param serviceCallId
|
||||
* The unique ID of the service call.
|
||||
* @param cb
|
||||
* The callback that will be called when a service response comes
|
||||
* back for the associated request.
|
||||
*/
|
||||
public void registerServiceCallback(String serviceCallId, ServiceCallback cb) {
|
||||
// add the callback
|
||||
this.serviceCallbacks.put(serviceCallId, cb);
|
||||
}
|
||||
|
||||
/**
|
||||
* Register a callback for a given incoming service request.
|
||||
*
|
||||
* @param serviceName
|
||||
* The unique name of the service call.
|
||||
* @param cb
|
||||
* The callback that will be called when a service request comes
|
||||
* in for the associated request.
|
||||
*/
|
||||
public void registerCallServiceCallback(String serviceName, CallServiceCallback cb) {
|
||||
// add the callback
|
||||
this.callServiceCallbacks.put(serviceName, cb);
|
||||
}
|
||||
|
||||
/**
|
||||
* Deregister a callback for a given incoming service request.
|
||||
*
|
||||
* @param serviceName
|
||||
* The unique name of the service call.
|
||||
*/
|
||||
public void deregisterCallServiceCallback(String serviceName) {
|
||||
// remove the callback
|
||||
callServiceCallbacks.remove(serviceName);
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,240 @@
|
||||
package edu.wpi.rail.jrosbridge;
|
||||
|
||||
import javax.json.Json;
|
||||
import javax.json.JsonObject;
|
||||
|
||||
import edu.wpi.rail.jrosbridge.callback.ServiceCallback;
|
||||
import edu.wpi.rail.jrosbridge.callback.CallServiceCallback;
|
||||
import edu.wpi.rail.jrosbridge.services.ServiceRequest;
|
||||
import edu.wpi.rail.jrosbridge.services.ServiceResponse;
|
||||
|
||||
/**
|
||||
* The Service object is responsible for calling or advertising a service in ROS.
|
||||
*
|
||||
* @author Russell Toris - russell.toris@gmail.com
|
||||
* @version November 26, 2014
|
||||
*/
|
||||
public class Service {
|
||||
|
||||
private final Ros ros;
|
||||
private final String name;
|
||||
private final String type;
|
||||
private boolean isAdvertised;
|
||||
|
||||
/**
|
||||
* Create a ROS service with the given information.
|
||||
*
|
||||
* @param ros
|
||||
* A handle to the ROS connection.
|
||||
* @param name
|
||||
* The name of the service (e.g., "/add_two_ints").
|
||||
* @param type
|
||||
* The service type (e.g., "rospy_tutorials/AddTwoInts").
|
||||
*/
|
||||
public Service(Ros ros, String name, String type) {
|
||||
this.ros = ros;
|
||||
this.name = name;
|
||||
this.type = type;
|
||||
this.isAdvertised = false;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the ROS connection handle for this service.
|
||||
*
|
||||
* @return The ROS connection handle for this service.
|
||||
*/
|
||||
public Ros getRos() {
|
||||
return this.ros;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the name of this service.
|
||||
*
|
||||
* @return The name of this service.
|
||||
*/
|
||||
public String getName() {
|
||||
return this.name;
|
||||
}
|
||||
|
||||
/**
|
||||
* Return the service type of this service.
|
||||
*
|
||||
* @return The service type of this service.
|
||||
*/
|
||||
public String getType() {
|
||||
return this.type;
|
||||
}
|
||||
|
||||
/**
|
||||
* Check if the current service is advertising to ROS.
|
||||
*
|
||||
* @return If the current service is advertising to ROS.
|
||||
*/
|
||||
public boolean isAdvertised() {
|
||||
return this.isAdvertised;
|
||||
}
|
||||
|
||||
/**
|
||||
* Call this service. The callback function will be called with the
|
||||
* associated service response.
|
||||
*
|
||||
* @param request
|
||||
* The service request to send.
|
||||
* @param cb
|
||||
* The callback used when the associated response comes back.
|
||||
*/
|
||||
public void callService(ServiceRequest request, ServiceCallback cb) {
|
||||
// construct the unique ID
|
||||
String callServceId = "call_service:" + this.name + ":"
|
||||
+ this.ros.nextId();
|
||||
|
||||
// register the callback function
|
||||
this.ros.registerServiceCallback(callServceId, cb);
|
||||
|
||||
// build and send the rosbridge call
|
||||
JsonObject call = Json.createObjectBuilder()
|
||||
.add(JRosbridge.FIELD_OP, JRosbridge.OP_CODE_CALL_SERVICE)
|
||||
.add(JRosbridge.FIELD_ID, callServceId)
|
||||
.add(JRosbridge.FIELD_TYPE, this.type)
|
||||
.add(JRosbridge.FIELD_SERVICE, this.name)
|
||||
.add(JRosbridge.FIELD_ARGS, request.toJsonObject()).build();
|
||||
this.ros.send(call);
|
||||
}
|
||||
|
||||
/**
|
||||
* Send a service response.
|
||||
*
|
||||
* @param response
|
||||
* The service response to send.
|
||||
* @param id
|
||||
* The ID of the response (matching that of the service call).
|
||||
*/
|
||||
public void sendResponse(ServiceResponse response, String id) {
|
||||
// build and send the rosbridge call
|
||||
JsonObject call = Json.createObjectBuilder()
|
||||
.add(JRosbridge.FIELD_OP, JRosbridge.OP_CODE_SERVICE_RESPONSE)
|
||||
.add(JRosbridge.FIELD_ID, id)
|
||||
.add(JRosbridge.FIELD_SERVICE, this.name)
|
||||
.add(JRosbridge.FIELD_VALUES, response.toJsonObject())
|
||||
.add(JRosbridge.FIELD_RESULT, response.getResult()).build();
|
||||
this.ros.send(call);
|
||||
}
|
||||
|
||||
/**
|
||||
* Registers as service advertiser.
|
||||
*/
|
||||
public void advertiseService(CallServiceCallback cb) {
|
||||
// register the callback
|
||||
this.ros.registerCallServiceCallback(this.name, cb);
|
||||
|
||||
// build and send the rosbridge call
|
||||
JsonObject call = Json.createObjectBuilder()
|
||||
.add(JRosbridge.FIELD_OP, JRosbridge.OP_CODE_ADVERTISE_SERVICE)
|
||||
.add(JRosbridge.FIELD_TYPE, this.type)
|
||||
.add(JRosbridge.FIELD_SERVICE, this.name).build();
|
||||
this.ros.send(call);
|
||||
|
||||
// set the flag indicating we are registered
|
||||
this.isAdvertised = true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Unregisters as service advertiser.
|
||||
*/
|
||||
public void unadvertiseService() {
|
||||
this.ros.deregisterCallServiceCallback(this.name);
|
||||
|
||||
// build and send the rosbridge call
|
||||
JsonObject call = Json.createObjectBuilder()
|
||||
.add(JRosbridge.FIELD_OP, JRosbridge.OP_CODE_UNADVERTISE_SERVICE)
|
||||
.add(JRosbridge.FIELD_SERVICE, this.name).build();
|
||||
this.ros.send(call);
|
||||
|
||||
// set the flag indicating we are registered
|
||||
this.isAdvertised = false;
|
||||
}
|
||||
|
||||
/**
|
||||
* Call the service and wait for a response. This is a blocking call and
|
||||
* will only return once rosbridge returns the service response. For an
|
||||
* asynchronous version of this call, see the
|
||||
* {@link #callService(ServiceRequest request, ServiceCallback cb)
|
||||
* callService} method.
|
||||
*
|
||||
* @param request
|
||||
* The service request to send.
|
||||
* @return The corresponding service response from ROS.
|
||||
*/
|
||||
public synchronized ServiceResponse callServiceAndWait(
|
||||
ServiceRequest request) {
|
||||
|
||||
// private inner class to use as a callback
|
||||
BlockingCallback cb = new BlockingCallback(this);
|
||||
// use the asynchronous version and block on the result
|
||||
this.callService(request, cb);
|
||||
|
||||
// wait for a response
|
||||
while (cb.getResponse() == null) {
|
||||
try {
|
||||
this.wait();
|
||||
} catch (InterruptedException e) {
|
||||
// continue on
|
||||
}
|
||||
}
|
||||
|
||||
return cb.getResponse();
|
||||
}
|
||||
|
||||
/**
|
||||
* A private {@link ServiceCallback
|
||||
* ServiceCallback} used to block and wait for a response from rosbridge.
|
||||
*
|
||||
* @author Russell Toris - russell.toris@gmail.com
|
||||
* @version April 1, 2014
|
||||
*/
|
||||
private class BlockingCallback implements ServiceCallback {
|
||||
|
||||
private ServiceResponse response;
|
||||
private Service service;
|
||||
|
||||
/**
|
||||
* Create a new callback function which will notify the given
|
||||
* {@link Service Service} once a response
|
||||
* has been received.
|
||||
*
|
||||
* @param service
|
||||
* The {@link Service Service}
|
||||
* to notify once a response has been received.
|
||||
*/
|
||||
public BlockingCallback(Service service) {
|
||||
this.response = null;
|
||||
this.service = service;
|
||||
}
|
||||
|
||||
/**
|
||||
* Store the response internally and notify the corresponding
|
||||
* {@link Service Service}.
|
||||
*
|
||||
* @param response
|
||||
* The incoming service response from ROS.
|
||||
*/
|
||||
@Override
|
||||
public void handleServiceResponse(ServiceResponse response) {
|
||||
this.response = response;
|
||||
synchronized (this.service) {
|
||||
this.service.notifyAll();
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the response stored in this callback, if one exists. Otherwise,
|
||||
* null is returned.
|
||||
*
|
||||
* @return The resulting service response from ROS, or null if one does
|
||||
* not exist yet.
|
||||
*/
|
||||
public ServiceResponse getResponse() {
|
||||
return this.response;
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,279 @@
|
||||
package edu.wpi.rail.jrosbridge;
|
||||
|
||||
import java.util.ArrayList;
|
||||
|
||||
import javax.json.Json;
|
||||
import javax.json.JsonObject;
|
||||
|
||||
import com.example.testdemo.ros.message.Message;
|
||||
import edu.wpi.rail.jrosbridge.callback.TopicCallback;
|
||||
|
||||
/**
|
||||
* The Topic object is responsible for publishing and/or subscribing to a topic
|
||||
* in ROS.
|
||||
*
|
||||
* @author Russell Toris - russell.toris@gmail.com
|
||||
* @version April 1, 2014
|
||||
*/
|
||||
public class Topic {
|
||||
|
||||
private final Ros ros;
|
||||
private final String name;
|
||||
private final String type;
|
||||
private boolean isAdvertised;
|
||||
private boolean isSubscribed;
|
||||
private final JRosbridge.CompressionType compression;
|
||||
private final int throttleRate;
|
||||
|
||||
// used to keep track of this object's callbacks
|
||||
private final ArrayList<TopicCallback> callbacks;
|
||||
|
||||
// used to keep track of the subscription IDs
|
||||
private final ArrayList<String> ids;
|
||||
|
||||
/**
|
||||
* Create a ROS topic with the given information. No compression or
|
||||
* throttling is used.
|
||||
*
|
||||
* @param ros
|
||||
* A handle to the ROS connection.
|
||||
* @param name
|
||||
* The name of the topic (e.g., "/cmd_vel").
|
||||
* @param type
|
||||
* The message type (e.g., "std_msgs/String").
|
||||
*/
|
||||
public Topic(Ros ros, String name, String type) {
|
||||
this(ros, name, type, JRosbridge.CompressionType.none, 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a ROS topic with the given information. No throttling is used.
|
||||
*
|
||||
* @param ros
|
||||
* A handle to the ROS connection.
|
||||
* @param name
|
||||
* The name of the topic (e.g., "/cmd_vel").
|
||||
* @param type
|
||||
* The message type (e.g., "std_msgs/String").
|
||||
* @param compression
|
||||
* The type of compression used for this topic.
|
||||
*/
|
||||
public Topic(Ros ros, String name, String type,
|
||||
JRosbridge.CompressionType compression) {
|
||||
this(ros, name, type, compression, 0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a ROS topic with the given information. No compression is used.
|
||||
*
|
||||
* @param ros
|
||||
* A handle to the ROS connection.
|
||||
* @param name
|
||||
* The name of the topic (e.g., "/cmd_vel").
|
||||
* @param type
|
||||
* The message type (e.g., "std_msgs/String").
|
||||
* @param throttleRate
|
||||
* The throttle rate to use for this topic.
|
||||
*/
|
||||
public Topic(Ros ros, String name, String type, int throttleRate) {
|
||||
this(ros, name, type, JRosbridge.CompressionType.none, throttleRate);
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a ROS topic with the given information.
|
||||
*
|
||||
* @param ros
|
||||
* A handle to the ROS connection.
|
||||
* @param name
|
||||
* The name of the topic (e.g., "/cmd_vel").
|
||||
* @param type
|
||||
* The message type (e.g., "std_msgs/String").
|
||||
* @param compression
|
||||
* The type of compression used for this topic.
|
||||
* @param throttleRate
|
||||
* The throttle rate to use for this topic.
|
||||
*/
|
||||
public Topic(Ros ros, String name, String type,
|
||||
JRosbridge.CompressionType compression, int throttleRate) {
|
||||
this.ros = ros;
|
||||
this.name = name;
|
||||
this.type = type;
|
||||
this.isAdvertised = false;
|
||||
this.isSubscribed = false;
|
||||
this.compression = compression;
|
||||
this.throttleRate = throttleRate;
|
||||
this.callbacks = new ArrayList<TopicCallback>();
|
||||
this.ids = new ArrayList<String>();
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the ROS connection handle for this topic.
|
||||
*
|
||||
* @return The ROS connection handle for this topic.
|
||||
*/
|
||||
public Ros getRos() {
|
||||
return this.ros;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the name of this topic.
|
||||
*
|
||||
* @return The name of this topic.
|
||||
*/
|
||||
public String getName() {
|
||||
return this.name;
|
||||
}
|
||||
|
||||
/**
|
||||
* Return the message type of this topic.
|
||||
*
|
||||
* @return The message type of this topic.
|
||||
*/
|
||||
public String getType() {
|
||||
return this.type;
|
||||
}
|
||||
|
||||
/**
|
||||
* Check if the current topic is advertising to ROS.
|
||||
*
|
||||
* @return If the current topic is advertising to ROS.
|
||||
*/
|
||||
public boolean isAdvertised() {
|
||||
return this.isAdvertised;
|
||||
}
|
||||
|
||||
/**
|
||||
* Check if the current topic is subscribed to ROS.
|
||||
*
|
||||
* @return If the current topic is subscribed to ROS.
|
||||
*/
|
||||
public boolean isSubscribed() {
|
||||
return this.isSubscribed;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the compression type for this topic.
|
||||
*
|
||||
* @return The compression type for this topic.
|
||||
*/
|
||||
public JRosbridge.CompressionType getCompression() {
|
||||
return this.compression;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the throttle rate for this topic.
|
||||
*
|
||||
* @return The throttle rate for this topic.
|
||||
*/
|
||||
public int getThrottleRate() {
|
||||
return this.throttleRate;
|
||||
}
|
||||
|
||||
/**
|
||||
* Subscribe to this topic. A callback function is required and will be
|
||||
* called with any incoming message for this topic.
|
||||
*
|
||||
* @param cb
|
||||
* The callback that will be called when incoming messages are
|
||||
* received.
|
||||
*/
|
||||
public void subscribe(TopicCallback cb) {
|
||||
// register the callback function
|
||||
this.ros.registerTopicCallback(this.name, cb);
|
||||
// internal reference used during unsubscribe
|
||||
this.callbacks.add(cb);
|
||||
|
||||
String subscribeId = "subscribe:" + this.name + ":" + this.ros.nextId();
|
||||
this.ids.add(subscribeId);
|
||||
|
||||
// build and send the rosbridge call
|
||||
JsonObject call = Json.createObjectBuilder()
|
||||
.add(JRosbridge.FIELD_OP, JRosbridge.OP_CODE_SUBSCRIBE)
|
||||
.add(JRosbridge.FIELD_ID, subscribeId)
|
||||
.add(JRosbridge.FIELD_TYPE, this.type)
|
||||
.add(JRosbridge.FIELD_TOPIC, this.name)
|
||||
.add(JRosbridge.FIELD_COMPRESSION, this.compression.toString())
|
||||
.add(JRosbridge.FIELD_THROTTLE_RATE, this.throttleRate).build();
|
||||
this.ros.send(call);
|
||||
|
||||
// set the flag indicating we have subscribed
|
||||
this.isSubscribed = true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Unregisters as a subscriber for the topic. Unsubscribing will remove all
|
||||
* the associated subscribe callbacks.
|
||||
*/
|
||||
public void unsubscribe() {
|
||||
// remove this object's associated callbacks.
|
||||
for (TopicCallback cb : this.callbacks) {
|
||||
this.ros.deregisterTopicCallback(this.name, cb);
|
||||
}
|
||||
this.callbacks.clear();
|
||||
|
||||
// build and send the rosbridge calls
|
||||
for (String id : this.ids) {
|
||||
JsonObject call = Json.createObjectBuilder()
|
||||
.add(JRosbridge.FIELD_OP, JRosbridge.OP_CODE_UNSUBSCRIBE)
|
||||
.add(JRosbridge.FIELD_ID, id)
|
||||
.add(JRosbridge.FIELD_TOPIC, this.name).build();
|
||||
this.ros.send(call);
|
||||
}
|
||||
|
||||
// set the flag indicating we are not longer subscribed
|
||||
this.isSubscribed = false;
|
||||
}
|
||||
|
||||
/**
|
||||
* Registers as a publisher for the topic. This call will be automatically
|
||||
* called by publish if you do not explicitly call it.
|
||||
*/
|
||||
public void advertise() {
|
||||
// build and send the rosbridge call
|
||||
String advertiseId = "advertise:" + this.name + ":" + this.ros.nextId();
|
||||
JsonObject call = Json.createObjectBuilder()
|
||||
.add(JRosbridge.FIELD_OP, JRosbridge.OP_CODE_ADVERTISE)
|
||||
.add(JRosbridge.FIELD_ID, advertiseId)
|
||||
.add(JRosbridge.FIELD_TYPE, this.type)
|
||||
.add(JRosbridge.FIELD_TOPIC, this.name).build();
|
||||
this.ros.send(call);
|
||||
|
||||
// set the flag indicating we are registered
|
||||
this.isAdvertised = true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Unregister as a publisher for the topic.
|
||||
*/
|
||||
public void unadvertise() {
|
||||
// build and send the rosbridge call
|
||||
String unadvertiseId = "unadvertise:" + this.name + ":"
|
||||
+ this.ros.nextId();
|
||||
JsonObject call = Json.createObjectBuilder()
|
||||
.add(JRosbridge.FIELD_OP, JRosbridge.OP_CODE_UNADVERTISE)
|
||||
.add(JRosbridge.FIELD_ID, unadvertiseId)
|
||||
.add(JRosbridge.FIELD_TOPIC, this.name).build();
|
||||
this.ros.send(call);
|
||||
|
||||
// set the flag indicating we are no longer registered
|
||||
this.isAdvertised = false;
|
||||
}
|
||||
|
||||
/**
|
||||
* Publish the given message to ROS on this topic. If the topic is not
|
||||
* advertised, it will be advertised first.
|
||||
*
|
||||
* @param message
|
||||
* The message to publish.
|
||||
*/
|
||||
public void publish(Message message) {
|
||||
// check if we have advertised yet.
|
||||
if (!this.isAdvertised()) {
|
||||
this.advertise();
|
||||
}
|
||||
|
||||
// build and send the rosbridge call
|
||||
String publishId = "publish:" + this.name + ":" + this.ros.nextId();
|
||||
//JsonObject call = Json.createObjectBuilder()
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,23 @@
|
||||
package edu.wpi.rail.jrosbridge.callback;
|
||||
|
||||
import edu.wpi.rail.jrosbridge.services.ServiceRequest;
|
||||
|
||||
/**
|
||||
* The CallServiceCallback interface defines a single method which will be called
|
||||
* when an incoming service request is received for an associated service request.
|
||||
*
|
||||
* @author Russell Toris - russell.toris@gmail.com
|
||||
* @version November 26, 2014
|
||||
*/
|
||||
public interface CallServiceCallback {
|
||||
|
||||
/**
|
||||
* This function is called when an incoming service request is received for
|
||||
* a given service request. No ROS type checking is done on the internal
|
||||
* data.
|
||||
*
|
||||
* @param request
|
||||
* The service request that was received.
|
||||
*/
|
||||
public void handleServiceCall(ServiceRequest request);
|
||||
}
|
||||
@ -0,0 +1,24 @@
|
||||
package edu.wpi.rail.jrosbridge.callback;
|
||||
|
||||
import edu.wpi.rail.jrosbridge.services.ServiceResponse;
|
||||
|
||||
/**
|
||||
* The ServiceCallback interface defines a single method which will be called
|
||||
* when an incoming service response is received for an associated service
|
||||
* request.
|
||||
*
|
||||
* @author Russell Toris - russell.toris@gmail.com
|
||||
* @version April 1, 2014
|
||||
*/
|
||||
public interface ServiceCallback {
|
||||
|
||||
/**
|
||||
* This function is called when an incoming service response is received for
|
||||
* a given service request. No ROS type checking is done on the internal
|
||||
* data. A flag indicating if the call was successful is given.
|
||||
*
|
||||
* @param response
|
||||
* The service response that was received.
|
||||
*/
|
||||
public void handleServiceResponse(ServiceResponse response);
|
||||
}
|
||||
@ -0,0 +1,23 @@
|
||||
package edu.wpi.rail.jrosbridge.callback;
|
||||
|
||||
|
||||
import com.example.testdemo.ros.message.Message;
|
||||
|
||||
/**
|
||||
* The TopicCallback interface defines a single method which will be called when
|
||||
* an incoming message is received for an associated topic.
|
||||
*
|
||||
* @author Russell Toris - russell.toris@gmail.com
|
||||
* @version April 1, 2014
|
||||
*/
|
||||
public interface TopicCallback {
|
||||
|
||||
/**
|
||||
* This function is called when an incoming message is received for a given
|
||||
* topic. No ROS type checking is done on the internal message data.
|
||||
*
|
||||
* @param message
|
||||
* The message that was received.
|
||||
*/
|
||||
public void handleMessage(Message message);
|
||||
}
|
||||
@ -0,0 +1,41 @@
|
||||
package edu.wpi.rail.jrosbridge.handler;
|
||||
|
||||
import javax.websocket.Session;
|
||||
|
||||
/**
|
||||
* The RosHandler interface defines the methods that will be called during
|
||||
* certain events in the Ros connection object.
|
||||
*
|
||||
* @author Russell Toris - russell.toris@gmail.com
|
||||
* @version April 1, 2014
|
||||
*/
|
||||
public interface RosHandler {
|
||||
|
||||
/**
|
||||
* Handle the connection event. This occurs during a successful connection
|
||||
* to rosbridge.
|
||||
*
|
||||
* @param session
|
||||
* The session associated with the connection.
|
||||
*/
|
||||
public void handleConnection(Session session);
|
||||
|
||||
/**
|
||||
* Handle the disconnection event. This occurs during a successful
|
||||
* disconnection from rosbridge.
|
||||
*
|
||||
* @param session
|
||||
* The session associated with the disconnection.
|
||||
*/
|
||||
public void handleDisconnection(Session session);
|
||||
|
||||
/**
|
||||
* Handle the error event.
|
||||
*
|
||||
* @param session
|
||||
* The session associated with the error.
|
||||
* @param t
|
||||
* The error.
|
||||
*/
|
||||
public void handleError(Session session, Throwable t);
|
||||
}
|
||||
@ -0,0 +1,166 @@
|
||||
package edu.wpi.rail.jrosbridge.primitives;
|
||||
|
||||
import java.io.StringReader;
|
||||
|
||||
import javax.json.Json;
|
||||
import javax.json.JsonObject;
|
||||
|
||||
/**
|
||||
* The ROS duration primitive.
|
||||
*
|
||||
* @author Russell Toris - russell.toris@gmail.com
|
||||
* @version April 1, 2014
|
||||
*/
|
||||
public class Duration extends TimeBase<Duration> {
|
||||
|
||||
/**
|
||||
* The primitive type.
|
||||
*/
|
||||
public static final String TYPE = "duration";
|
||||
|
||||
/**
|
||||
* Create a new Duration with a default of 0.
|
||||
*/
|
||||
public Duration() {
|
||||
super(Duration.TYPE);
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a new Duration with the given seconds and nanoseconds values.
|
||||
*
|
||||
* @param secs
|
||||
* The seconds value of this duration.
|
||||
* @param nsecs
|
||||
* The nanoseconds value of this duration.
|
||||
*/
|
||||
public Duration(int secs, int nsecs) {
|
||||
super(secs, nsecs, Duration.TYPE);
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a new Duration with the given duration in seconds (and partial
|
||||
* seconds).
|
||||
*
|
||||
* @param sec
|
||||
* The duration in seconds.
|
||||
*/
|
||||
public Duration(double sec) {
|
||||
super(sec, Duration.TYPE);
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a new Duration with the given duration in nanoseconds.
|
||||
*
|
||||
* @param sec
|
||||
* The duration in nanoseconds.
|
||||
*/
|
||||
public Duration(long nano) {
|
||||
super(nano, Duration.TYPE);
|
||||
}
|
||||
|
||||
/**
|
||||
* Add the given Duration to this Duration and return a new Duration with
|
||||
* that value.
|
||||
*
|
||||
* @param d
|
||||
* The Duration to add.
|
||||
* @return A new Duration with the new value.
|
||||
*/
|
||||
@Override
|
||||
public Duration add(Duration d) {
|
||||
return new Duration(this.toSec() + d.toSec());
|
||||
}
|
||||
|
||||
/**
|
||||
* Subtract the given Duration from this Duration and return a new Duration
|
||||
* with that value.
|
||||
*
|
||||
* @param d
|
||||
* The Duration to subtract.
|
||||
* @return A new Duration with the new value.
|
||||
*/
|
||||
@Override
|
||||
public Duration subtract(Duration d) {
|
||||
return new Duration(this.toSec() - d.toSec());
|
||||
}
|
||||
|
||||
/**
|
||||
* Attempt to sleep for the duration specified in this object.
|
||||
*
|
||||
* @return If the sleep was successful.
|
||||
*/
|
||||
public boolean sleep() {
|
||||
try {
|
||||
Thread.sleep(
|
||||
(this.secs * TimeBase.SECS_TO_MILLI)
|
||||
+ ((long) ((double) this.nsecs / (double) TimeBase.MILLI_TO_NSECS)),
|
||||
this.nsecs % (int) TimeBase.MILLI_TO_NSECS);
|
||||
return true;
|
||||
} catch (InterruptedException e) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a clone of this Duration.
|
||||
*/
|
||||
@Override
|
||||
public Duration clone() {
|
||||
return new Duration(this.secs, this.nsecs);
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a new Duration message based on the given seconds.
|
||||
*
|
||||
* @param sec
|
||||
* The duration in seconds.
|
||||
*
|
||||
* @return The new Duration primitive.
|
||||
*/
|
||||
public static Duration fromSec(double sec) {
|
||||
return new Duration(sec);
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a new Duration message based on the given nanoseconds.
|
||||
*
|
||||
* @param nano
|
||||
* The duration in nanoseconds.
|
||||
*
|
||||
* @return The new Duration primitive.
|
||||
*/
|
||||
public static Duration fromNano(long nano) {
|
||||
return new Duration(nano);
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a new Duration based on the given JSON string. Any missing values
|
||||
* will be set to their defaults.
|
||||
*
|
||||
* @param jsonString
|
||||
* The JSON string to parse.
|
||||
* @return A Duration message based on the given JSON string.
|
||||
*/
|
||||
public static Duration fromJsonString(String jsonString) {
|
||||
// convert to a JSON object
|
||||
return Duration.fromJsonObject(Json.createReader(
|
||||
new StringReader(jsonString)).readObject());
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a new Duration based on the given JSON object. Any missing values
|
||||
* will be set to their defaults.
|
||||
*
|
||||
* @param jsonObject
|
||||
* The JSON object to parse.
|
||||
* @return A Duration message based on the given JSON object.
|
||||
*/
|
||||
public static Duration fromJsonObject(JsonObject jsonObject) {
|
||||
// check the fields
|
||||
int secs = jsonObject.containsKey(Duration.FIELD_SECS) ? jsonObject
|
||||
.getInt(Duration.FIELD_SECS) : 0;
|
||||
int nsecs = jsonObject.containsKey(Duration.FIELD_NSECS) ? jsonObject
|
||||
.getInt(Duration.FIELD_NSECS) : 0;
|
||||
return new Duration(secs, nsecs);
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,313 @@
|
||||
package edu.wpi.rail.jrosbridge.primitives;
|
||||
|
||||
import java.io.StringReader;
|
||||
import java.math.BigInteger;
|
||||
|
||||
import javax.json.Json;
|
||||
import javax.json.JsonObject;
|
||||
|
||||
import edu.wpi.rail.jrosbridge.JsonWrapper;
|
||||
|
||||
/**
|
||||
* Primitive objects are used as a wrapper for non-native ROS primitives. These
|
||||
* primitives act as wrappers around JSON objects.
|
||||
*
|
||||
* This class also contains static functions for dealing with ROS' unsigned
|
||||
* numbers.
|
||||
*
|
||||
* @author Russell Toris - russell.toris@gmail.com
|
||||
* @version April 1, 2014
|
||||
*/
|
||||
public abstract class Primitive extends JsonWrapper {
|
||||
|
||||
/**
|
||||
* The String representation of an empty primitive in JSON.
|
||||
*/
|
||||
public static final String EMPTY_MESSAGE = JsonWrapper.EMPTY_JSON;
|
||||
|
||||
private String primitiveType;
|
||||
|
||||
/**
|
||||
* Create a Primitive based on the given String representation of a JSON
|
||||
* object.
|
||||
*
|
||||
* @param jsonString
|
||||
* The JSON String to parse.
|
||||
* @param primitiveType
|
||||
* The type of the primitive (e.g., "geometry_msgs/Twist").
|
||||
*/
|
||||
public Primitive(String jsonString, String primitiveType) {
|
||||
// parse and pass it to the JSON constructor
|
||||
this(Json.createReader(new StringReader(jsonString)).readObject(),
|
||||
primitiveType);
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a Primitive based on the given JSON object.
|
||||
*
|
||||
* @param jsonObject
|
||||
* The JSON object containing the primitive data.
|
||||
* @param primitiveType
|
||||
* The type of the primitive (e.g., "time").
|
||||
*/
|
||||
public Primitive(JsonObject jsonObject, String primitiveType) {
|
||||
// setup the JSON information
|
||||
super(jsonObject);
|
||||
// set the type
|
||||
this.primitiveType = primitiveType;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the type of the primitive.
|
||||
*
|
||||
* @return The type of the primitive.
|
||||
*/
|
||||
public String getPrimitiveType() {
|
||||
return this.primitiveType;
|
||||
}
|
||||
|
||||
/**
|
||||
* Set the type of the primitive.
|
||||
*
|
||||
* @param primitiveType
|
||||
* The type of the primitive (e.g., "time").
|
||||
*/
|
||||
public void setPrimitiveType(String primitiveType) {
|
||||
this.primitiveType = primitiveType;
|
||||
}
|
||||
|
||||
/**
|
||||
* Convert the given value to an unsigned 8-bit unsigned integer. This
|
||||
* ignores the high 8-bits of the short.
|
||||
*
|
||||
* @param value
|
||||
* The value to convert.
|
||||
* @return The value encoded as an 8-bit unsigned integer.
|
||||
*/
|
||||
public static byte toUInt8(short value) {
|
||||
// zero out the high 8-bits
|
||||
short tmp = (short) ((value >> 8) << 8);
|
||||
return (byte) (value - tmp);
|
||||
}
|
||||
|
||||
/**
|
||||
* Convert the given values to unsigned 8-bit unsigned integers. This
|
||||
* ignores the high 8-bits of the input.
|
||||
*
|
||||
* @param values
|
||||
* The values to convert.
|
||||
* @return The values encoded as an 8-bit unsigned integer.
|
||||
*/
|
||||
public static byte[] toUInt8(short[] values) {
|
||||
byte[] tmp = new byte[values.length];
|
||||
for (int i = 0; i < tmp.length; i++) {
|
||||
tmp[i] = Primitive.toUInt8(values[i]);
|
||||
}
|
||||
return tmp;
|
||||
}
|
||||
|
||||
/**
|
||||
* Convert the given value in unsigned 8-bit representation into its actual
|
||||
* value. That is, all return values of this function will be positive.
|
||||
*
|
||||
* @param value
|
||||
* The unsigned 8-bit value to convert.
|
||||
* @return The value of the given 8-bit unsigned value.
|
||||
*/
|
||||
public static short fromUInt8(byte value) {
|
||||
return (short) (value & (short) 0xFF);
|
||||
}
|
||||
|
||||
/**
|
||||
* Convert the given values in unsigned 8-bit representation into their
|
||||
* actual values. That is, all return values of this function will be
|
||||
* positive.
|
||||
*
|
||||
* @param values
|
||||
* The unsigned 8-bit values to convert.
|
||||
* @return The values of the given 8-bit unsigned values.
|
||||
*/
|
||||
public static short[] fromUInt8(byte[] values) {
|
||||
short[] tmp = new short[values.length];
|
||||
for (int i = 0; i < tmp.length; i++) {
|
||||
tmp[i] = Primitive.fromUInt8(values[i]);
|
||||
}
|
||||
return tmp;
|
||||
}
|
||||
|
||||
/**
|
||||
* Convert the given value to an unsigned 16-bit unsigned integer. This
|
||||
* ignores the high 16-bits of the input.
|
||||
*
|
||||
* @param value
|
||||
* The value to convert.
|
||||
* @return The value encoded as an 16-bit unsigned integer.
|
||||
*/
|
||||
public static short toUInt16(int value) {
|
||||
// zero out the high 16-bits
|
||||
int tmp = (int) ((value >> 16) << 16);
|
||||
return (short) (value - tmp);
|
||||
}
|
||||
|
||||
/**
|
||||
* Convert the given values to unsigned 16-bit unsigned integers. This
|
||||
* ignores the high 16-bits of the input.
|
||||
*
|
||||
* @param values
|
||||
* The values to convert.
|
||||
* @return The values encoded as an 16-bit unsigned integer.
|
||||
*/
|
||||
public static short[] toUInt16(int[] values) {
|
||||
short[] tmp = new short[values.length];
|
||||
for (int i = 0; i < tmp.length; i++) {
|
||||
tmp[i] = Primitive.toUInt16(values[i]);
|
||||
}
|
||||
return tmp;
|
||||
}
|
||||
|
||||
/**
|
||||
* Convert the given value in unsigned 16-bit representation into its actual
|
||||
* value. That is, all return values of this function will be positive.
|
||||
*
|
||||
* @param value
|
||||
* The unsigned 16-bit value to convert.
|
||||
* @return The value of the given 16-bit unsigned value.
|
||||
*/
|
||||
public static int fromUInt16(short value) {
|
||||
return (int) (value & 0xFFFF);
|
||||
}
|
||||
|
||||
/**
|
||||
* Convert the given values in unsigned 16-bit representation into their
|
||||
* actual values. That is, all return values of this function will be
|
||||
* positive.
|
||||
*
|
||||
* @param values
|
||||
* The unsigned 16-bit values to convert.
|
||||
* @return The values of the given 16-bit unsigned values.
|
||||
*/
|
||||
public static int[] fromUInt16(short[] values) {
|
||||
int[] tmp = new int[values.length];
|
||||
for (int i = 0; i < tmp.length; i++) {
|
||||
tmp[i] = Primitive.fromUInt16(values[i]);
|
||||
}
|
||||
return tmp;
|
||||
}
|
||||
|
||||
/**
|
||||
* Convert the given value to an unsigned 32-bit unsigned integer. This
|
||||
* ignores the high 32-bits of the input.
|
||||
*
|
||||
* @param value
|
||||
* The value to convert.
|
||||
* @return The value encoded as an 32-bit unsigned integer.
|
||||
*/
|
||||
public static int toUInt32(long value) {
|
||||
// zero out the high 32-bits
|
||||
long tmp = (long) ((value >> 32) << 32);
|
||||
return (int) (value - tmp);
|
||||
}
|
||||
|
||||
/**
|
||||
* Convert the given values to unsigned 32-bit unsigned integers. This
|
||||
* ignores the high 32-bits of the input.
|
||||
*
|
||||
* @param values
|
||||
* The values to convert.
|
||||
* @return The values encoded as an 32-bit unsigned integer.
|
||||
*/
|
||||
public static int[] toUInt32(long[] values) {
|
||||
int[] tmp = new int[values.length];
|
||||
for (int i = 0; i < tmp.length; i++) {
|
||||
tmp[i] = Primitive.toUInt32(values[i]);
|
||||
}
|
||||
return tmp;
|
||||
}
|
||||
|
||||
/**
|
||||
* Convert the given value in unsigned 32-bit representation into its actual
|
||||
* value. That is, all return values of this function will be positive.
|
||||
*
|
||||
* @param value
|
||||
* The unsigned 32-bit value to convert.
|
||||
* @return The value of the given 32-bit unsigned value.
|
||||
*/
|
||||
public static long fromUInt32(int value) {
|
||||
return (long) (value & 0xFFFFFFFFL);
|
||||
}
|
||||
|
||||
/**
|
||||
* Convert the given values in unsigned 32-bit representation into their
|
||||
* actual values. That is, all return values of this function will be
|
||||
* positive.
|
||||
*
|
||||
* @param values
|
||||
* The unsigned 32-bit values to convert.
|
||||
* @return The values of the given 64-bit unsigned values.
|
||||
*/
|
||||
public static long[] fromUInt32(int[] values) {
|
||||
long[] tmp = new long[values.length];
|
||||
for (int i = 0; i < tmp.length; i++) {
|
||||
tmp[i] = Primitive.fromUInt32(values[i]);
|
||||
}
|
||||
return tmp;
|
||||
}
|
||||
|
||||
/**
|
||||
* Convert the given value to an unsigned 64-bit unsigned integer. This
|
||||
* ignores the high 64-bits of the input.
|
||||
*
|
||||
* @param value
|
||||
* The value to convert.
|
||||
* @return The value encoded as an 64-bit unsigned integer.
|
||||
*/
|
||||
public static long toUInt64(BigInteger value) {
|
||||
return value.longValue();
|
||||
}
|
||||
|
||||
/**
|
||||
* Convert the given values to unsigned 64-bit unsigned integers. This
|
||||
* ignores the high 64-bits of the input.
|
||||
*
|
||||
* @param values
|
||||
* The values to convert.
|
||||
* @return The values encoded as an 64-bit unsigned integer.
|
||||
*/
|
||||
public static long[] toUInt64(BigInteger[] values) {
|
||||
long[] tmp = new long[values.length];
|
||||
for (int i = 0; i < tmp.length; i++) {
|
||||
tmp[i] = Primitive.toUInt64(values[i]);
|
||||
}
|
||||
return tmp;
|
||||
}
|
||||
|
||||
/**
|
||||
* Convert the given value in unsigned 64-bit representation into its actual
|
||||
* value. That is, all return values of this function will be positive.
|
||||
*
|
||||
* @param value
|
||||
* The unsigned 64-bit value to convert.
|
||||
* @return The value of the given 64-bit unsigned value.
|
||||
*/
|
||||
public static BigInteger fromUInt64(long value) {
|
||||
return BigInteger.valueOf(value).and(
|
||||
BigInteger.ONE.shiftLeft(64).subtract(BigInteger.ONE));
|
||||
}
|
||||
|
||||
/**
|
||||
* Convert the given values in unsigned 64-bit representation into their
|
||||
* actual values. That is, all return values of this function will be
|
||||
* positive.
|
||||
*
|
||||
* @param values
|
||||
* The unsigned 64-bit values to convert.
|
||||
* @return The values of the given 64-bit unsigned values.
|
||||
*/
|
||||
public static BigInteger[] fromUInt64(long[] values) {
|
||||
BigInteger[] tmp = new BigInteger[values.length];
|
||||
for (int i = 0; i < tmp.length; i++) {
|
||||
tmp[i] = Primitive.fromUInt64(values[i]);
|
||||
}
|
||||
return tmp;
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,203 @@
|
||||
package edu.wpi.rail.jrosbridge.primitives;
|
||||
|
||||
import java.io.StringReader;
|
||||
import java.util.Calendar;
|
||||
import java.util.Date;
|
||||
|
||||
import javax.json.Json;
|
||||
import javax.json.JsonObject;
|
||||
|
||||
/**
|
||||
* The ROS time primitive.
|
||||
*
|
||||
* @author Russell Toris - russell.toris@gmail.com
|
||||
* @version April 1, 2014
|
||||
*/
|
||||
public class Time extends TimeBase<Time> {
|
||||
|
||||
/**
|
||||
* The primitive type.
|
||||
*/
|
||||
public static final String TYPE = "time";
|
||||
|
||||
/**
|
||||
* Create a new Time with a default of 0.
|
||||
*/
|
||||
public Time() {
|
||||
super(Time.TYPE);
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a new Time with the given seconds and nanoseconds values.
|
||||
*
|
||||
* @param secs
|
||||
* The seconds value of this time.
|
||||
* @param nsecs
|
||||
* The nanoseconds value of this time.
|
||||
*/
|
||||
public Time(int secs, int nsecs) {
|
||||
super(secs, nsecs, Time.TYPE);
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a new Time with the given time in seconds (and partial seconds).
|
||||
*
|
||||
* @param sec
|
||||
* The time in seconds.
|
||||
*/
|
||||
public Time(double sec) {
|
||||
super(sec, Time.TYPE);
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a new Time with the given time in nanoseconds.
|
||||
*
|
||||
* @param sec
|
||||
* The time in nanoseconds.
|
||||
*/
|
||||
public Time(long nano) {
|
||||
super(nano, Time.TYPE);
|
||||
}
|
||||
|
||||
/**
|
||||
* Add the given Time to this Time and return a new Time with that value.
|
||||
*
|
||||
* @param t
|
||||
* The Time to add.
|
||||
* @return A new Time with the new value.
|
||||
*/
|
||||
@Override
|
||||
public Time add(Time t) {
|
||||
return new Time(this.toSec() + t.toSec());
|
||||
}
|
||||
|
||||
/**
|
||||
* Subtract the given Time from this Time and return a new Time with that
|
||||
* value.
|
||||
*
|
||||
* @param t
|
||||
* The Time to subtract.
|
||||
* @return A new Time with the new value.
|
||||
*/
|
||||
@Override
|
||||
public Time subtract(Time t) {
|
||||
return new Time(this.toSec() - t.toSec());
|
||||
}
|
||||
|
||||
/**
|
||||
* Check if this Time is valid. A time is valid if it is non-zero.
|
||||
*
|
||||
* @return If this Time is valid.
|
||||
*/
|
||||
public boolean isValid() {
|
||||
return !this.isZero();
|
||||
}
|
||||
|
||||
/**
|
||||
* Crate a new Java Date object based on this message.
|
||||
*
|
||||
* @return A new Java Date object based on this message.
|
||||
*/
|
||||
public Date toDate() {
|
||||
Calendar c = Calendar.getInstance();
|
||||
c.setTimeInMillis((long) (this.toSec() * (double) TimeBase.SECS_TO_MILLI));
|
||||
return c.getTime();
|
||||
}
|
||||
|
||||
/**
|
||||
* Sleep until the given time.
|
||||
*
|
||||
* @param t
|
||||
* The time to sleep until.
|
||||
* @return If the sleep was successful.
|
||||
*/
|
||||
public static boolean sleepUntil(Time t) {
|
||||
// use a duration to sleep with
|
||||
return Duration.fromSec(t.subtract(Time.now()).toSec()).sleep();
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a clone of this Time.
|
||||
*/
|
||||
@Override
|
||||
public Time clone() {
|
||||
return new Time(this.secs, this.nsecs);
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a new Time message based on the current system time. Note that
|
||||
* this might not match the current ROS time.
|
||||
*
|
||||
* @return The new Time message.
|
||||
*/
|
||||
public static Time now() {
|
||||
return Time.fromSec(((double) System.currentTimeMillis())
|
||||
* TimeBase.MILLI_TO_SECS);
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a new Time message based on the given seconds.
|
||||
*
|
||||
* @param sec
|
||||
* The time in seconds.
|
||||
*
|
||||
* @return The new Time primitive.
|
||||
*/
|
||||
public static Time fromSec(double sec) {
|
||||
return new Time(sec);
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a new Time message based on the given nanoseconds.
|
||||
*
|
||||
* @param nano
|
||||
* The time in nanoseconds.
|
||||
*
|
||||
* @return The new Time primitive.
|
||||
*/
|
||||
public static Time fromNano(long nano) {
|
||||
return new Time(nano);
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a new Time from the given Java Data object.
|
||||
*
|
||||
* @param date
|
||||
* The Date to create a Time from.
|
||||
* @return The resulting Time primitive.
|
||||
*/
|
||||
public static Time fromDate(Date date) {
|
||||
return Time.fromSec(((double) date.getTime()) * TimeBase.MILLI_TO_SECS);
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a new Time based on the given JSON string. Any missing values will
|
||||
* be set to their defaults.
|
||||
*
|
||||
* @param jsonString
|
||||
* The JSON string to parse.
|
||||
* @return A Time message based on the given JSON string.
|
||||
*/
|
||||
public static Time fromJsonString(String jsonString) {
|
||||
// convert to a JSON object
|
||||
return Time.fromJsonObject(Json.createReader(
|
||||
new StringReader(jsonString)).readObject());
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a new Time based on the given JSON object. Any missing values will
|
||||
* be set to their defaults.
|
||||
*
|
||||
* @param jsonObject
|
||||
* The JSON object to parse.
|
||||
* @return A Time message based on the given JSON object.
|
||||
*/
|
||||
public static Time fromJsonObject(JsonObject jsonObject) {
|
||||
// check the fields
|
||||
int secs = jsonObject.containsKey(Time.FIELD_SECS) ? jsonObject
|
||||
.getInt(Time.FIELD_SECS) : 0;
|
||||
int nsecs = jsonObject.containsKey(Time.FIELD_NSECS) ? jsonObject
|
||||
.getInt(Time.FIELD_NSECS) : 0;
|
||||
return new Time(secs, nsecs);
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,202 @@
|
||||
package edu.wpi.rail.jrosbridge.primitives;
|
||||
|
||||
import javax.json.Json;
|
||||
|
||||
/**
|
||||
* The TimeBase class is an abstract implementation of common time/duration
|
||||
* primitive functions for ROS time and duration primitives. Since these
|
||||
* primitives are serialized like messages, they are immutable.
|
||||
*
|
||||
* @author Russell Toris - russell.toris@gmail.com
|
||||
* @version April 1, 2014
|
||||
*
|
||||
* @param <T>
|
||||
* The type of TimeBase used in add and subtract.
|
||||
*/
|
||||
public abstract class TimeBase<T extends Primitive> extends Primitive implements
|
||||
Comparable<TimeBase<T>> {
|
||||
|
||||
/**
|
||||
* The name of the seconds field for the Primitive.
|
||||
*/
|
||||
public static final String FIELD_SECS = "secs";
|
||||
|
||||
/**
|
||||
* The name of the nanoseconds field for the Primitive.
|
||||
*/
|
||||
public static final String FIELD_NSECS = "nsecs";
|
||||
|
||||
/**
|
||||
* The number of milliseconds in a second.
|
||||
*/
|
||||
protected static final long SECS_TO_MILLI = 1000;
|
||||
|
||||
/**
|
||||
* The fraction of a second in a millisecond.
|
||||
*/
|
||||
protected static final double MILLI_TO_SECS = 0.001;
|
||||
|
||||
/**
|
||||
* The number of nanoseconds in a second.
|
||||
*/
|
||||
protected static final long SECS_TO_NSECS = 1000000000l;
|
||||
|
||||
/**
|
||||
* The fraction of a second in a nanosecond.
|
||||
*/
|
||||
protected static final double NSECS_TO_SECS = 1e-9;
|
||||
|
||||
/**
|
||||
* The number of milliseconds in a second.
|
||||
*/
|
||||
protected static final long MILLI_TO_NSECS = 1000000;
|
||||
|
||||
/**
|
||||
* The number of milliseconds in a second.
|
||||
*/
|
||||
protected static final double NSECS_TO_MILLI = 1e-6;
|
||||
|
||||
public final int secs, nsecs;
|
||||
|
||||
/**
|
||||
* Create an empty TimeBase with the given type field.
|
||||
*
|
||||
* @param type
|
||||
* The type of primitive.
|
||||
*/
|
||||
public TimeBase(String type) {
|
||||
this(0, 0, type);
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a new TimeBase with the given time in seconds (and partial
|
||||
* seconds).
|
||||
*
|
||||
* @param sec
|
||||
* The time in seconds.
|
||||
* @param type
|
||||
* The type of TimeBase primitive.
|
||||
*/
|
||||
public TimeBase(double sec, String type) {
|
||||
this((long) (sec * TimeBase.SECS_TO_NSECS), type);
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a new TimeBase with the given time in nanoseconds.
|
||||
*
|
||||
* @param nano
|
||||
* The time in nanoseconds.
|
||||
* @param type
|
||||
* The type of TimeBase primitive.
|
||||
*/
|
||||
public TimeBase(long nano, String type) {
|
||||
// extract seconds and nanoseconds
|
||||
this((int) (nano / TimeBase.SECS_TO_NSECS),
|
||||
(int) (nano % TimeBase.SECS_TO_NSECS), type);
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a new TimeBase with the given time in seconds and nanoseconds.
|
||||
*
|
||||
* @param secs
|
||||
* The amount of seconds.
|
||||
* @param nsecs
|
||||
* The amount of additional nanoseconds.
|
||||
* @param type
|
||||
* The type of TimeBase primitive.
|
||||
*/
|
||||
public TimeBase(int secs, int nsecs, String type) {
|
||||
// build the JSON object
|
||||
super(Json.createObjectBuilder().add(Duration.FIELD_SECS, secs)
|
||||
.add(Duration.FIELD_NSECS, nsecs).build(), type);
|
||||
this.secs = secs;
|
||||
this.nsecs = nsecs;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the seconds value of this TimeBase.
|
||||
*
|
||||
* @return The seconds value of this TimeBase.
|
||||
*/
|
||||
public int getSecs() {
|
||||
return this.secs;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the nanoseconds value of this TimeBase.
|
||||
*
|
||||
* @return The nanoseconds value of this TimeBase.
|
||||
*/
|
||||
public int getNsecs() {
|
||||
return this.nsecs;
|
||||
}
|
||||
|
||||
/**
|
||||
* Check if the value of this TimeBase is zero.
|
||||
*
|
||||
* @return If the value of this TimeBase is zero.
|
||||
*/
|
||||
public boolean isZero() {
|
||||
return (this.secs + this.nsecs) == 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Convert this TimeBase to seconds (and partial seconds).
|
||||
*
|
||||
* @return This TimeBase to seconds (and partial seconds).
|
||||
*/
|
||||
public double toSec() {
|
||||
return this.secs + (TimeBase.NSECS_TO_SECS * (double) this.nsecs);
|
||||
}
|
||||
|
||||
/**
|
||||
* Convert this TimeBase to nanoseconds.
|
||||
*
|
||||
* @return This TimeBase to nanoseconds.
|
||||
*/
|
||||
public long toNSec() {
|
||||
return ((long) (this.secs * TimeBase.SECS_TO_NSECS))
|
||||
+ ((long) this.nsecs);
|
||||
}
|
||||
|
||||
/**
|
||||
* Compare the given TimeBase object to this one.
|
||||
*
|
||||
* @param t
|
||||
* The TimeBase to compare to.
|
||||
* @return 0 if the values are equal, less than 0 if t is less that this
|
||||
* TimeBase, and greater than 0 otherwise.
|
||||
*/
|
||||
@Override
|
||||
public int compareTo(TimeBase<T> t) {
|
||||
return Double.compare(this.toSec(), t.toSec());
|
||||
}
|
||||
|
||||
/**
|
||||
* Add the given type to this TimeBase and return a new object with that
|
||||
* value.
|
||||
*
|
||||
* @param t
|
||||
* Add the given type to this TimeBase.
|
||||
* @return A new object with the new value.
|
||||
*/
|
||||
public abstract T add(T t);
|
||||
|
||||
/**
|
||||
* Subtract the given type to this TimeBase and return a new object with
|
||||
* that value.
|
||||
*
|
||||
* @param t
|
||||
* Subtract the given type to this TimeBase.
|
||||
* @return A new object with the new value.
|
||||
*/
|
||||
public abstract T subtract(T t);
|
||||
|
||||
/**
|
||||
* Create a clone of this TimeBase.
|
||||
*
|
||||
* @return A clone of this TimeBase.
|
||||
*/
|
||||
@Override
|
||||
public abstract T clone();
|
||||
}
|
||||
@ -0,0 +1,135 @@
|
||||
package edu.wpi.rail.jrosbridge.services;
|
||||
|
||||
import java.io.StringReader;
|
||||
|
||||
import javax.json.Json;
|
||||
import javax.json.JsonObject;
|
||||
|
||||
import edu.wpi.rail.jrosbridge.JsonWrapper;
|
||||
|
||||
/**
|
||||
* ServiceRequest objects are used for making a request to a service. These
|
||||
* service requests act as wrappers around JSON objects. Service request data is
|
||||
* immutable.
|
||||
*
|
||||
* @author Russell Toris - russell.toris@gmail.com
|
||||
* @version April 1, 2014
|
||||
*/
|
||||
public class ServiceRequest extends JsonWrapper {
|
||||
|
||||
/**
|
||||
* The String representation of an empty service request in JSON.
|
||||
*/
|
||||
public static final String EMPTY_MESSAGE = JsonWrapper.EMPTY_JSON;
|
||||
|
||||
private String serviceRequestType;
|
||||
private String id;
|
||||
|
||||
/**
|
||||
* Create a new, empty service request. The type will be set to the empty
|
||||
* string.
|
||||
*/
|
||||
public ServiceRequest() {
|
||||
this(ServiceRequest.EMPTY_MESSAGE, "");
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a ServiceRequest based on the given String representation of a
|
||||
* JSON object. The type will be set to the empty string.
|
||||
*
|
||||
* @param jsonString
|
||||
* The JSON String to parse.
|
||||
*/
|
||||
public ServiceRequest(String jsonString) {
|
||||
this(jsonString, "");
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a ServiceRequest based on the given String representation of a
|
||||
* JSON object.
|
||||
*
|
||||
* @param jsonString
|
||||
* The JSON String to parse.
|
||||
* @param serviceRequestType
|
||||
* The type of the service request (e.g., "std_srvs/Empty").
|
||||
*/
|
||||
public ServiceRequest(String jsonString, String serviceRequestType) {
|
||||
// parse and pass it to the JSON constructor
|
||||
this(Json.createReader(new StringReader(jsonString)).readObject(),
|
||||
serviceRequestType);
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a ServiceRequest based on the given JSON object. The type will be
|
||||
* set to the empty string.
|
||||
*
|
||||
* @param jsonObject
|
||||
* The JSON object containing the service request data.
|
||||
*/
|
||||
public ServiceRequest(JsonObject jsonObject) {
|
||||
// setup the JSON information
|
||||
this(jsonObject, "");
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a ServiceRequest based on the given JSON object.
|
||||
*
|
||||
* @param jsonObject
|
||||
* The JSON object containing the service request data.
|
||||
* @param serviceRequestType
|
||||
* The type of the service request (e.g., "std_srvs/Empty").
|
||||
*/
|
||||
public ServiceRequest(JsonObject jsonObject, String serviceRequestType) {
|
||||
// setup the JSON information
|
||||
super(jsonObject);
|
||||
// set the type
|
||||
this.serviceRequestType = serviceRequestType;
|
||||
this.id = "";
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the type of the service request if one was set.
|
||||
*
|
||||
* @return The type of the service request.
|
||||
*/
|
||||
public String getServiceRequestType() {
|
||||
return this.serviceRequestType;
|
||||
}
|
||||
|
||||
/**
|
||||
* Set the type of the service request.
|
||||
*
|
||||
* @param serviceRequestType
|
||||
* The type of the service request (e.g., "std_srvs/Empty").
|
||||
*/
|
||||
public void setServiceRequestType(String serviceRequestType) {
|
||||
this.serviceRequestType = serviceRequestType;
|
||||
}
|
||||
|
||||
/**
|
||||
* Set the ID of the service request.
|
||||
*
|
||||
* @param id
|
||||
* The new ID.
|
||||
*/
|
||||
public void setId(String id) {
|
||||
this.id = id;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the ID of the service request.
|
||||
*
|
||||
* @return The ID.
|
||||
*/
|
||||
public String getId() {
|
||||
return this.id;
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a clone of this ServiceRequest.
|
||||
*/
|
||||
@Override
|
||||
public ServiceRequest clone() {
|
||||
return new ServiceRequest(this.toJsonObject(), this.serviceRequestType);
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,141 @@
|
||||
package edu.wpi.rail.jrosbridge.services;
|
||||
|
||||
import java.io.StringReader;
|
||||
|
||||
import javax.json.Json;
|
||||
import javax.json.JsonObject;
|
||||
|
||||
import edu.wpi.rail.jrosbridge.JsonWrapper;
|
||||
|
||||
/**
|
||||
* ServiceResponse objects are used for making a response to a service. These
|
||||
* service responses act as wrappers around JSON objects. Service response data
|
||||
* is immutable.
|
||||
*
|
||||
* @author Russell Toris - russell.toris@gmail.com
|
||||
* @version April 1, 2014
|
||||
*/
|
||||
public class ServiceResponse extends JsonWrapper {
|
||||
|
||||
/**
|
||||
* The String representation of an empty service response in JSON.
|
||||
*/
|
||||
public static final String EMPTY_MESSAGE = JsonWrapper.EMPTY_JSON;
|
||||
|
||||
private String serviceResponseType;
|
||||
private final boolean result;
|
||||
|
||||
/**
|
||||
* Create a new, empty service response. The type will be set to the empty
|
||||
* string.
|
||||
*/
|
||||
public ServiceResponse() {
|
||||
this(ServiceResponse.EMPTY_MESSAGE, "", true);
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a ServiceResponse based on the given String representation of a
|
||||
* JSON object. The type will be set to the empty string.
|
||||
*
|
||||
* @param jsonString
|
||||
* The JSON String to parse.
|
||||
* @param result
|
||||
* The result flag for the response (i.e., if the service server
|
||||
* returned a success).
|
||||
*/
|
||||
public ServiceResponse(String jsonString, boolean result) {
|
||||
this(jsonString, "", result);
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a ServiceResponse based on the given String representation of a
|
||||
* JSON object.
|
||||
*
|
||||
* @param jsonString
|
||||
* The JSON String to parse.
|
||||
* @param serviceResponseType
|
||||
* The type of the service response (e.g., "std_srvs/Empty").
|
||||
* @param result
|
||||
* The result flag for the response (i.e., if the service server
|
||||
* returned a success).
|
||||
*/
|
||||
public ServiceResponse(String jsonString, String serviceResponseType,
|
||||
boolean result) {
|
||||
// parse and pass it to the JSON constructor
|
||||
this(Json.createReader(new StringReader(jsonString)).readObject(),
|
||||
serviceResponseType, result);
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a ServiceResponse based on the given JSON object. The type will be
|
||||
* set to the empty string.
|
||||
*
|
||||
* @param jsonObject
|
||||
* The JSON object containing the service response data.
|
||||
* @param result
|
||||
* The result flag for the response (i.e., if the service server
|
||||
* returned a success).
|
||||
*/
|
||||
public ServiceResponse(JsonObject jsonObject, boolean result) {
|
||||
// setup the JSON information
|
||||
this(jsonObject, "", result);
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a ServiceResponse based on the given JSON object.
|
||||
*
|
||||
* @param jsonObject
|
||||
* The JSON object containing the service response data.
|
||||
* @param serviceResponseType
|
||||
* The type of the service response (e.g., "std_srvs/Empty").
|
||||
* @param result
|
||||
* The result flag for the response (i.e., if the service server
|
||||
* returned a success).
|
||||
*/
|
||||
public ServiceResponse(JsonObject jsonObject, String serviceResponseType,
|
||||
boolean result) {
|
||||
// setup the JSON information
|
||||
super(jsonObject);
|
||||
// set the type
|
||||
this.serviceResponseType = serviceResponseType;
|
||||
this.result = result;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the type of the service response if one was set.
|
||||
*
|
||||
* @return The type of the service response.
|
||||
*/
|
||||
public String getServiceResponseType() {
|
||||
return this.serviceResponseType;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the result flag of this response (i.e., if the service server
|
||||
* returned a success).
|
||||
*
|
||||
* @return The result flag for the response.
|
||||
*/
|
||||
public boolean getResult() {
|
||||
return this.result;
|
||||
}
|
||||
|
||||
/**
|
||||
* Set the type of the service response.
|
||||
*
|
||||
* @param serviceResponseType
|
||||
* The type of the service response (e.g., "std_srvs/Empty").
|
||||
*/
|
||||
public void setServiceResponseType(String serviceResponseType) {
|
||||
this.serviceResponseType = serviceResponseType;
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a clone of this ServiceResponse.
|
||||
*/
|
||||
@Override
|
||||
public ServiceResponse clone() {
|
||||
return new ServiceResponse(this.toJsonObject(),
|
||||
this.serviceResponseType, this.result);
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,157 @@
|
||||
package edu.wpi.rail.jrosbridge.services.std;
|
||||
|
||||
import javax.json.JsonObject;
|
||||
|
||||
import edu.wpi.rail.jrosbridge.services.ServiceRequest;
|
||||
import edu.wpi.rail.jrosbridge.services.ServiceResponse;
|
||||
|
||||
/**
|
||||
* The std_srvs/Empty service.
|
||||
*
|
||||
* @author Russell Toris -- russell.toris@gmail.com
|
||||
* @version April 1, 2014
|
||||
*/
|
||||
public class Empty {
|
||||
|
||||
/**
|
||||
* The service type.
|
||||
*/
|
||||
public static final String TYPE = "std_srvs/Empty";
|
||||
|
||||
/**
|
||||
* The service request for the Empty service.
|
||||
*
|
||||
* @author Russell Toris -- russell.toris@gmail.com
|
||||
* @version April 1, 2014
|
||||
*/
|
||||
public static class Request extends ServiceRequest {
|
||||
|
||||
/**
|
||||
* Create a new Empty ServiceRequest.
|
||||
*/
|
||||
public Request() {
|
||||
super(ServiceRequest.EMPTY_MESSAGE, Empty.TYPE);
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a clone of this Empty ServiceRequest.
|
||||
*/
|
||||
@Override
|
||||
public Request clone() {
|
||||
return new Request();
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a new Empty ServiceRequest based on the given JSON string. Any
|
||||
* missing values will be set to their defaults.
|
||||
*
|
||||
* @param jsonString
|
||||
* The JSON string to parse.
|
||||
* @return A Empty ServiceRequest based on the given JSON string.
|
||||
*/
|
||||
public static Request fromJsonString(String jsonString) {
|
||||
// convert to a ServiceRequest
|
||||
return Request.fromServiceRequest(new ServiceRequest(
|
||||
jsonString));
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a new Empty ServiceRequest based on the given ServiceRequest.
|
||||
* Any missing values will be set to their defaults.
|
||||
*
|
||||
* @param m
|
||||
* The ServiceRequest to parse.
|
||||
* @return A Empty ServiceRequest based on the given Message.
|
||||
*/
|
||||
public static Request fromServiceRequest(ServiceRequest req) {
|
||||
// get it from the JSON object
|
||||
return Request.fromJsonObject(req.toJsonObject());
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a new Empty ServiceRequest based on the given JSON object. Any
|
||||
* missing values will be set to their defaults.
|
||||
*
|
||||
* @param jsonObject
|
||||
* The JSON object to parse.
|
||||
* @return A Empty ServiceRequest based on the given JSON object.
|
||||
*/
|
||||
public static Request fromJsonObject(JsonObject jsonObject) {
|
||||
return new Request();
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* The service response for the Empty service.
|
||||
*
|
||||
* @author Russell Toris -- russell.toris@gmail.com
|
||||
* @version April 1, 2014
|
||||
*/
|
||||
public static class Response extends ServiceResponse {
|
||||
|
||||
/**
|
||||
* Create a new Empty ServiceResponse.
|
||||
*/
|
||||
public Response() {
|
||||
this(true);
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a new Empty ServiceResponse.
|
||||
*
|
||||
* @param result
|
||||
* The result flag for the response (i.e., if the service
|
||||
* server returned a success).
|
||||
*/
|
||||
public Response(boolean result) {
|
||||
super(ServiceResponse.EMPTY_MESSAGE, Empty.TYPE, result);
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a clone of this Empty ServiceResponse.
|
||||
*/
|
||||
@Override
|
||||
public Response clone() {
|
||||
return new Response();
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a new Empty ServiceResponse based on the given JSON string.
|
||||
* Any missing values will be set to their defaults.
|
||||
*
|
||||
* @param jsonString
|
||||
* The JSON string to parse.
|
||||
* @return A Empty ServiceResponse based on the given JSON string.
|
||||
*/
|
||||
public static Response fromJsonString(String jsonString) {
|
||||
// convert to a ServiceResponse
|
||||
return Response.fromServiceResponse(new ServiceResponse(
|
||||
jsonString, true));
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a new Empty ServiceResponse based on the given
|
||||
* ServiceResponse. Any missing values will be set to their defaults.
|
||||
*
|
||||
* @param m
|
||||
* The ServiceResponse to parse.
|
||||
* @return A Empty ServiceResponse based on the given Message.
|
||||
*/
|
||||
public static Response fromServiceResponse(ServiceResponse resp) {
|
||||
// get it from the JSON object
|
||||
return Response.fromJsonObject(resp.toJsonObject());
|
||||
}
|
||||
|
||||
/**
|
||||
* Create a new Empty ServiceResponse based on the given JSON object.
|
||||
* Any missing values will be set to their defaults.
|
||||
*
|
||||
* @param jsonObject
|
||||
* The JSON object to parse.
|
||||
* @return A Empty ServiceResponse based on the given JSON object.
|
||||
*/
|
||||
public static Response fromJsonObject(JsonObject jsonObject) {
|
||||
return new Response();
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -0,0 +1,26 @@
|
||||
Manifest-Version: 1.0
|
||||
Main-Class: com.example.testdemo.ServerApplication
|
||||
Class-Path: log4j-to-slf4j-2.14.1.jar selenium-support-3.141.59.jar jsr3
|
||||
05-1.3.9.jar okhttp-3.14.9.jar spring-boot-starter-json-2.5.4.jar sprin
|
||||
g-web-5.3.9.jar logback-core-1.2.5.jar tomcat-embed-core-9.0.52.jar spr
|
||||
ing-aop-5.3.9.jar animal-sniffer-annotations-1.14.jar error_prone_annot
|
||||
ations-2.1.3.jar selenium-ie-driver-3.141.59.jar jackson-datatype-jdk8-
|
||||
2.12.4.jar jackson-core-2.12.4.jar spring-boot-2.5.4.jar j2objc-annotat
|
||||
ions-1.1.jar tomcat-embed-el-9.0.52.jar guava-25.0-jre.jar spring-expre
|
||||
ssion-5.3.9.jar jul-to-slf4j-1.7.32.jar selenium-java-3.141.59.jar jack
|
||||
son-datatype-jsr310-2.12.4.jar spring-boot-starter-logging-2.5.4.jar sp
|
||||
ring-context-5.3.9.jar logback-classic-1.2.5.jar spring-messaging-5.3.9
|
||||
.jar selenium-opera-driver-3.141.59.jar selenium-chrome-driver-3.141.59
|
||||
.jar selenium-firefox-driver-3.141.59.jar okio-1.14.0.jar checker-compa
|
||||
t-qual-2.0.0.jar spring-boot-starter-tomcat-2.5.4.jar selenium-remote-d
|
||||
river-3.141.59.jar jakarta.annotation-api-1.3.5.jar selenium-api-3.141.
|
||||
59.jar jackson-databind-2.12.4.jar fastjson-1.2.73.jar snakeyaml-1.28.j
|
||||
ar selenium-edge-driver-3.141.59.jar spring-boot-starter-2.5.4.jar spri
|
||||
ng-websocket-5.3.9.jar spring-core-5.3.9.jar byte-buddy-1.10.22.jar jac
|
||||
kson-annotations-2.12.4.jar selenium-safari-driver-3.141.59.jar lombok-
|
||||
1.18.20.jar spring-webmvc-5.3.9.jar spring-beans-5.3.9.jar commons-exec
|
||||
-1.3.jar spring-jcl-5.3.9.jar log4j-api-2.14.1.jar spring-boot-starter-
|
||||
websocket-2.5.4.jar spring-boot-autoconfigure-2.5.4.jar jackson-module-
|
||||
parameter-names-2.12.4.jar slf4j-api-1.7.32.jar tomcat-embed-websocket-
|
||||
9.0.52.jar spring-boot-starter-web-2.5.4.jar
|
||||
|
||||
Loading…
Reference in new issue