public OdometryMsg(HeaderMsg header, string child_frame_id, PoseWithCovarianceMsg pose, TwistWithCovarianceMsg twist)
{
_header = header;
_child_frame_id = child_frame_id;
_pose = pose;
_twist = twist;
}
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);
}
}
public override void Deserialize(JSONNode msg)
{
_header = new HeaderMsg(msg["header"]);
_child_frame_id = msg["child_frame_id"].ToString();
_pose = new PoseWithCovarianceMsg(msg["pose"]);
_twist = new TwistWithCovarianceMsg(msg["twist"]);
}
public OdometryMsg(JSONNode msg)
{
_header = new HeaderMsg(msg["header"]);
_child_frame_id = msg["child_frame_id"];
_pose = new PoseWithCovarianceMsg(msg["pose"]);
_twist = new TwistWithCovarianceMsg(msg["twist"]);
}
public override void OverridePositionAndOrientation(Vector3 newPosition, Quaternion newOrientation)
{
_rosLogger.PublishData(new StringMsg("Odometry overwritten"));
//Unity repositioning for the virtual robot.
transform.SetPositionAndRotation(newPosition, newOrientation);
//reset initial poses for the odom calculation. This is the equivalent of moving the robot in the physical room.
// We simply move the local coords.
InitialPosition = newPosition;
InitialRotation = newOrientation;
//Publishing to odo_calib_pose for initial gps position, just like the arlobot controller.
GeoPointWGS84 wgs84 = newPosition.ToUTM().ToWGS84();
PoseWithCovarianceMsg pose = new PoseWithCovarianceMsg(
new PoseMsg(
new PointMsg(wgs84.longitude, wgs84.latitude, wgs84.altitude),
new QuaternionMsg(newOrientation.x, newOrientation.z, newOrientation.y, newOrientation.w)
));
OdometryMsg odometryUpdate = new OdometryMsg(pose);
_rosOdometryOverride.PublishData(odometryUpdate);
}
public OdometryMsg(PoseWithCovarianceMsg pose)
{
_header = new HeaderMsg(0, new TimeMsg(0, 0), "0");
_child_frame_id = "0";
_pose = pose;
_twist = new TwistWithCovarianceMsg(new TwistMsg(new Vector3Msg(0, 0, 0), new Vector3Msg(0, 0, 0)), new double[36]);
}
public DetectedPersonMsg(float confidence, uint detection_id, string modality, PoseWithCovarianceMsg pose)
{
_confidence = confidence;
_detection_id = detection_id;
_modality = modality;
_pose = pose;
}
public DetectedPersonMsg(JSONNode msg)
{
_confidence = msg["confidence"].AsFloat;
_detection_id = uint.Parse(msg["detection_id"]);
_modality = msg["modality"];
_pose = new PoseWithCovarianceMsg(msg["pose"]);
}
//publishing on the /odom topic
// the newposition should be the difference between the current position
//and the InitialPosition of the robot in Unity vec3, effectively simulating the local space coords of odom
//The geopoint conversion is in here.
public void PublishOdometryData(Vector3 newPosition, Quaternion newOrientation)
{
GeoPointWGS84 wgs84 = newPosition.ToUTM().ToWGS84();
PoseWithCovarianceMsg pose = new PoseWithCovarianceMsg(
new PoseMsg(
new PointMsg(wgs84.longitude, wgs84.latitude, wgs84.altitude),
new QuaternionMsg(newOrientation.x, newOrientation.z, newOrientation.y, newOrientation.w)
));
OdometryMsg odometryUpdate = new OdometryMsg(pose);
// _rosOdometry.PublishData(odometryUpdate);
}
public override void OverridePositionAndOrientation(Vector3 newPosition, Quaternion newOrientation)
{
GeoPointWGS84 wgs84 = newPosition.ToUTM().ToWGS84();
PoseWithCovarianceMsg pose = new PoseWithCovarianceMsg(
new PoseMsg(
new PointMsg(wgs84.longitude, wgs84.latitude, wgs84.altitude),
new QuaternionMsg(newOrientation.x, newOrientation.z, newOrientation.y, newOrientation.w)
));
OdometryMsg odometryOverride = new OdometryMsg(pose);
_rosOdometryOverride.PublishData(odometryOverride);
}
//callback functions is called whenever the message is received
public static void CallBack(ROSBridgeMsg msg)
{
GameObject cube_obj = GameObject.Find("Cube");
if (cube_obj == null)
{
Debug.Log("Cube not found!!!");
}
else
{
OdometryMsg odom = (OdometryMsg)msg;
PoseWithCovarianceMsg posemsg = odom._pose;
PoseMsg pose = posemsg.GetPose(); // get the pose part of the odometry message
//move the cube in the scene along with the kinect which is on top of the robot
cube_obj.transform.position = new Vector3(pose._position.GetX(), pose._position.GetZ(), pose._position.GetY());
cube_obj.transform.rotation = Quaternion.Euler(-pose._orientation.GetX() * 180.0f / 3.1415f, -pose._orientation.GetZ() * 180.0f / 3.1415f, -pose._orientation.GetY() * 180.0f / 3.1415f);
}
}
//Publisher to /robot_gps_pose every interval time
private IEnumerator SendTransformUpdate(float interval)
{
while (true)
{
//Add the Noise here. Unity should have the ACTUAL position of the physical world
// and what it publishes and we see on the webmap is the where it thinks it is.
GeoPointWGS84 wgs = transform.position.ToUTM().ToWGS84();
Quaternion rot = transform.rotation;
PoseMsg pose = new PoseMsg(new PointMsg(wgs.longitude, wgs.latitude, wgs.altitude),
new QuaternionMsg(rot.x, rot.y, rot.z, rot.w));
PoseWithCovarianceMsg poseWithCovariance = new PoseWithCovarianceMsg(pose, new double[36]);
OdometryMsg odometry = new OdometryMsg(poseWithCovariance);
odometry._pose = poseWithCovariance;
_rosOdometryGPSPose.PublishData(odometry);
yield return(new WaitForSeconds(interval));
}
}
// Send Odometry to ROS
private IEnumerator SendOdometryToROS(ROS ros)
{
while (Application.isPlaying)
{
if (ros.IsConnected())
{
double[] covariance_pose = new double[36];
for (int i = 0; i < covariance_pose.Length; i++)
{
covariance_pose[i] = 0.0f;
}
HeaderMsg globalHead = new HeaderMsg(0, new TimeMsg(DateTime.Now.Second, 0), "map");
PoseWithCovarianceMsg pose = new PoseWithCovarianceMsg(new PoseMsg(new PointMsg(transform.position, true), new QuaternionMsg(transform.rotation, true)),
covariance_pose);
TwistWithCovarianceMsg twist = new TwistWithCovarianceMsg(new TwistMsg(new Vector3Msg(agent.velocity, true), new Vector3Msg(0.0f, 0.0f, 0.0f)), covariance_pose);
OdometryMsg odometrymsg = new OdometryMsg(globalHead, "odom", pose, twist);
ros.Publish(Odometry_pub.GetMessageTopic(), odometrymsg);
}
yield return(new WaitForSeconds(rOSFrecuencyOdometry));
}
}
public PoseWithCovarianceStampedMsg(HeaderMsg header, PoseWithCovarianceMsg pose)
{
_header = header;
_pose = pose;
}
public PoseWithCovarianceStampedMsg(JSONNode msg)
{
_header = new HeaderMsg(msg ["header"]);
_pose = new PoseWithCovarianceMsg(msg["pose"]);
}
