//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);
}
protected override void StartROS()
{
Debug.Log("Started ROS");
Debug.Log(_rosBridge);
_rosLocomotionDirect = new ROSLocomotionDirect(ROSAgent.AgentJob.Subscriber, _rosBridge, "/cmd_vel");
_rosLocomotionDirect.OnDataReceived += ReceivedLocomotionDirectUpdate;
_rosJoystick = new ROSGenericSubscriber <TwistMsg>(_rosBridge, "/teleop_velocity_smoother/raw_cmd_vel",
TwistMsg.GetMessageType(), (msg) => new TwistMsg(msg));
_rosJoystick.OnDataReceived += ReceivedJoystickUpdate;
//odometry subscriber is not needed for the virtual robot
//_rosOdometrySubscriber = new ROSGenericSubscriber<OdometryMsg>(_rosBridge, "/odom", OdometryMsg.GetMessageType(), (msg) => new OdometryMsg(msg));
//_rosOdometrySubscriber.OnDataReceived += ReceivedOdometryUpdate;
//odometry publisher
_rosOdometry = new ROSGenericPublisher(_rosBridge, "/odom", OdometryMsg.GetMessageType());
//robot_gps_pose publisher
_rosOdometryGPSPose = new ROSGenericPublisher(_rosBridge, "/robot_gps_pose", OdometryMsg.GetMessageType());
//odo_calib_pose publisher
_rosOdometryOverride = new ROSGenericPublisher(_rosBridge, "/odo_calib_pose", OdometryMsg.GetMessageType());
_transformUpdateCoroutine = StartCoroutine(SendTransformUpdate(_publishInterval));
_rosLocomotionWaypointState = new ROSLocomotionWaypointState(ROSAgent.AgentJob.Publisher, _rosBridge, "/waypoint/state");
_rosLocomotionWaypoint = new ROSLocomotionWaypoint(ROSAgent.AgentJob.Publisher, _rosBridge, "/waypoint");
_rosLocomotionLinear = new ROSGenericPublisher(_rosBridge, "/waypoint/max_linear_speed", Float32Msg.GetMessageType());
_rosLocomotionAngular = new ROSGenericPublisher(_rosBridge, "/waypoint/max_angular_speed", Float32Msg.GetMessageType());
_rosLocomotionControlParams = new ROSLocomotionControlParams(ROSAgent.AgentJob.Publisher, _rosBridge, "/waypoint/control_parameters");
_rosLogger = new ROSGenericPublisher(_rosBridge, "/debug_output", StringMsg.GetMessageType());
_rosLocomotionLinear.PublishData(new Float32Msg(RobotConfig.MaxLinearSpeed));
_rosLocomotionAngular.PublishData(new Float32Msg(RobotConfig.MaxAngularSpeed));
_rosLocomotionControlParams.PublishData(RobotConfig.LinearSpeedParameter, RobotConfig.RollSpeedParameter, RobotConfig.PitchSpeedParameter, RobotConfig.AngularSpeedParameter);
}
new public void Button_Red()
{
//Debug.Log ("Red button clickedasdfasdfasdf");
var ButtonObj = GameObject.Find("WorldObjects/Canvas/Button0");
var DropdownObj = GameObject.Find("WorldObjects/Canvas/Dropdown");
if (CustomRosBridge.isMoving)
{
CustomRosBridge.isMoving = false;
ButtonObj.GetComponentInChildren <Text>().text = "Stop";
//Stop navigation
TwistMsg msg = new TwistMsg(new Vector3Msg(0, 0, 0), new Vector3Msg(0, 0, 0));
if (CustomRosBridge.ros != null)
{
CustomRosBridge.ros.Publish(RobotVelPublisher.GetMessageTopic(), msg);
}
}
else
{
CustomRosBridge.isMoving = true;
ButtonObj.GetComponentInChildren <Text>().text = "Confirm";
//Start navigation
StringMsg msg = new StringMsg("\"" + DropdownObj.GetComponentInChildren <Label>().text + "\"");
if (CustomRosBridge.ros != null)
{
CustomRosBridge.ros.Publish(RobotDataPublisher.GetMessageTopic(), msg);
}
}
}
void Update()
{
ros.Render();
if (!_running)
{
return;
}
timer -= Time.deltaTime;
if (timer <= 0)
{
timer = pollRate;
PointMsg point = new PointMsg(1, 2, 3);
QuaternionMsg quat = new QuaternionMsg(1, 2, 3, 4);
PoseMsg pose = new PoseMsg(point, quat);
PoseWithCovarianceMsg posec = new PoseWithCovarianceMsg(pose);
Vector3Msg vec3 = new Vector3Msg(1, 2, 3);
TwistMsg twist = new TwistMsg(vec3, vec3);
TwistWithCovarianceMsg twistc = new TwistWithCovarianceMsg(twist, new double[36]);
HeaderMsg header = new HeaderMsg(1, new TimeMsg(1, 1), "0");
PoseStampedMsg ps = new PoseStampedMsg(header, pose);
PathMsg msg = new PathMsg(header, new PoseStampedMsg[] { ps, ps, ps });
BoolMsg boolmsg = new BoolMsg(true);
StringMsg str = new StringMsg("This is a test");
RegionOfInterestMsg roi = new RegionOfInterestMsg(0, 1, 2, 3, true);
CameraInfoMsg caminfo = new CameraInfoMsg(header, 100, 200, "plumb_bob", new double[5], new double[9], new double[9], new double[12], 14, 123, roi);
_genericPub.PublishData(caminfo);
}
}
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]);
}
public void SerializeDeserialize_UnicodeString_ReturnsSameString()
{
const string v = "ಠ_ಠ";
StringMsg inMsg = new StringMsg(v);
StringMsg outMsg = MessageRoundTrip(inMsg, StringMsg.Deserialize);
Assert.AreEqual(inMsg.data, v);
Assert.AreEqual(inMsg.data, outMsg.data);
}
public void Serialization_does_not_throw_with_value()
{
var message = new StringMsg
{
Value = "Test message"
};
Serialize(message).Should()
.NotThrow();
}
public void OnJoinRoomRequest(NetworkMessage msg)
{
Debug.Log("CLIENT REQUESTING JOIN ROOM");
StringMsg parsed = msg.ReadMessage <StringMsg> ();
if (!waitJoin.ContainsKey(parsed.value))
{
waitJoin.Add(parsed.value, msg.conn);
}
}
public void Serialization_does_not_throw_with_empty_value()
{
var message = new StringMsg
{
Value = string.Empty
};
Serialize(message).Should()
.NotThrow();
}
public void PublishData(string data)
{
if (_publisher == null)
{
return;
}
StringMsg state = new StringMsg(data);
_publisher.PublishData(state);
}
public void Serialization_fails_with_null_value()
{
var message = new StringMsg
{
Value = null
};
Serialize(message).Should()
.Throw <RosFieldSerializationException>()
.Where(x => x.Identifier == "value");
}
public static void Emit(string topic, string message)
{
if (Instance._rosA == null)
{
Instance.Connect();
}
Debug.Log(string.Format("RosBridge: Sending {0} on {1}", message, topic));
var str = new StringMsg(message);
Instance._rosA.Publish(topic, str);
Instance._rosA.Render();
}
public void AddAndGet_OneMessage_ReturnsSameMessage()
{
MessagePool <StringMsg> pool = new MessagePool <StringMsg>();
const string firstMessageContents = "first message";
StringMsg str1 = new StringMsg(firstMessageContents);
pool.AddMessage(str1);
StringMsg str2 = pool.GetOrCreateMessage();
StringMsg str3 = pool.GetOrCreateMessage();
Assert.AreEqual(str1, str2);
Assert.AreNotEqual(str2, str3);
}
// This function should fire on each ros message
public new static void CallBack(ROSBridgeMsg msg)
{
StringMsg tmp = (StringMsg)msg;
GameManager.instance.DecodeStatus(tmp.GetData());
//Debug.Log("image received");
//Debug.Log("Here is the message received " + msg);
// Update ball position, or whatever
//ball.x = msg.x; // Check msg definition in rosbridgelib
//ball.y = msg.y;
//ball.z = msg.z;
}
public override void OnGUI()
{
if (!visible)
return;
GUI.skin = gSkin;
GUI.depth = -2;
int w = 300;
int x = Screen.width / 2 - (w / 2);
int h = 125;
int y = Screen.height / 2 - (h / 2);
GUILayout.BeginArea(new Rect(x, y, w, h), GUI.skin.customStyles[0]);
GUILayout.BeginVertical();
GUILayout.Space(4);
GUILayout.Label(textbox, GUI.skin.customStyles[4], GUILayout.Height(70), GUILayout.Width(285));
GUILayout.Space(10);
GUILayout.BeginHorizontal();
if (hasCancel)
{
GUILayout.Space(30);
if (GUILayout.Button("OK", GUI.skin.customStyles[1], GUILayout.Width(50), GUILayout.Height(20)))
{
StringMsg msg = new StringMsg();
msg.message = commandString;
//DoctorBrain.GetInstance().PutMessage(msg);
print(commandString);
}
GUILayout.Space(w - 180);
if (GUILayout.Button("Cancel", GUI.skin.customStyles[1], GUILayout.Width(50), GUILayout.Height(20)))
visible = false;
}
else
{
GUILayout.Space(120);
if (GUILayout.Button("Yes", GUI.skin.customStyles[1], GUILayout.Width(50), GUILayout.Height(20)))
{
// go back to main menu
Application.LoadLevel("finish");
Application.ExternalCall("finishLMS");
}
}
GUILayout.EndHorizontal();
GUILayout.EndVertical();
GUILayout.EndArea();
}
public void AddAndGet_MultipleMessages_ReturnCorrectMessage()
{
MessagePool <StringMsg> pool = new MessagePool <StringMsg>();
const string firstMessageContents = "first message";
StringMsg str1 = new StringMsg(firstMessageContents);
StringMsg str2 = new StringMsg(firstMessageContents);
pool.AddMessage(str1);
pool.AddMessage(str2);
StringMsg str3 = pool.GetOrCreateMessage();
StringMsg str4 = pool.GetOrCreateMessage();
StringMsg str5 = pool.GetOrCreateMessage();
Assert.AreEqual(str1, str3);
Assert.AreEqual(str2, str4);
Assert.AreNotEqual(str1, str5);
Assert.AreNotEqual(str2, str5);
}
//Inorder to send the image from both cameras,images are encoded/compressed first using JPG compression
//then the compressed data is converted into a string in Base64 format.
// Update is called once per frame
void Update()
{
#if img2
//encoding part:
StringBuilder imgToSend = new StringBuilder("", 500000);
//Bottom Cam encoding:
RenderTexture.active = bottomImage;
imageToSend.ReadPixels(new Rect(0, 0, bottomImage.width, bottomImage.height), 0, 0);
imageToSend.Apply();
Byte[] bottom_cam_image_jpg = ImageConversion.EncodeToJPG(imageToSend, 100);
string bottom_cam_image_base64 = Convert.ToBase64String(bottom_cam_image_jpg);
imgToSend.Append(bottom_cam_image_base64).Append("!");
//Front cam encoding
RenderTexture.active = frontImage;
imageToSend2.ReadPixels(new Rect(0, 0, frontImage.width, frontImage.height), 0, 0);
imageToSend2.Apply();
Byte[] front_cam_image_jpg = ImageConversion.EncodeToJPG(imageToSend2, 100);
//Byte[] front_cam_image_jpg =imageToSend2.GetRawTextureData<Color32>()
string front_cam_image_base64 = Convert.ToBase64String(front_cam_image_jpg);
imgToSend.Append(front_cam_image_base64).Append("!");
//sending the image data
try
{
#if self2
imgMsg = new StringMsg(imgToSend.ToString());
obj.GetComponent <ROS_Initialize>().ros.Publish(ImagePublisher.GetMessageTopic(), imgMsg);
Debug.Log("Sending to topic: " + ImagePublisher.GetMessageTopic());
//Debug.Log(imgMsg);
#endif
}
catch (Exception e)
{
Debug.Log("Socket error" + e);
}
#endif
}
public ROSLocomotionControlParams(AgentJob job, ROSBridgeWebSocketConnection rosConnection, string topicName)
{
if (job == AgentJob.Publisher)
{
_publisher = new ROSGenericPublisher(rosConnection, topicName, StringMsg.GetMessageType());
rosConnection.AddPublisher(_publisher);
}
else if (job == AgentJob.Subscriber)
{
_subscriber = new ROSGenericSubscriber <StringMsg>(rosConnection, topicName, StringMsg.GetMessageType(), (msg) => new StringMsg(msg));
_subscriber.OnDataReceived += (data) =>
{
if (OnDataReceived != null)
{
OnDataReceived(data);
}
};
rosConnection.AddSubscriber(_subscriber);
}
}
// Update is called once per frame
void Update()
{
#if img
//encoding part:
StringBuilder imgToSend = new StringBuilder("", 500000);
//bottom cam encoding
RenderTexture.active = depthImage;
imageToSend.ReadPixels(new Rect(0, 0, depthImage.width, depthImage.height), 0, 0);
imageToSend.Apply();
Byte[] depth_cam_image_jpg = ImageConversion.EncodeToJPG(imageToSend, 100);
string depth_cam_image_base64 = Convert.ToBase64String(depth_cam_image_jpg);
imgToSend.Append(depth_cam_image_base64).Append("!");
//front cam encoding
RenderTexture.active = rgbImage;
imageToSend2.ReadPixels(new Rect(0, 0, rgbImage.width, rgbImage.height), 0, 0);
imageToSend2.Apply();
Byte[] rgb_cam_image_jpg = ImageConversion.EncodeToJPG(imageToSend2, 100);
string rgb_cam_image_base64 = Convert.ToBase64String(rgb_cam_image_jpg);
imgToSend.Append(rgb_cam_image_base64).Append("!");
//sending the image data
try
{
#if self
imgMsg = new StringMsg(imgToSend.ToString());
obj.GetComponent <ROSInitializerSLAM>().rosSLAM.Publish(ImagePublisher.GetMessageTopic(), imgMsg);
//Debug.Log("Sending to topic: " + ImagePublisher.GetMessageTopic());
//Debug.Log(imgMsg);
#endif
}
catch (Exception e)
{
Debug.Log("Socket error" + e);
}
#endif
}
/*
* private void HandleImage(ROSAgent sender, CompressedImageMsg compressedImage, CameraInfo info)
* {
* lock (_cameraDataToConsume)
* {
* _cameraInfoToConsume = info;
* _cameraDataToConsume = compressedImage;
* _hasCameraDataToConsume = true;
* }
* }*/
protected override void StartROS()
{
_rosLocomotionDirect = new ROSLocomotionDirect(ROSAgent.AgentJob.Publisher, _rosBridge, "/cmd_vel");
_rosLocomotionWaypoint = new ROSLocomotionWaypoint(ROSAgent.AgentJob.Publisher, _rosBridge, "/waypoint");
_rosLocomotionWaypointState = new ROSLocomotionWaypointState(ROSAgent.AgentJob.Publisher, _rosBridge, "/waypoint/state");
_rosLocomotionControlParams = new ROSLocomotionControlParams(ROSAgent.AgentJob.Publisher, _rosBridge, "/waypoint/control_parameters");
_rosLocomotionLinear = new ROSGenericPublisher(_rosBridge, "/waypoint/max_linear_speed", Float32Msg.GetMessageType());
_rosLocomotionAngular = new ROSGenericPublisher(_rosBridge, "/waypoint/max_angular_speed", Float32Msg.GetMessageType());
_rosOdometryOverride = new ROSGenericPublisher(_rosBridge, "/odo_calib_pose", OdometryMsg.GetMessageType());
_rosReset = new ROSGenericPublisher(_rosBridge, "arlobot/reset_motorBoard", BoolMsg.GetMessageType());
_rosLogger = new ROSGenericPublisher(_rosBridge, "/debug_output", StringMsg.GetMessageType());
_rosUltrasound = new ROSGenericSubscriber <StringMsg>(_rosBridge, "/ultrasonic_data", StringMsg.GetMessageType(), (msg) => new StringMsg(msg));
_rosUltrasound.OnDataReceived += ReceivedUltrasoundUpdata;
_rosOdometry = new ROSGenericSubscriber <OdometryMsg>(_rosBridge, "/robot_gps_pose", OdometryMsg.GetMessageType(), (msg) => new OdometryMsg(msg));
_rosOdometry.OnDataReceived += ReceivedOdometryUpdate;
_maxLinearSpeed = RobotConfig.MaxLinearSpeed;
_maxAngularSpeed = RobotConfig.MaxAngularSpeed;
_rosLocomotionLinear.PublishData(new Float32Msg(_maxLinearSpeed));
_rosLocomotionAngular.PublishData(new Float32Msg(_maxAngularSpeed));
_rosLocomotionControlParams.PublishData(RobotConfig.LinearSpeedParameter, RobotConfig.RollSpeedParameter, RobotConfig.PitchSpeedParameter, RobotConfig.AngularSpeedParameter);
}
public void ReceivedUltrasoundUpdata(ROSBridgeMsg data)
{
//TODO: Not implemented yet
StringMsg s = (StringMsg)data;
}
public void DisplayLogMessage(NetworkMessage msg)
{
StringMsg parsed = msg.ReadMessage <StringMsg> ();
DisplayLog(parsed.value);
}
public override void Initialise(ROSBridgeWebSocketConnection rosBridge)
{
base.Initialise(rosBridge);
_logs = new List <RobotLog>();
_subscriber = new ROSGenericSubscriber <StringMsg>(rosBridge, "/debug_output", StringMsg.GetMessageType(), (data) => new StringMsg(data));
_subscriber.OnDataReceived += LogReceivedData;
}
public static string ToYAMLString(StringMsg msg)
{
Debug.Log(msg.ToYAMLString());
return(msg.ToYAMLString());
}
public static string GetMessageType()
{
return(StringMsg.GetMessageType());
}
public override void Initialise(ROSBridgeWebSocketConnection rosBridge)
{
_rosUltrasoundPublisher = new ROSGenericPublisher(rosBridge, "/ultrasonic_data", StringMsg.GetMessageType());
base.Initialise(rosBridge);
}
public void ReceivedNavigationState(ROSBridgeMsg newState)
{
StringMsg data = (StringMsg)newState;
state = data._data;
}
/* Update function sends images from the cameras attached to the AUV, to the control algorithm. In order
* to maintain sufficient communication frequency, images cannot be sent in their entirety. Therefore,
* edge detection is performed on the images, to detect meaningful data.
*
*/
void Update()
{
// Examine whether Lock is required
Vector3 CurRot = transform.parent.transform.rotation.eulerAngles;
// if(CurRot.x > 180.0f)
// CurRot.x -= 360.0f;
// if(CurRot.y > 180.0f)
// CurRot.y -= 360.0f;
// if(CurRot.z > 180.0f)
// CurRot.z -= 360.0f;
//
// CurRot *= 3.14f/180.0f;
// Debug.Log (CurRot);
#if UsePics
// if(Input.GetKey(KeyCode.Space))
// firstSend = true;
// Debug.Log (Time.deltaTime);
if (!Lock)
{
Lock = true;
firstSend = false;
RenderTexture.active = bottomImage;
imageToSend.ReadPixels(new Rect(0, 0, bottomImage.width, bottomImage.height), 0, 0);
imageToSend.Apply();
StringBuilder pixelsToSend1 = new StringBuilder("", 500000);
StringBuilder pixelsToSend2 = new StringBuilder("", 500000);
StringBuilder pixelsToSend3 = new StringBuilder("", 500000);
StringBuilder pixelsToSend4 = new StringBuilder("", 500000);
StringBuilder pixelsToSend5 = new StringBuilder("", 500000);
StringBuilder pixelsToSend6 = new StringBuilder("", 500000);
int cnt = 0;
Color32[] allPixels = imageToSend.GetPixels32();
//Bottom Image R Component
for (int i = 0; i < bottomImage.height; i++)
{
bool prev = false;
bool noChanges = true;
for (int j = 0; j < bottomImage.width; j++)
{
Color32 currentVal = allPixels [(ImageHeight - 1 - i) * bottomImage.width + j];
if ((currentVal.r > colThreshHigh && currentVal.g < colThreshLow && currentVal.b < colThreshLow) != prev)
{
noChanges = false;
prev = !prev;
pixelsToSend1.Append(j).Append(":");
}
}
if (noChanges)
{
pixelsToSend1.Append("-1");
}
pixelsToSend1.Append(">");
}
//Bottom Image G Component
for (int i = 0; i < bottomImage.height; i++)
{
bool prev = false;
bool noChanges = true;
for (int j = 0; j < bottomImage.width; j++)
{
Color32 currentVal = allPixels [(ImageHeight - 1 - i) * bottomImage.width + j];
if ((currentVal.g > colThreshHigh && currentVal.r < colThreshLow && currentVal.b < colThreshLow) != prev)
{
noChanges = false;
prev = !prev;
pixelsToSend2.Append(j).Append(":");
}
}
if (noChanges)
{
pixelsToSend2.Append("-1");
}
pixelsToSend2.Append(">");
}
//Bottom Image B Component
for (int i = 0; i < bottomImage.height; i++)
{
bool prev = false;
bool noChanges = true;
for (int j = 0; j < bottomImage.width; j++)
{
Color32 currentVal = allPixels [(ImageHeight - 1 - i) * bottomImage.width + j];
if ((currentVal.b > colThreshHigh && currentVal.r < colThreshLow && currentVal.g < colThreshLow) != prev)
{
noChanges = false;
prev = !prev;
pixelsToSend3.Append(j).Append(":");
}
}
if (noChanges)
{
pixelsToSend3.Append("-1");
}
pixelsToSend3.Append(">");
}
// for(int i=0; i<bottomImage.width*bottomImage.height; i++) {
// Color32 pixel = allPixels [i];
// if (pixel.r > colThresh) {
// pixelsToSend1.Append(cnt.ToString ("00000")).Append(":");
// }
// cnt++;
// }
cnt = 0;
RenderTexture.active = frontImage;
imageToSend2.ReadPixels(new Rect(0, 0, frontImage.width, frontImage.height), 0, 0);
imageToSend2.Apply();
allPixels = imageToSend2.GetPixels32();
//Front Image R Component
for (int i = 0; i < frontImage.height; i++)
{
bool prev = false;
bool noChanges = true;
for (int j = 0; j < frontImage.width; j++)
{
Color32 currentVal = allPixels [(ImageHeight - 1 - i) * frontImage.width + j];
if ((currentVal.r > colThreshHigh && currentVal.g < colThreshLow && currentVal.b < colThreshLow) != prev)
{
noChanges = false;
prev = !prev;
pixelsToSend4.Append(j).Append(":");
}
}
if (noChanges)
{
pixelsToSend4.Append("-1");
}
pixelsToSend4.Append(">");
}
//Front Image G Component
for (int i = 0; i < frontImage.height; i++)
{
bool prev = false;
bool noChanges = true;
for (int j = 0; j < frontImage.width; j++)
{
Color32 currentVal = allPixels [(ImageHeight - 1 - i) * frontImage.width + j];
if ((currentVal.g > colThreshHigh && currentVal.r < colThreshLow && currentVal.b < colThreshLow) != prev)
{
noChanges = false;
prev = !prev;
pixelsToSend5.Append(j).Append(":");
}
}
if (noChanges)
{
pixelsToSend5.Append("-1");
}
pixelsToSend5.Append(">");
}
//Front Image B Component
for (int i = 0; i < frontImage.height; i++)
{
bool prev = false;
bool noChanges = true;
for (int j = 0; j < frontImage.width; j++)
{
Color32 currentVal = allPixels [(ImageHeight - 1 - i) * frontImage.width + j];
if ((currentVal.b > colThreshHigh && currentVal.r < colThreshLow && currentVal.g < colThreshLow) != prev)
{
noChanges = false;
prev = !prev;
pixelsToSend6.Append(j).Append(":");
}
}
if (noChanges)
{
pixelsToSend6.Append("-1");
}
pixelsToSend6.Append(">");
}
// for(int i=0; i<frontImage.width*frontImage.height; i++) {
// Color32 pixel = allPixels [i];
// if (pixel.r > colThresh) {
// pixelsToSend2.Append(cnt.ToString ("00000")).Append(":");
// }
// cnt++;
// }
// Debug.Log(pixelsToSend1.Length + " " + pixelsToSend1);
// Debug.Log("Bottom Cam" + pixelsToSend1.Length + " " + pixelsToSend1.ToString());
pixelsToSend1.Append("!").Append(pixelsToSend2).Append("!").Append(pixelsToSend3).Append("!")
.Append(pixelsToSend4).Append("!").Append(pixelsToSend5).Append("!").Append(pixelsToSend6);
// pixelsToSend1.Append ("!").Append (pixelsToSend3);
try {
#if notSelf
// theWriter.WriteLine(pixelsToSend1.Length.ToString("000000"));
// theWriter.Flush();
// pixelsToSend1.Insert(0, pixelsToSend1.Length.ToString("00000000") + " ");
imgMsg = new StringMsg(pixelsToSend1.ToString());
obj.GetComponent <ROS_Initialize> ().ros.Publish(ImagePublisher.GetMessageTopic(), imgMsg);
// theWriter.WriteLine(pixelsToSend1);//.ToString().Substring(0, 2000));//.ToString().Substring(0, 1022));
// theWriter.Flush();
// theWriter.WriteLine(pixelsToSend1.ToString().Substring(0, 2048));
// theWriter.WriteLine(pixelsToSend1);
// Debug.Log(Encoding.ASCII.GetBytes(pixelsToSend1.ToString()).Length);
// theStream.Write(Encoding.ASCII.GetBytes(pixelsToSend1.ToString()), 0, Encoding.ASCII.GetBytes(pixelsToSend1.ToString()).Length);
// theWriter.Flush();
#endif
// Debug.Log(pixelsToSend1.Length + " " + pixelsToSend1);
// Debug.Log(pixelsToSend2.Length + " " + pixelsToSend2);
// Debug.Log("Front Cam" + pixelsToSend2.Length + " " + pixelsToSend2.ToString());
// Debug.Log(pixelsToSend3.Length + " " + pixelsToSend3);
// Debug.Log(pixelsToSend3.Length + " " + pixelsToSend3.ToString());
} catch (Exception e) {
Debug.Log("Socket error" + e);
}
// pixelsToSend2 = "";
cnt = 0;
// foreach (Color32 pixel in imageToSend.GetPixels32()) {
// if (pixel.r > colThresh) {
//// pixelsToSend2 = pixelsToSend2 + " " + (cnt / bottomImage.width).ToString ("000") + " " +
//// (cnt % bottomImage.height).ToString ("000") + ":";
// Debug.Log (cnt);
// }
// cnt++;
// }
// try {
// #if notSelf
// theWriter.WriteLine(imageToSend.GetPixels32()[0]);
// theWriter.Flush();
// theWriter.WriteLine(":");
// theWriter.Flush();
// Debug.Log("afaeeagaggaegggggggggggggggggg " + imageToSend.GetPixels32()[0]);
//
// #endif
//
//// Debug.Log(pixelsToSend1.Length + pixelsToSend1);
// } catch (Exception e) {
// Debug.Log ("Socket error" + e);
// }
}
#endif
}
public static string ToYAMLString(StringMsg msg)
{
return(msg.ToYAMLString());
}
public override void Initialise(ROSBridgeWebSocketConnection rosBridge)
{
base.Initialise(rosBridge);
_rosLocomotionDirect = new ROSLocomotionDirect(ROSAgent.AgentJob.Publisher, rosBridge, "/cmd_vel");
_rosLocomotionState = new ROSGenericPublisher(rosBridge, "/waypoint/robot_state", StringMsg.GetMessageType());
_rosLocomotionWaypoint = new ROSLocomotionWaypoint(ROSAgent.AgentJob.Subscriber, rosBridge, "/waypoint");
_rosLocomotionWaypoint.OnDataReceived += ReceivedWaypoint;
_rosLocomotionWaypointState = new ROSLocomotionWaypointState(ROSAgent.AgentJob.Subscriber, rosBridge, "/waypoint/state");
_rosLocomotionWaypointState.OnDataReceived += ReceivedNavigationState;
_rosLocomotionControlParams = new ROSLocomotionControlParams(ROSAgent.AgentJob.Subscriber, rosBridge, "/waypoint/control_parameters");
_rosLocomotionControlParams.OnDataReceived += ReceivedNavigationParameters;
_rosLocomotionAngular = new ROSGenericSubscriber <Float32Msg>(rosBridge, "/waypoint/max_angular_speed", Float32Msg.GetMessageType(), (msg) => new Float32Msg(msg));
_rosLocomotionAngular.OnDataReceived += ReceivedNavigationAngularSpeedParameter;
_rosLocomotionLinear = new ROSGenericSubscriber <Float32Msg>(rosBridge, "/waypoint/max_linear_speed", Float32Msg.GetMessageType(), (msg) => new Float32Msg(msg));
_rosLocomotionLinear.OnDataReceived += ReceivedNavigationLinearSpeedParameter;
}
