public new static void CallBack(ROSBridgeMsg msg)
{
ImageMsg image = (ImageMsg)msg;
byte[] color_data = image.GetImage();
int height = (int)image.GetHeight();
int width = (int)image.GetWidth();
int step = (int)image.GetRowStep();
Texture2D tex = new Texture2D(width, height, TextureFormat.RGB24, false);
Color[] colors = new Color[width * height];
for (int i = 0; i < width; i++)
{
for (int j = 0; j < height; j++)
{
byte B = color_data[j * step + i * 3];
byte G = color_data[j * step + i * 3 + 1];
byte R = color_data[j * step + i * 3 + 2];
Color c = new Color((float)R / 255.0f, (float)G / 255.0f, (float)B / 255.0f);
colors[j * width + i] = c;
//tex.SetPixel(i, height - j, c);
}
}
tex.SetPixels(colors);
tex.Apply();
GameObject cam_image = GameObject.Find("CameraImage");
cam_image.GetComponent <RawImage>().texture = tex;
}
public static void CallBack(ROSBridgeMsg msg)
{
Debug.Log("callback");
Vector3 tablePos = GameObject.FindWithTag("Table").transform.position;
ObstacleMsg pose = (ObstacleMsg)msg;
Debug.Log("obstacle callback");
if (!ids.Contains(pose.id) && pose.id != 0)
{
ids.Add(pose.id);
GameObject world = GameObject.FindWithTag("World");
GameObject torus = (GameObject)world.GetComponent <WorldScript>().torus;
GameObject newTorus = Object.Instantiate(torus);
//newTorus.name = pose.id + "";
newTorus.transform.parent = world.transform;
newTorus.transform.localPosition = new Vector3(-pose._x, pose._z + tablePos.z + 0.148f, -pose._y);
newTorus.transform.localScale = new Vector3(pose.scale_x, pose.scale_x, pose.scale_x) * 5;
WorldScript.obstacles.Add(newTorus);
}
}
public new static void CallBack(ROSBridgeMsg msg)
{
CompressedImageMsg imgMsg = msg as CompressedImageMsg;
//HUD.droneCamera.LoadImage(imgMsg.GetImage());
ROSManager.getInstance().getdroneCam().LoadImage(imgMsg.GetImage());
}
public static void CallBack(ROSBridgeMsg msg)
{
EnvironmentMsg poseMsg = (EnvironmentMsg)msg;
if (poseMsg.id_.Contains("hoop"))
{
char numID = poseMsg.id_[poseMsg.id_.Length - 1];
//Debug.Log("Got a tf message: " + poseMsg.id_[poseMsg.id_.Length - 1]);
GameObject currentHoop = null;
if (!WorldProperties.hoopsDict.ContainsKey(numID))
{
GameObject world = GameObject.FindWithTag("World");
currentHoop = Object.Instantiate(world.GetComponent <WorldProperties>().torus);
currentHoop.transform.parent = world.transform;
WorldProperties.hoopsDict[numID] = currentHoop;
Debug.Log("Made hoop with id: " + numID);
}
else
{
currentHoop = WorldProperties.hoopsDict[numID];
}
currentHoop.transform.localPosition = WorldProperties.RosSpaceToWorldSpace(poseMsg.x_, poseMsg.y_, poseMsg.z_) +
WorldProperties.torusModelOffset;
currentHoop.transform.localRotation = new Quaternion(poseMsg.x_rot_ + 1, poseMsg.y_rot_, poseMsg.z_rot_, poseMsg.w_rot_);
}
}
public new static void CallBack(ROSBridgeMsg msg)
{
;
modelController modelControllerScript;
GameObject mc = GameObject.Find("Motorcycle"); // get mc gameobject
modelControllerScript = mc.GetComponent <modelController>(); // mc script
modelControllerScript.timeSinceCB = 0f; // reset counter for timeout limit
Simulink2Unity arraymsg = (Simulink2Unity)msg;
//Debug.Log (arraymsg.ToYAMLString ());
//Debug.Log(arraymsg.ToString ());
//parse message and send to model script
modelControllerScript.roll = arraymsg._roll;
modelControllerScript.pitch = arraymsg._pitch;
modelControllerScript.yaw = arraymsg._yaw;
modelControllerScript.steeringAngle = arraymsg._steering_angle;
modelControllerScript.rpm = arraymsg._rpm;
modelControllerScript.speed = arraymsg._speed;
modelControllerScript.brake_front = arraymsg._brake_front;
modelControllerScript.throttle = arraymsg._throttle;
modelControllerScript.clutch_switch = arraymsg._clutch_switch;
modelControllerScript.gear = arraymsg._gear;
modelControllerScript.emergencyStop = arraymsg._emergencyStop;
modelControllerScript.rigPositionX = arraymsg._rigPositionX;
//Debug.Log (arraymsg);
}
/**
* Add a subscriber callback to this connection. There can be many subscribers.
*/
public void AddSubscriber(Type subscriber, int throttle_rate = 0)
{
IsValidSubscriber(subscriber);
if (throttle_rate == 0)
{
_subscribers.Add(subscriber);
if (_connected)
{
_ws.Send(ROSBridgeMsg.Subscribe(GetMessageTopic(subscriber), GetMessageType(subscriber)));
}
}
else
{
if (_connected)
{
_subscribers.Add(subscriber);
_ws.Send(ROSBridgeMsg.Subscribe(GetMessageTopic(subscriber), GetMessageType(subscriber), throttle_rate));
}
else
{
Debug.LogWarning("Throttle_rate requires to be connected.");
}
}
}
private void Run()
{
_ws = new WebSocket(_host + ":" + _port);
_ws.OnMessage += (sender, e) => this.OnMessage(e.Data);
//_ws.CloseAsync();
_ws.Connect();
foreach (Type p in _subscribers)
{
_ws.Send(ROSBridgeMsg.Subscribe(GetMessageTopic(p), GetMessageType(p)));
if (_debug)
{
Debug.Log("Sending " + ROSBridgeMsg.Subscribe(GetMessageTopic(p), GetMessageType(p)));
}
}
foreach (Type p in _publishers)
{
_ws.Send(ROSBridgeMsg.Advertise(GetMessageTopic(p), GetMessageType(p)));
if (_debug)
{
Debug.Log("Sending " + ROSBridgeMsg.Advertise(GetMessageTopic(p), GetMessageType(p)));
}
}
while (true)
{
Thread.Sleep(10000);
}
}
// This function should fire on each ros message
public new static void CallBack(ROSBridgeMsg msg)
{
// Update ball position, or whatever
//ball.x = msg.x; // Check msg definition in rosbridgelib
//ball.y = msg.y;
//ball.z = msg.z;
}
public new static void CallBack(ROSBridgeMsg msg)
{
GameObject model = GameObject.Find("Model");
GameObject robot = GameObject.Find("Robot");
Robot r = (Robot)model.GetComponent(typeof(Robot));
if (model == null)
{
Debug.Log("<color=red>ERROR:</color> Can't find GameObject Robot. Create it in Unity");
}
else
{
NavStsMsg nav_sts = (NavStsMsg)msg;
NEDMsg p = nav_sts.GetPosition();
RPYMsg o = nav_sts.GetOrientation();
robot.transform.position = new Vector3(-p.GetNorth(), 0, p.GetEast());
Vector3 next_position = new Vector3(-p.GetNorth(), -p.GetDepth(), p.GetEast());
Quaternion next_orientation = Quaternion.Euler(o.GetRollDegrees() + 90f, o.GetYawDegrees(), o.GetPitchDegrees());
r.AddWaypoint(next_position, next_orientation);
//model.transform.position = new Vector3(-p.GetNorth(), -p.GetDepth(), p.GetEast());
//model.transform.rotation = Quaternion.Euler(o.GetRollDegrees() + 90f, o.GetYawDegrees(), o.GetPitchDegrees());
GameObject ned_canvas = GameObject.Find("NED");
Text NED_text = ned_canvas.GetComponent <Text>();
int dp = 2;
string format = string.Format("#.{0} m;-#.{0} m", new string('#', dp));
NED_text.text = p.GetNorth().ToString(format) +
"\n" + p.GetEast().ToString(format) +
"\n" + p.GetDepth().ToString(format) +
"\n" + nav_sts.GetAltitude().GetData().ToString(format);
}
}
public void Publish(String topic, ROSBridgeMsg msg)
{
if (!viewfinder)
{
_ros.Publish(topic, msg);
}
}
// This function should fire on each ros message
public new static void CallBack(ROSBridgeMsg msg)
{
Debug.Log(msg.ToYAMLString());
ThrusterSpeedsMsg thrustVals = (ThrusterSpeedsMsg)msg;
ForceVals = thrustVals.GetData();
}
public new static void CallBack(ROSBridgeMsg msg)
{
JSONNode json_msg = JSON.Parse(((StringMsg)msg).GetData());
//send data from ROS to objects manager
ObjectsManager.Instance.setDataFromROS(json_msg);
}
public static void CallBack(ROSBridgeMsg msg)
{
Debug.Log("callback");
Vector3 tablePos = GameObject.FindWithTag("Table").transform.position;
ObstacleMsg pose = (ObstacleMsg)msg;
if (!WorldProperties.obstacleids.Contains(pose.id) && pose.id != 0)
{
WorldProperties.obstacleids.Add(pose.id);
GameObject world = GameObject.FindWithTag("World");
GameObject torus = (GameObject)WorldProperties.worldObject.GetComponent <WorldProperties>().torus;
GameObject newTorus = Object.Instantiate(torus);
//newTorus.name = pose.id + "";
newTorus.transform.parent = world.transform;
newTorus.transform.localPosition = new Vector3(-pose._x, pose._z + tablePos.z + 0.148f, -pose._y);
newTorus.transform.localScale = new Vector3(pose.scale_x, pose.scale_x, pose.scale_x) * 5;
WorldProperties.obstacles.Add(newTorus);
/*
* var radius = newTorus.GetComponent<MeshFilter>().mesh.bounds.extents.x;
* Vector3 start = new Vector3(newTorus.transform.localPosition.x, newTorus.transform.localPosition.y - radius, newTorus.transform.localPosition.z);
* Vector3 end = new Vector3(newTorus.transform.localPosition.x, tablePos.z, newTorus.transform.localPosition.z);
* Vector3 offset = start - end;
* var scale = new Vector3(1, offset.magnitude / 2, 1);
* var position = start + (offset / 2);
*
* GameObject pole = GameObject.CreatePrimitive(PrimitiveType.Cylinder);
* pole.transform.localPosition = position;
* pole.transform.up = offset;
* pole.transform.localScale = scale;*/
//Debug.Log("making torus id: " + newTorus.name);
}
}
// This function should fire on each received ros message
public static void CallBack(ROSBridgeMsg msg1)
{
Float64Msg msg = (Float64Msg)msg1;
Debug.Log("Recieved Message : " + msg.GetData());
// Update ball position, or whatever
player = GameObject.Find("Player");
rb = player.GetComponent <Rigidbody>();
if (msg.GetData() == 8)
{
rb.AddForce(new Vector3(0, 0, 500));
}
if (msg.GetData() == 6)
{
rb.AddForce(new Vector3(500, 0, 0));
}
if (msg.GetData() == 4)
{
rb.AddForce(new Vector3(-500, 0, 0));
}
if (msg.GetData() == 2)
{
rb.AddForce(new Vector3(0, 0, -500));
}
if (msg.GetData() == 5)
{
rb.AddForce(new Vector3(0, 500, 0));
}
}
//TODO: Not yet implemented
public void ReceivedLocomotionStateUpdata(ROSBridgeMsg data)
{
//TODO: Not implemented yet
StringMsg s = (StringMsg)data;
//_currentRobotLocomotionState = (RobotLocomotionState) Enum.Parse(typeof(RobotLocomotionState), s.data);
}
public void ReceivedOdometryUpdate(ROSBridgeMsg data)
{
//In WGS84
OdometryMsg nav = (OdometryMsg)data;
GeoPointWGS84 geoPoint = new GeoPointWGS84
{
latitude = nav._pose._pose._position.GetY(),
longitude = nav._pose._pose._position.GetX(),
altitude = nav._pose._pose._position.GetZ(),
};
Quaternion orientation = new Quaternion(
x: nav._pose._pose._orientation.GetX(),
z: nav._pose._pose._orientation.GetY(),
y: nav._pose._pose._orientation.GetZ(),
w: nav._pose._pose._orientation.GetW()
);
_odometryDataToConsume = new OdometryData
{
Position = geoPoint,
Orientation = orientation
};
_hasOdometryDataToConsume = true;
}
public new static void CallBack(ROSBridgeMsg msg)
{
OccupancyGridMsg occupancy = (OccupancyGridMsg)msg;
MapMetaDataInfoMsg meta = occupancy.GetInfo();
int width = meta.GetWidth(), height = meta.GetHeight();
float resolution = meta.GetResolution();
PointMsg origin = meta.GetOrigin().GetPosition();
QuaternionMsg pose = meta.GetOrigin().GetOrientation();
Debug.Log("Width: " + width + "\nHeight: " + height + "\nResolution: " + resolution);
RawImage texObj = GameObject.Find(objectName).GetComponent <RawImage>();
Texture2D tex = texObj.texture as Texture2D;
if (tex == null || tex.width != width || tex.height != height)
{
tex = new Texture2D(width, height, TextureFormat.RGBA32, false);
texObj.texture = tex;
}
NativeArray <Color32> pixels = tex.GetRawTextureData <Color32>();
byte[] raw = occupancy.GetData();
for (int i = 0; i < raw.Length; i++)
{
pixels[i] = new Color32((byte)(255 - raw[i]), (byte)(255 - raw[i]), (byte)(255 - raw[i]), (byte)Mathf.Min(255, 350 - raw[i]));
}
tex.Apply();
}
public new static void CallBack(ROSBridgeMsg msg)
{
Float32Msg speedMessage = (Float32Msg)msg;
playerSpeed = speedMessage.GetData();
Debug.Log("speedMessage: " + playerSpeed);
}
private void ReceivedNavigationParameters(ROSBridgeMsg parameters)
{
StringMsg data = (StringMsg)parameters;
string[] split = data._data.Split(',');
k_rho = float.Parse(split[0]);
k_alpha = float.Parse(split[1]);
}
private void PoseMsgSubscriber_OnCallBack(ROSBridgeMsg msg)
{
pose = (PoseStampedMsg)msg;
drone.transform.rotation = new Quaternion(pose._pose._orientation.GetX(), pose._pose._orientation.GetZ(), pose._pose._orientation.GetY(), pose._pose._orientation.GetW());
drone.transform.position = new Vector3(pose._pose._position.GetX(), pose._pose._position.GetZ(), pose._pose._position.GetY());
sec = pose._header.GetTimeMsg().GetSecs();
nsec = pose._header.GetTimeMsg().GetNsecs();
}
// This function should fire on each ros message
public new static void CallBack(ROSBridgeMsg msg)
{
//Debug.Log("---recvd something");
// Update ball position, or whatever
action = ((Action1)msg).data; // Check msg definition in rosbridgelib
//5/12/2019
//Debug.Log("received - "+action);
}
public new static void CallBack(ROSBridgeMsg msg)
{
Int8Msg idMsg = (Int8Msg)msg;
id = idMsg.GetData();
thrusters = GameObject.Find("Thrusters");
thrusters.GetComponent <DataPublisher>().SendPos(id);
}
// This function should fire on each ros message
public new static void CallBack(ROSBridgeMsg msg)
{
//Debug.Log (msg.ToYAMLString());
ThrusterSpeedsMsg thrustVals = (ThrusterSpeedsMsg)msg;
ForceVals = thrustVals.GetData();
thrusters = GameObject.Find("Thrusters");
thrusters.GetComponent <ThrusterController>().AddForces(ForceVals);
}
/**
* Add a publisher to this connection. There can be many publishers.
*/
public void AddPublisher(Type publisher)
{
IsValidPublisher(publisher);
_publishers.Add(publisher);
if (_connected)
{
_ws.Send(ROSBridgeMsg.Advertise(GetMessageTopic(publisher), GetMessageType(publisher)));
}
}
public void UsbCamImageRawSubscriber_OnCallBack(ROSBridgeMsg msg)
{
if (frames.Count > size)
{
frames.Dequeue();
}
frames.Enqueue((ImageMsg)msg);
}
public new static void CallBack(ROSBridgeMsg msg)
{
// CamRos.CallBackFromRos(cmdvel.GetLinear().GetX(), cmdvel.GetAngular().GetZ());
msg_flag = true;
in_msg = msg;
// Application.Quit();
Debug.Log("Message Recieved");
// s.UpdateCmdVel(cmdvel.linear.x, cmdvel.angular.z);
}
public new static void CallBack(ROSBridgeMsg msg)
{
GeoPointMsg LocationGPS = msg as GeoPointMsg;
ROSManager.getInstance().setLatitude(LocationGPS.GetLatitude());
ROSManager.getInstance().setLongitude(LocationGPS.GetLongitude());
ROSManager.getInstance().setAltitude(LocationGPS.GetAltitude());
//Debug.Log(LocationGPS.ToYAMLString());
}
public new static void CallBack(ROSBridgeMsg msg)
{
GameObject capsule = GameObject.Find("Capsule");
ThrusterMsg t = (ThrusterMsg)msg;
CapsuleCollider capsuleColl = capsule.GetComponent <CapsuleCollider>();
Rigidbody rb = capsule.GetComponent <Rigidbody>();
float radius = capsuleColl.radius;
float height = capsuleColl.height;
Vector3 pos = capsule.transform.position;
// thruster position calculation
p1 = new Vector3(pos.x + radius, pos.y, pos.z - height / 2);
p2 = new Vector3(pos.x - radius, pos.y, pos.z - height / 2);
p3 = new Vector3(pos.x + radius, pos.y, pos.z + height / 2);
p4 = new Vector3(pos.x - radius, pos.y, pos.z + height / 2);
if (capsule == null)
{
Debug.Log("Cant Find AUV");
}
else
{
float td1 = t.Gettd1() - 290;
float td2 = t.Gettd2() - 290;
float td3 = t.Gettd3() - 290;
float td4 = t.Gettd4() - 290;
float tfl = t.Gettfl() - 290;
float tfr = t.Gettfr() - 290;
float trl = t.Gettrl() - 290;
float trr = t.Gettrr() - 290;
rb = capsule.GetComponent <Rigidbody>();
// depth thruster forces
rb.AddForceAtPosition(new Vector3(0, td1, 0), p1);
rb.AddForceAtPosition(new Vector3(0, td2, 0), p2);
rb.AddForceAtPosition(new Vector3(0, td3, 0), p3);
rb.AddForceAtPosition(new Vector3(0, td4, 0), p4);
// vector thruster forces
rb.AddForceAtPosition(new Vector3(-tfl / 2, 0, -tfl / 2), p1);
rb.AddForceAtPosition(new Vector3(tfr / 2, 0, -tfr / 2), p2);
rb.AddForceAtPosition(new Vector3(trl / 2, 0, -trl / 2), p3);
rb.AddForceAtPosition(new Vector3(-trr / 2, 0, -trr / 2), p4);
// v1 = new Vector3();
// capsule.GetComponent<Rigidbody>().AddForce(0,zforce,0);
}
}
public new static void CallBack(ROSBridgeMsg msg)
{
if (_i % 5 == 0)
{
PointCloud2Msg cloud_msg = (PointCloud2Msg)msg;
GameObject robot = GameObject.Find("Robot"); // "Robot" -->
ArseaViewer viewer = (ArseaViewer)robot.GetComponent(typeof(ArseaViewer));
viewer.PushCloud(cloud_msg);
}
_i = _i + 1;
}
public void CallService(string service, string args)
{
if (_ws != null)
{
string s = ROSBridgeMsg.CallService(service, args);
if (_debug)
{
Debug.Log("Sending " + s);
}
_ws.Send(s);
}
}
