Skip to content
This repository is currently being migrated. It's locked while the migration is in progress.

Commit

Permalink
various fixes
Browse files Browse the repository at this point in the history
* corrections for simple data types in ROS message types according to ROS specification
* moving message generation classes to Unity3D project
* resourceFileUri fix
* Prismatic Joint translation fix
* updating dependencies
  • Loading branch information
ReitDan authored and MartinBischoff committed Mar 26, 2019
1 parent 0466a52 commit 6687c50
Show file tree
Hide file tree
Showing 23 changed files with 105 additions and 41 deletions.
6 changes: 3 additions & 3 deletions Libraries/RosBridgeClient/Messages/Sensor/Image.cs
Original file line number Diff line number Diff line change
Expand Up @@ -22,11 +22,11 @@ public class Image : Message
[JsonIgnore]
public const string RosMessageName = "sensor_msgs/Image";
public Standard.Header header;
public int height;
public int width;
public uint height;
public uint width;
public string encoding;
public byte is_bigendian;
public int step;
public uint step;
public byte[] data;
public Image()
{
Expand Down
13 changes: 7 additions & 6 deletions Libraries/RosBridgeClient/Messages/Sensor/JointState.cs
Original file line number Diff line number Diff line change
Expand Up @@ -23,16 +23,17 @@ public class JointState : Message
public const string RosMessageName = "sensor_msgs/JointState";
public Standard.Header header;
public string[] name;
public float[] position;
public float[] velocity;
public float[] effort;
public double[] position;
public double[] velocity;
public double[] effort;

public JointState()
{
header = new Standard.Header();
name = new string[0];
position = new float[0];
velocity = new float[0];
effort = new float[0];
position = new double[0];
velocity = new double[0];
effort = new double[0];
}
}
}
8 changes: 4 additions & 4 deletions Libraries/RosBridgeClient/Messages/Sensor/PointCloud2.cs
Original file line number Diff line number Diff line change
Expand Up @@ -22,12 +22,12 @@ public class PointCloud2 : Message
[JsonIgnore]
public const string RosMessageName = "sensor_msgs/PointCloud2";
public Standard.Header header;
public int height;
public int width;
public uint height;
public uint width;
public PointField[] fields;
public bool is_bigendian;
public int point_step;
public int row_step;
public uint point_step;
public uint row_step;

