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 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);
}
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);
}
//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 static string ToYAMLString(OdometryMsg msg)
{
return(msg.ToYAMLString());
}
public new static ROSBridgeMsg ParseMessage(JSONNode msg)
{
OdometryMsg odometry = new OdometryMsg(msg);
return(odometry);
}
/*
* 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);
}
