4
4
using SimpleJSON ;
5
5
using ROSBridgeLib . std_msgs ;
6
6
using ROSBridgeLib . geometry_msgs ;
7
- // Need geometry_msgs/PoseStamped[]
7
+
8
+ /**
9
+ * Define a nav_msgs Path message. This has been hand-crafted from the corresponding
10
+ * nav_msgs message file.
11
+ *
12
+ * @author Miquel Massot Campos
13
+ */
8
14
9
15
namespace ROSBridgeLib {
10
16
namespace nav_msgs {
@@ -19,32 +25,47 @@ public PathMsg(JSONNode msg) {
19
25
_poses . Add ( new PoseStampedMsg ( msg [ "poses" ] [ i ] ) ) ;
20
26
}
21
27
}
22
-
23
- /* public CompressedImageMsg(JSONNode msg) {
24
- _format = msg ["format"];
25
- _header = new HeaderMsg (msg ["header"]);
26
- //_poses =
27
- } */
28
-
29
- /* public static string GetMessageType() {
30
- return "geometry_msgs/Twist";
31
- } */
32
-
33
- /* public HeaderMsg GetHeader() {
28
+
29
+ public static string GetMessageType ( ) {
30
+ return "nav_msgs/Path" ;
31
+ }
32
+
33
+ public HeaderMsg GetHeader ( ) {
34
34
return _header ;
35
- } */
35
+ }
36
+
37
+ public PoseStampedMsg GetPoseStamped ( int idx = 0 ) {
38
+ if ( idx < _poses . Count ) {
39
+ return _poses [ idx ] ;
40
+ } else {
41
+ return null ;
42
+ }
43
+ }
36
44
37
- /* public PoseStamped[] GetPoses() {
38
- return _poses;
39
- } */
40
- /*
41
45
public override string ToString ( ) {
42
- return "Twist [linear=" + _linear.ToString() + ", angular=" + _angular.ToString() + "]";
46
+ string array = "[" ;
47
+ for ( int i = 0 ; i < _poses . Count ; i ++ ) {
48
+ array = array + _poses [ i ] . ToString ( ) ;
49
+ if ( _poses . Count - i <= 1 )
50
+ array += "," ;
51
+ }
52
+ array += "]" ;
53
+
54
+ return "Path [header=" + _header . ToString ( )
55
+ + ", poses=" + array + "]" ;
43
56
}
44
-
57
+
45
58
public override string ToYAMLString ( ) {
46
- return "{\"linear\" : " + _linear.ToYAMLString() + ", \"angular\" : " + _angular.ToYAMLString() + "}";
47
- } */
59
+ string array = "{" ;
60
+ for ( int i = 0 ; i < _poses . Count ; i ++ ) {
61
+ array = array + _poses [ i ] . ToYAMLString ( ) ;
62
+ if ( _poses . Count - i <= 1 )
63
+ array += "," ;
64
+ }
65
+ array += "}" ;
66
+ return "{\" header\" : " + _header . ToYAMLString ( )
67
+ + ", \" poses\" : " + array + "}" ;
68
+ }
48
69
}
49
70
}
50
71
}
0 commit comments