public byte[] data;
public bool is_dense;
Expand Down
14 changes: 11 additions & 3 deletions Libraries/RosBridgeClient/Messages/Sensor/PointField.cs
Original file line number Diff line number Diff line change
Expand Up @@ -21,10 +21,18 @@ public class PointField : Message
{
[JsonIgnore]
public const string RosMessageName = "sensor_msgs/PointField";
public int datatype;
public const byte INT8 = 1;
public const byte UINT8 = 2;
public const byte INT16 = 3;
public const byte UINT16 = 4;
public const byte INT32 = 5;
public const byte UINT32 = 6;
public const byte FLOAT32 = 7;
public const byte FLOAT64 = 8;
public byte datatype;
public string name;
public int offset;
public int count;
public uint offset;
public uint count;
public PointField()
{
datatype = 0;
Expand Down
4 changes: 2 additions & 2 deletions Libraries/RosBridgeClient/Messages/Standard/Float64.cs
Original file line number Diff line number Diff line change
Expand Up @@ -21,11 +21,11 @@ public class Float64 : Message
{
[JsonIgnore]
public const string RosMessageName = "std_msgs/Float64";
public float data;
public double data;

public Float64()
{
data = 0.0f;
data = 0.0;
}
}
}
2 changes: 1 addition & 1 deletion Libraries/RosBridgeClient/Messages/Standard/Header.cs
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,7 @@ public class Header : Message
{
[JsonIgnore]
public const string RosMessageName = "std_msgs/Header";
public int seq;
public uint seq;
public Time stamp;
public string frame_id;
public Header()
Expand Down
4 changes: 2 additions & 2 deletions Libraries/RosBridgeClient/Messages/Standard/Time.cs
Original file line number Diff line number Diff line change
Expand Up @@ -21,8 +21,8 @@ public class Time : Message
{
[JsonIgnore]
public const string RosMessageName = "std_msgs/Time";
public int secs;
public int nsecs;
public uint secs;
public uint nsecs;
public Time()
{
secs = 0;
Expand Down
12 changes: 6 additions & 6 deletions Libraries/RosBridgeClient/PointCloud.cs
Original file line number Diff line number Diff line change
Expand Up @@ -24,10 +24,10 @@ public class PointCloud

public PointCloud(PointCloud2 pointCloud2)
{
int I = pointCloud2.data.Length / pointCloud2.point_step;
long I = pointCloud2.data.Length / pointCloud2.point_step;
Points = new RgbPoint3[I];
byte[] byteSlice = new byte[pointCloud2.point_step];
for (int i = 0; i < I; i++)
for (long i = 0; i < I; i++)
{
Array.Copy(pointCloud2.data, i * pointCloud2.point_step, byteSlice, 0, pointCloud2.point_step);
Points[i] = new RgbPoint3(byteSlice, pointCloud2.fields);
Expand All @@ -37,15 +37,15 @@ public PointCloud(PointCloud2 pointCloud2)
public PointCloud(Image depthImage, Image rgbImage, float focal)
{

int width = depthImage.width;
int height = depthImage.height;
uint width = depthImage.width;
uint height = depthImage.height;
float invFocal = 1.0f / focal;

Points = new RgbPoint3[width * height];

for (int v = 0; v < height; v++)
for (uint v = 0; v < height; v++)
{
for (int u = 0; u < width; u++)
for (uint u = 0; u < width; u++)
{
float depth = 0;// depthImage[u, v];
if (depth == 0)
Expand Down
4 changes: 0 additions & 4 deletions Libraries/RosBridgeClient/RosBridgeClient.csproj
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,6 @@
<Compile Include="Messages\Actionlib\FibonacciFeedback.cs" />
<Compile Include="Messages\Actionlib\FibonacciGoal.cs" />
<Compile Include="Messages\Actionlib\FibonacciResult.cs" />
<Compile Include="Messages\ActionMessageGenerator.cs" />
<Compile Include="Messages\Geometry\Accel.cs" />
<Compile Include="Messages\Geometry\Point.cs" />
<Compile Include="Messages\Geometry\PointStamped.cs" />
Expand All @@ -68,7 +67,6 @@
<Compile Include="Messages\Actionlib\GoalID.cs" />
<Compile Include="Messages\Actionlib\GoalStatus.cs" />
<Compile Include="Messages\Actionlib\GoalStatusArray.cs" />
<Compile Include="Messages\MessageGenerationGenerics.cs" />
<Compile Include="Messages\Navigation\MapMetaData.cs" />
<Compile Include="Messages\Navigation\OccupancyGrid.cs" />
<Compile Include="Messages\Navigation\Odometry.cs" />
Expand All @@ -79,8 +77,6 @@
<Compile Include="Messages\Sensor\Joy.cs" />
<Compile Include="Messages\Sensor\PointCloud2.cs" />
<Compile Include="Messages\Sensor\PointField.cs" />
<Compile Include="Messages\ServiceMessageGenerator.cs" />
<Compile Include="Messages\SimpleMessageGenerator.cs" />
<Compile Include="Messages\Standard\Header.cs" />
<Compile Include="Messages\Standard\Float64.cs" />
<Compile Include="Messages\Standard\Bool.cs" />
Expand Down
2 changes: 1 addition & 1 deletion Libraries/RosBridgeClientTest/RosSocketConsoleExample.cs
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@ namespace RosSharp.RosBridgeClientTest
{
public class RosSocketConsole
{
static readonly string uri = "ws://192.168.56.102:9090";
static readonly string uri = "ws://192.168.56.103:9090";

public static void Main(string[] args)
{
Expand Down
Binary file modified Unity3D/Assets/RosSharp/Plugins/RosBridgeClient.dll
Binary file not shown.
Original file line number Diff line number Diff line change
Expand Up @@ -20,8 +20,8 @@ public static class HeaderExtensions
public static void Update(this Messages.Standard.Header header)
{
float time = UnityEngine.Time.realtimeSinceStartup;
int secs = (int)time;
int nsecs = (int)(1e9 *(time-secs));
uint secs = (uint)time;
uint nsecs = (uint)(1e9 *(time-secs));
header.seq++;
header.stamp.secs = secs;
header.stamp.nsecs = nsecs;
Expand Down

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

Original file line number Diff line number Diff line change
Expand Up @@ -42,9 +42,9 @@ private void InitializeMessage()
{
header = new Messages.Standard.Header { frame_id = FrameId },
name = new string[jointStateLength],
position = new float[jointStateLength],
velocity = new float[jointStateLength],
effort = new float[jointStateLength]
position = new double[jointStateLength],
velocity = new double[jointStateLength],
effort = new double[jointStateLength]
};
}

Expand All @@ -59,11 +59,18 @@ private void UpdateMessage()

private void UpdateJointState(int i)
{

JointStateReaders[i].Read(
out message.name[i],
out message.position[i],
out message.velocity[i],
out message.effort[i]);
out float position,
out float velocity,
out float effort);

message.position[i] = position;
message.velocity[i] = velocity;
message.effort[i] = effort;
}


}
}
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,7 @@ protected override void ReceiveMessage(Messages.Sensor.JointState message)
{
index = JointNames.IndexOf(message.name[i]);
if (index != -1)
JointStateWriters[index].Write(message.position[i]);
JointStateWriters[index].Write((float) message.position[i]);
}
}
}
Expand Down

0 comments on commit 6687c50

Please sign in to comment.