From 27d9fcb95d4167b9dc54ceb944c289183c5a08b8 Mon Sep 17 00:00:00 2001 From: ben60523 Date: Wed, 19 Oct 2022 18:19:25 +0800 Subject: [PATCH 1/5] Fix some bugs --- geometry_msgs/PointMsg.cs | 4 ++++ geometry_msgs/PoseWithCovarianceMsg.cs | 6 +++--- geometry_msgs/QuaternionMsg.cs | 6 +++++- nav_msgs/PathMsg.cs | 2 +- 4 files changed, 13 insertions(+), 5 deletions(-) diff --git a/geometry_msgs/PointMsg.cs b/geometry_msgs/PointMsg.cs index 0b185af..e130d44 100644 --- a/geometry_msgs/PointMsg.cs +++ b/geometry_msgs/PointMsg.cs @@ -43,6 +43,10 @@ public float GetY() { public float GetZ() { return _z; } + + public Vector3 GetVector3() { + return new Vector3(_x, _y, _z); + } public override string ToString() { return "geometry_msgs/Point [x=" + _x + ", y=" + _y + ", z=" + _z + "]"; diff --git a/geometry_msgs/PoseWithCovarianceMsg.cs b/geometry_msgs/PoseWithCovarianceMsg.cs index a8e4759..4bdeb59 100644 --- a/geometry_msgs/PoseWithCovarianceMsg.cs +++ b/geometry_msgs/PoseWithCovarianceMsg.cs @@ -19,10 +19,10 @@ public class PoseWithCovarianceMsg : ROSBridgeMsg { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; public PoseWithCovarianceMsg(JSONNode msg) { - _pose = new PoseMsg(msg["pose"]); + _pose = new PoseMsg(msg["pose"]["pose"]); // Treat covariance - for (int i = 0; i < msg["covariance"].Count; i++ ) { - _covariance[i] = double.Parse(msg["covariance"][i]); + for (int i = 0; i < msg["pose"]["covariance"].Count; i++ ) { + _covariance[i] = double.Parse(msg["pose"]["covariance"][i]); } } diff --git a/geometry_msgs/QuaternionMsg.cs b/geometry_msgs/QuaternionMsg.cs index 311d216..4c327c3 100644 --- a/geometry_msgs/QuaternionMsg.cs +++ b/geometry_msgs/QuaternionMsg.cs @@ -1,7 +1,7 @@ using System.Collections; using System.Text; using SimpleJSON; - +using UnityEngine; /** * Define a geometry_msgs quaternion message. This has been hand-crafted from the corresponding * geometry_msgs message file. @@ -47,6 +47,10 @@ public float GetZ() { public float GetW() { return _w; } + + public Quaternion GetQuaternion() { + return new Quaternion(_x, _y, _z, _w); + } public override string ToString() { return "geometry_msgs/Quaternion [x=" + _x + ", y=" + _y + ", z=" + _z + ", w=" + _w + "]"; diff --git a/nav_msgs/PathMsg.cs b/nav_msgs/PathMsg.cs index d04a586..3872294 100644 --- a/nav_msgs/PathMsg.cs +++ b/nav_msgs/PathMsg.cs @@ -16,7 +16,7 @@ namespace ROSBridgeLib { namespace nav_msgs { public class PathMsg : ROSBridgeMsg { public HeaderMsg _header; - public List _poses; + public List _poses = new List(); public PathMsg(JSONNode msg) { _header = new HeaderMsg(msg["header"]); From 7469b6f7086c531bc20bffa3bfc62fcf100093f6 Mon Sep 17 00:00:00 2001 From: ben60523 Date: Wed, 19 Oct 2022 18:19:49 +0800 Subject: [PATCH 2/5] Add new ROS messages --- actionlib_msgs/GoalIDMsg.cs | 25 +++++++ actionlib_msgs/GoalStatusMsg.cs | 32 +++++++++ actionlib_msgs/GoalStatusType.cs | 24 +++++++ move_base_msgs/MoveBaseActionGoalMsg.cs | 37 ++++++++++ move_base_msgs/MoveBaseGoalMsg.cs | 28 ++++++++ move_base_msgs/MoveBaseResultMsg.cs | 28 ++++++++ move_base_msgs/MoveBaxeActionResultMsg.cs | 41 +++++++++++ nav_msgs/MapMetaDataMsg.cs | 73 ++++++++++++++++++++ nav_msgs/OccupancyGridMsg.cs | 84 +++++++++++++++++++++++ 9 files changed, 372 insertions(+) create mode 100644 actionlib_msgs/GoalIDMsg.cs create mode 100644 actionlib_msgs/GoalStatusMsg.cs create mode 100644 actionlib_msgs/GoalStatusType.cs create mode 100644 move_base_msgs/MoveBaseActionGoalMsg.cs create mode 100644 move_base_msgs/MoveBaseGoalMsg.cs create mode 100644 move_base_msgs/MoveBaseResultMsg.cs create mode 100644 move_base_msgs/MoveBaxeActionResultMsg.cs create mode 100644 nav_msgs/MapMetaDataMsg.cs create mode 100644 nav_msgs/OccupancyGridMsg.cs diff --git a/actionlib_msgs/GoalIDMsg.cs b/actionlib_msgs/GoalIDMsg.cs new file mode 100644 index 0000000..afc225c --- /dev/null +++ b/actionlib_msgs/GoalIDMsg.cs @@ -0,0 +1,25 @@ +using ROSBridgeLib.std_msgs; +using SimpleJSON; + +namespace ROSBridgeLib +{ + namespace actionlib_msgs + { + public class GoalIDMsg : ROSBridgeMsg + { + private TimeMsg _stamp; + private string _id; + + public GoalIDMsg(JSONNode msg) + { + _stamp = new TimeMsg(msg["stamp"]); + _id = msg["id"].ToString(); + } + + public static string GetMessageType() + { + return "actionlib_msgs/GoalID"; + } + } + } +} \ No newline at end of file diff --git a/actionlib_msgs/GoalStatusMsg.cs b/actionlib_msgs/GoalStatusMsg.cs new file mode 100644 index 0000000..b25b0c2 --- /dev/null +++ b/actionlib_msgs/GoalStatusMsg.cs @@ -0,0 +1,32 @@ +using ROSBridgeLib.std_msgs; +using ROSBridgeLib.actionlib_msgs; +using SimpleJSON; + +namespace ROSBridgeLib +{ + namespace actionlib_msgs + { + public class GoalStatusMsg : ROSBridgeMsg + { + private GoalIDMsg _goal_id; + private int _status; + private string _text; + public GoalStatusMsg(JSONNode msg) + { + _goal_id = new GoalIDMsg(msg["goal_id"]); + _status = int.Parse(msg["status"]); + _text = msg["text"]; + } + + public static string GetMessageType() + { + return "actionlib_msgs/GoalStatus"; + } + + public StatusType GetStatus() + { + return (StatusType)_status; + } + } + } +} \ No newline at end of file diff --git a/actionlib_msgs/GoalStatusType.cs b/actionlib_msgs/GoalStatusType.cs new file mode 100644 index 0000000..25bb52f --- /dev/null +++ b/actionlib_msgs/GoalStatusType.cs @@ -0,0 +1,24 @@ +namespace ROSBridgeLib +{ + namespace actionlib_msgs + { + public enum StatusType + { + PENDING, // The goal has yet to be processed by the action server. + ACTIVE, // The goal is currently being processed by the action server + PREEMPTED, // The goal received a cancel request after it started executing + // and has since completed its execution (Terminal State) + SUCCEEDED, // The goal was achieved successfully by the action server (Terminal State) + REJECTED, // The goal was aborted during execution by the action server due to some failure (Terminal State) + PREEMPTING, // The goal received a cancel request after it started executing + // and has not yet completed execution + RECALLING, // The goal received a cancel request before it started executing, + // but the action server has not yet confirmed that the goal is canceled + RECALLED, // The goal received a cancel request before it started executing + // and was successfully cancelled (Terminal State) + LOST // An action client can determine that a goal is LOST. This should not be + // sent over the wire by an action server + } + + } +} \ No newline at end of file diff --git a/move_base_msgs/MoveBaseActionGoalMsg.cs b/move_base_msgs/MoveBaseActionGoalMsg.cs new file mode 100644 index 0000000..d9e3022 --- /dev/null +++ b/move_base_msgs/MoveBaseActionGoalMsg.cs @@ -0,0 +1,37 @@ +using ROSBridgeLib.std_msgs; +using ROSBridgeLib.actionlib_msgs; +using ROSBridgeLib.geometry_msgs; +using SimpleJSON; + +namespace ROSBridgeLib +{ + namespace move_base_msgs + { + public class MoveBaseActionGoalMsg : ROSBridgeMsg + { + private HeaderMsg _header; + private GoalIDMsg _goal_id; + private MoveBaseGoalMsg _goal; + public MoveBaseActionGoalMsg(JSONNode msg) + { + _header = new HeaderMsg(msg["header"]); + _goal_id = new GoalIDMsg(msg["goal_id"]); + _goal = new MoveBaseGoalMsg(msg["goal"]); + } + + public MoveBaseGoalMsg GetGoal() + { + return _goal; + } + + public static string GetMessageType() + { + return "move_base_msgs/MoveBaseActionGoal"; + } + public PoseStampedMsg GetGoalPose() + { + return _goal.GetTargetPose(); + } + } + } +} \ No newline at end of file diff --git a/move_base_msgs/MoveBaseGoalMsg.cs b/move_base_msgs/MoveBaseGoalMsg.cs new file mode 100644 index 0000000..c9d1b63 --- /dev/null +++ b/move_base_msgs/MoveBaseGoalMsg.cs @@ -0,0 +1,28 @@ +using ROSBridgeLib.std_msgs; +using ROSBridgeLib.geometry_msgs; +using SimpleJSON; + +namespace ROSBridgeLib +{ + namespace move_base_msgs + { + public class MoveBaseGoalMsg : ROSBridgeMsg + { + private PoseStampedMsg target_pose; + public MoveBaseGoalMsg(JSONNode msg) + { + target_pose = new PoseStampedMsg(msg["target_pose"]); + } + + public static string GetMessageType() + { + return "move_base_msgs/MoveBaseGoal"; + } + + public PoseStampedMsg GetTargetPose() + { + return target_pose; + } + } + } +} \ No newline at end of file diff --git a/move_base_msgs/MoveBaseResultMsg.cs b/move_base_msgs/MoveBaseResultMsg.cs new file mode 100644 index 0000000..047c8b3 --- /dev/null +++ b/move_base_msgs/MoveBaseResultMsg.cs @@ -0,0 +1,28 @@ +using ROSBridgeLib.std_msgs; +using ROSBridgeLib.geometry_msgs; +using SimpleJSON; + +namespace ROSBridgeLib +{ + namespace move_base_msgs + { + public class MoveBaseResultMsg : ROSBridgeMsg + { + private string res; + public MoveBaseResultMsg(JSONNode msg) + { + res = msg.ToString(); + } + + public static string GetMessageType() + { + return "move_base_msgs/MoveBaseResult"; + } + + public string GetResult() + { + return res; + } + } + } +} \ No newline at end of file diff --git a/move_base_msgs/MoveBaxeActionResultMsg.cs b/move_base_msgs/MoveBaxeActionResultMsg.cs new file mode 100644 index 0000000..23cd293 --- /dev/null +++ b/move_base_msgs/MoveBaxeActionResultMsg.cs @@ -0,0 +1,41 @@ +using ROSBridgeLib.actionlib_msgs; +using ROSBridgeLib.std_msgs; +using SimpleJSON; + +namespace ROSBridgeLib +{ + namespace move_base_msgs + { + class MoveBaseActionResultMsg : ROSBridgeMsg + { + private HeaderMsg _header; + private GoalStatusMsg _status; + private MoveBaseResultMsg _result; + + public static string GetMessageType() + { + return "move_base_msgs/MoveBaseActionResult"; + } + + public MoveBaseActionResultMsg(JSONNode msg) + { + _header = new HeaderMsg(msg["header"]); + _status = new GoalStatusMsg(msg["status"]); + _result = new MoveBaseResultMsg(msg["result"]); + } + public int GetResultStatus() + { + return (int)_status.GetStatus(); + } + public string GetResultStatusDescription() + { + return _status.GetStatus().ToString(); + } + + public StatusType GetResultStatusEnum() + { + return _status.GetStatus(); + } + } + } +} \ No newline at end of file diff --git a/nav_msgs/MapMetaDataMsg.cs b/nav_msgs/MapMetaDataMsg.cs new file mode 100644 index 0000000..d626715 --- /dev/null +++ b/nav_msgs/MapMetaDataMsg.cs @@ -0,0 +1,73 @@ +using System.Collections; +using System.IO; +using System.Text; +using SimpleJSON; +using ROSBridgeLib.std_msgs; +using ROSBridgeLib.geometry_msgs; +using UnityEngine; + +namespace ROSBridgeLib { + namespace nav_msgs { + public class MapMetaDataMsg: ROSBridgeMsg { + private TimeMsg map_load_time; + private float resolution; + private int width; + private int height; + private PoseMsg origin; + + public MapMetaDataMsg(JSONNode msg) { + map_load_time = new TimeMsg(msg["map_load_time"]); + resolution = float.Parse(msg["resolution"]); + width = int.Parse(msg["width"]); + height = int.Parse(msg["height"]); + origin = new PoseMsg(msg["origin"]); + } + + public MapMetaDataMsg(TimeMsg time, float _resolution, int _width, int _height, PoseMsg pose) { + map_load_time = time; + resolution = _resolution; + width = _width; + height = _height; + origin = pose; + } + + public int GetWidth() + { + return width; + } + public int GetHeight() + { + return height; + } + public float GetResolution() + { + return resolution; + } + public PoseMsg GetPose() + { + return origin; + } + + + public static string GetMessageType() { + return "nav_msgs/MapMetaData"; + } + + public override string ToString() { + return "Map Meta Data [map_load_time=" + map_load_time + ", resolution=" + resolution + ", size (" + width+ ", " + height + ")]"; + } + + public override string ToYAMLString() { + return "{\"map_load_time\" : " + "\"" + + map_load_time.ToYAMLString() + + "\", \"resolution\" : \"" + + resolution + + "\", \"width\" : " + width + + "\", \"height\" : " + height + + "\", \"origin\" : " + origin.ToYAMLString() + + "}"; + } + + } + } +} \ No newline at end of file diff --git a/nav_msgs/OccupancyGridMsg.cs b/nav_msgs/OccupancyGridMsg.cs new file mode 100644 index 0000000..869c1fe --- /dev/null +++ b/nav_msgs/OccupancyGridMsg.cs @@ -0,0 +1,84 @@ +using System.Collections; +using System.Collections.Generic; +using System.IO; +using System.Text; +using SimpleJSON; +using ROSBridgeLib.std_msgs; +using ROSBridgeLib.geometry_msgs; +using UnityEngine; + +namespace ROSBridgeLib +{ + namespace nav_msgs + { + public class OccupancyGridMsg : ROSBridgeMsg + { + private HeaderMsg header; + private MapMetaDataMsg info; + // private sbyte[] data; + // private string data; + private List data = new List(); + public OccupancyGridMsg(JSONNode msg) + { + header = new HeaderMsg(msg["header"]); + info = new MapMetaDataMsg(msg["info"]); + // data = System.Convert.FromBase64String(msg["data"]); + for (int i = 0; i < msg["data"].Count; i++) + { + data.Add(msg["data"][i].AsInt); + } + } + + public OccupancyGridMsg(HeaderMsg _header, MapMetaDataMsg _info, List _data) + { + header = _header; + info = _info; + data = _data; + } + + public List GetData() + { + return data; + } + public int GetWidth() + { + return info.GetWidth(); + } + public int GetHeight() + { + return info.GetHeight(); + } + public float GetResolution() + { + return info.GetResolution(); + } + + public PoseMsg GetOriginPose() + { + return info.GetPose(); + } + + public static string GetMessageType() + { + return "nav_msgs/OccupancyGrid"; + } + + public override string ToString() + { + return "Map [Header=" + header.ToString() + ", metadata=" + info.ToString() + ", " + "data=" + data.ToString() + "]"; + } + + public override string ToYAMLString() + { + return "{\"header\" : " + "\"" + + header.ToYAMLString() + + "\", \"info\" : \"" + + info.ToYAMLString() + + "\", \"data\" : " + + data + + "}"; + } + + } + } +} \ No newline at end of file From 283ffa9e44c854d68e00c0310d4992a0ca843783 Mon Sep 17 00:00:00 2001 From: ben60523 Date: Fri, 28 Oct 2022 10:09:24 +0800 Subject: [PATCH 3/5] Add ToString and ToYAMLString in move_base_msgs and actionlib_msgs --- actionlib_msgs/GoalIDMsg.cs | 24 ++++++++++--- actionlib_msgs/GoalStatusMsg.cs | 31 +++++++++++++---- move_base_msgs/MoveBaseActionGoalMsg.cs | 40 ++++++++++++++++----- move_base_msgs/MoveBaseGoalMsg.cs | 17 +++++++++ move_base_msgs/MoveBaseResultMsg.cs | 15 ++++++++ move_base_msgs/MoveBaxeActionResultMsg.cs | 42 +++++++++++++++++------ 6 files changed, 139 insertions(+), 30 deletions(-) diff --git a/actionlib_msgs/GoalIDMsg.cs b/actionlib_msgs/GoalIDMsg.cs index afc225c..dcc07fa 100644 --- a/actionlib_msgs/GoalIDMsg.cs +++ b/actionlib_msgs/GoalIDMsg.cs @@ -7,19 +7,35 @@ namespace actionlib_msgs { public class GoalIDMsg : ROSBridgeMsg { - private TimeMsg _stamp; - private string _id; + private TimeMsg time; + private string id; public GoalIDMsg(JSONNode msg) { - _stamp = new TimeMsg(msg["stamp"]); - _id = msg["id"].ToString(); + time = new TimeMsg(msg["stamp"]); + id = msg["id"].ToString(); + } + + public GoalIDMsg(TimeMsg _time, string _id) + { + time = _time; + id = _id; } public static string GetMessageType() { return "actionlib_msgs/GoalID"; } + + public override string ToString() + { + return "actionlib_msgs/GoalID [ stamp: " + time.ToString() + "id: " + id + "}"; + } + + public override string ToYAMLString() + { + return "{\"stamp\": " + time.ToYAMLString() + ", \"id\": " + id + "}"; + } } } } \ No newline at end of file diff --git a/actionlib_msgs/GoalStatusMsg.cs b/actionlib_msgs/GoalStatusMsg.cs index b25b0c2..9bfac10 100644 --- a/actionlib_msgs/GoalStatusMsg.cs +++ b/actionlib_msgs/GoalStatusMsg.cs @@ -8,14 +8,21 @@ namespace actionlib_msgs { public class GoalStatusMsg : ROSBridgeMsg { - private GoalIDMsg _goal_id; - private int _status; - private string _text; + private GoalIDMsg goal_id; + private int status; + private string text; public GoalStatusMsg(JSONNode msg) { - _goal_id = new GoalIDMsg(msg["goal_id"]); - _status = int.Parse(msg["status"]); - _text = msg["text"]; + goal_id = new GoalIDMsg(msg["goal_id"]); + status = int.Parse(msg["status"]); + text = msg["text"]; + } + + public GoalStatusMsg(GoalIDMsg _goal_id, int _status, string _text) + { + goal_id = _goal_id; + status = _status; + text = _text; } public static string GetMessageType() @@ -23,9 +30,19 @@ public static string GetMessageType() return "actionlib_msgs/GoalStatus"; } + public override string ToString() + { + return "actionlib_msgs/GoalStatus [goal_id=" + goal_id.ToString() + ", status=" + status.ToString() + ", text=" + text; + } + + public override string ToYAMLString() + { + return "{\"goal_id\": " + goal_id.ToYAMLString() + ", \"status\": " + status.ToString() + ", \"text\": " + text + "}"; + } + public StatusType GetStatus() { - return (StatusType)_status; + return (StatusType)status; } } } diff --git a/move_base_msgs/MoveBaseActionGoalMsg.cs b/move_base_msgs/MoveBaseActionGoalMsg.cs index d9e3022..a3e02c8 100644 --- a/move_base_msgs/MoveBaseActionGoalMsg.cs +++ b/move_base_msgs/MoveBaseActionGoalMsg.cs @@ -9,28 +9,50 @@ namespace move_base_msgs { public class MoveBaseActionGoalMsg : ROSBridgeMsg { - private HeaderMsg _header; - private GoalIDMsg _goal_id; - private MoveBaseGoalMsg _goal; + private HeaderMsg header; + private GoalIDMsg goal_id; + private MoveBaseGoalMsg goal; public MoveBaseActionGoalMsg(JSONNode msg) { - _header = new HeaderMsg(msg["header"]); - _goal_id = new GoalIDMsg(msg["goal_id"]); - _goal = new MoveBaseGoalMsg(msg["goal"]); + header = new HeaderMsg(msg["header"]); + goal_id = new GoalIDMsg(msg["goal_id"]); + goal = new MoveBaseGoalMsg(msg["goal"]); } - public MoveBaseGoalMsg GetGoal() + public MoveBaseActionGoalMsg(HeaderMsg _header, GoalIDMsg _goal_id, MoveBaseGoalMsg _goal) { - return _goal; + header = _header; + goal_id = _goal_id; + goal = _goal; } public static string GetMessageType() { return "move_base_msgs/MoveBaseActionGoal"; } + + public override string ToString() + { + return "move_base_msgs/MoveBaseActionGoal [header=" + header.ToString() + + ", goal_id=" + goal_id.ToString() + + ", goal=" + goal.ToString(); + } + + public override string ToYAMLString() + { + return "{\"header\": " + header.ToYAMLString() + + "\"goal_id\": " + goal_id.ToYAMLString() + + "\"goal\": " + goal.ToYAMLString() + "}"; + } + + public MoveBaseGoalMsg GetGoal() + { + return goal; + } + public PoseStampedMsg GetGoalPose() { - return _goal.GetTargetPose(); + return goal.GetTargetPose(); } } } diff --git a/move_base_msgs/MoveBaseGoalMsg.cs b/move_base_msgs/MoveBaseGoalMsg.cs index c9d1b63..d13e19c 100644 --- a/move_base_msgs/MoveBaseGoalMsg.cs +++ b/move_base_msgs/MoveBaseGoalMsg.cs @@ -14,15 +14,32 @@ public MoveBaseGoalMsg(JSONNode msg) target_pose = new PoseStampedMsg(msg["target_pose"]); } + public MoveBaseGoalMsg(PoseStampedMsg _target_pose) + { + target_pose = _target_pose; + } + public static string GetMessageType() { return "move_base_msgs/MoveBaseGoal"; } + public override string ToString() + { + return "move_base_msgs/MoveBaseGoal [target_pose=" + target_pose.ToString(); + } + + + public override string ToYAMLString() + { + return "{\"target_pose\":" + target_pose.ToYAMLString() + "}"; + } + public PoseStampedMsg GetTargetPose() { return target_pose; } + } } } \ No newline at end of file diff --git a/move_base_msgs/MoveBaseResultMsg.cs b/move_base_msgs/MoveBaseResultMsg.cs index 047c8b3..b673a3d 100644 --- a/move_base_msgs/MoveBaseResultMsg.cs +++ b/move_base_msgs/MoveBaseResultMsg.cs @@ -14,11 +14,26 @@ public MoveBaseResultMsg(JSONNode msg) res = msg.ToString(); } + public MoveBaseResultMsg(string _res) + { + res = _res; + } + public static string GetMessageType() { return "move_base_msgs/MoveBaseResult"; } + public override string ToString() + { + return "move_base_msgs/MoveBaseResult [" + res + "]"; + } + + public override string ToYAMLString() + { + return "{" + res + "}"; + } + public string GetResult() { return res; diff --git a/move_base_msgs/MoveBaxeActionResultMsg.cs b/move_base_msgs/MoveBaxeActionResultMsg.cs index 23cd293..03dbdf0 100644 --- a/move_base_msgs/MoveBaxeActionResultMsg.cs +++ b/move_base_msgs/MoveBaxeActionResultMsg.cs @@ -8,33 +8,55 @@ namespace move_base_msgs { class MoveBaseActionResultMsg : ROSBridgeMsg { - private HeaderMsg _header; - private GoalStatusMsg _status; - private MoveBaseResultMsg _result; + private HeaderMsg header; + private GoalStatusMsg status; + private MoveBaseResultMsg result; + + public MoveBaseActionResultMsg(JSONNode msg) + { + header = new HeaderMsg(msg["header"]); + status = new GoalStatusMsg(msg["status"]); + result = new MoveBaseResultMsg(msg["result"]); + } + + public MoveBaseActionResultMsg(HeaderMsg _header, GoalStatusMsg _status, MoveBaseResultMsg _result) + { + header = _header; + status = _status; + result = _result; + } public static string GetMessageType() { return "move_base_msgs/MoveBaseActionResult"; } - public MoveBaseActionResultMsg(JSONNode msg) + public override string ToString() + { + return "move_base_msgs/MoveBaseActionResult [header=" + header.ToString() + + ", status=" + status.ToString() + + ", result=" + result.ToString() + "]"; + } + + public override string ToYAMLString() { - _header = new HeaderMsg(msg["header"]); - _status = new GoalStatusMsg(msg["status"]); - _result = new MoveBaseResultMsg(msg["result"]); + return "{\"header\": " + header.ToYAMLString() + + ", \"status\": " + status.ToYAMLString() + + ", \"result\": " + result.ToYAMLString() + "}"; } + public int GetResultStatus() { - return (int)_status.GetStatus(); + return (int)status.GetStatus(); } public string GetResultStatusDescription() { - return _status.GetStatus().ToString(); + return status.GetStatus().ToString(); } public StatusType GetResultStatusEnum() { - return _status.GetStatus(); + return status.GetStatus(); } } } From 903000f0c84efbe67b066eacd904bd1deab326ba Mon Sep 17 00:00:00 2001 From: ben60523 Date: Fri, 28 Oct 2022 11:14:14 +0800 Subject: [PATCH 4/5] Add geometry_msgs/PoseWithCovarianceStamped msg and restore geometry_msgs/PoseWithCovariance msg --- geometry_msgs/PoseMsg.cs | 4 +- geometry_msgs/PoseWithCovarianceMsg.cs | 8 +-- geometry_msgs/PoseWithCovarianceStampedMsg.cs | 50 +++++++++++++++++++ 3 files changed, 56 insertions(+), 6 deletions(-) create mode 100644 geometry_msgs/PoseWithCovarianceStampedMsg.cs diff --git a/geometry_msgs/PoseMsg.cs b/geometry_msgs/PoseMsg.cs index 74555a0..ba40006 100644 --- a/geometry_msgs/PoseMsg.cs +++ b/geometry_msgs/PoseMsg.cs @@ -12,8 +12,8 @@ namespace ROSBridgeLib { namespace geometry_msgs { public class PoseMsg : ROSBridgeMsg { - public PointMsg _position; - public QuaternionMsg _orientation; + private PointMsg _position; + private QuaternionMsg _orientation; public PoseMsg(JSONNode msg) { _position = new PointMsg(msg["position"]); diff --git a/geometry_msgs/PoseWithCovarianceMsg.cs b/geometry_msgs/PoseWithCovarianceMsg.cs index 4bdeb59..dc86af2 100644 --- a/geometry_msgs/PoseWithCovarianceMsg.cs +++ b/geometry_msgs/PoseWithCovarianceMsg.cs @@ -10,7 +10,7 @@ namespace ROSBridgeLib { namespace geometry_msgs { public class PoseWithCovarianceMsg : ROSBridgeMsg { - public PoseMsg _pose; + private PoseMsg _pose; private double[] _covariance = new double[36] {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, @@ -19,10 +19,10 @@ public class PoseWithCovarianceMsg : ROSBridgeMsg { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; public PoseWithCovarianceMsg(JSONNode msg) { - _pose = new PoseMsg(msg["pose"]["pose"]); + _pose = new PoseMsg(msg["pose"]); // Treat covariance - for (int i = 0; i < msg["pose"]["covariance"].Count; i++ ) { - _covariance[i] = double.Parse(msg["pose"]["covariance"][i]); + for (int i = 0; i < msg["covariance"].Count; i++ ) { + _covariance[i] = double.Parse(msg["covariance"][i]); } } diff --git a/geometry_msgs/PoseWithCovarianceStampedMsg.cs b/geometry_msgs/PoseWithCovarianceStampedMsg.cs new file mode 100644 index 0000000..c08afc5 --- /dev/null +++ b/geometry_msgs/PoseWithCovarianceStampedMsg.cs @@ -0,0 +1,50 @@ +using ROSBridgeLib.geometry_msgs; +using ROSBridgeLib.std_msgs; + +using SimpleJSON; + +namespace ROSBridgeLib +{ + namespace geometry_msgs { + class PoseWithCovarianceStampedMsg : ROSBridgeMsg{ + private HeaderMsg header; + private PoseWithCovarianceMsg pose; + + public PoseWithCovarianceStampedMsg(JSONNode msg) + { + header = new HeaderMsg(msg["header"]); + pose = new PoseWithCovarianceMsg(msg["pose"]); + } + + public PoseWithCovarianceStampedMsg(HeaderMsg _header, PoseWithCovarianceMsg _pose) + { + header = _header; + pose = _pose; + } + + public static string GetMessageType() + { + return "geometry_msgs/PoseWithCovarianceStamped"; + } + + public override string ToString() + { + return "geometry_msgs/PoseWithCovarianceStamped [header=" + header.ToString() + ", pose=" + pose.ToString() + "]"; + } + + public override string ToYAMLString() + { + return "{\"header\": " + header.ToYAMLString() + ", \"pose\": " + pose.ToYAMLString() + "}"; + } + + public PointMsg GetPosition() + { + return pose.GetPose().GetPosition(); + } + public QuaternionMsg GetRotation() + { + return pose.GetPose().GetOrientation(); + } + } + } +} \ No newline at end of file From 51a145cf19a136d9248f8fcf8d6e48e99ddb3ae3 Mon Sep 17 00:00:00 2001 From: ben60523 Date: Mon, 12 Dec 2022 18:03:31 +0800 Subject: [PATCH 5/5] Add LaserScan message --- sensor_msgs/LaserScanMsg.cs | 72 +++++++++++++++++++++++++++++++++++++ 1 file changed, 72 insertions(+) create mode 100644 sensor_msgs/LaserScanMsg.cs diff --git a/sensor_msgs/LaserScanMsg.cs b/sensor_msgs/LaserScanMsg.cs new file mode 100644 index 0000000..882c4e6 --- /dev/null +++ b/sensor_msgs/LaserScanMsg.cs @@ -0,0 +1,72 @@ +using System.Collections.Generic; +using ROSBridgeLib.std_msgs; +using SimpleJSON; + +namespace ROSBridgeLib +{ + namespace sensor_msgs + { + public class LaserScanMsg : ROSBridgeMsg + { + private HeaderMsg header; + private float angle_min, angle_max, angle_increment, time_increment, scan_time, range_min, range_max; + private List ranges = new List(); + private List intensities = new List(); + + public LaserScanMsg(JSONNode msg) + { + header = new HeaderMsg(msg["header"]); + angle_min = float.Parse(msg["angle_min"]); + angle_max = float.Parse(msg["angle_max"]); + angle_increment = float.Parse(msg["angle_increment"]); + time_increment = float.Parse(msg["time_increment"]); + scan_time = float.Parse(msg["scan_time"]); + range_min = float.Parse(msg["range_min"]); + range_max = float.Parse(msg["range_max"]); + + for (int i = 0; i < msg["ranges"].Count; i++) + { + float value = msg["ranges"][i].AsFloat; + if (value == 0) + { + value = range_max; + } + ranges.Add(value); + intensities.Add(msg["intensities"][i].AsFloat); + } + } + + public static string GetMessageType() + { + return "sensor_msgs/LaserScan"; + } + + public List GetRanges() + { + return ranges; + } + + public float GetAngleMin() + { + return angle_min; + } + + public float GetAngleMax() + { + return angle_max; + } + + public float GetAngleIncrement() + { + return angle_increment; + } + + /** + public LaserScanMsg() + { + + } + */ + } + } +} \ No newline at end of file