public override void ExtendedStart()
{
odomHandler.isPublishing = true;
odomHandler.isROSMsg = isROSMsg;
odom = new Odometry();
}
public override void ExtendedStart()
{
base.ExtendedStart();
odomHandler.isROSMsg = isROSMsg;
odomHandler.isPublishing = false;
odom = new Odometry();
odomHandler.space = referenceSystem;
}
void UpdateDroneOdom(Odometry odom)
{
Vector3 pos = odom.pose.pose.position.ToUnity();
Quaternion quat = new Quaternion((float)odom.pose.pose.orientation.x, (float)odom.pose.pose.orientation.y, (float)odom.pose.pose.orientation.z, (float)odom.pose.pose.orientation.w);
Quaternion rot = TransformExtensions.Ros2Unity(quat);
Drone.transform.localPosition = pos;
Drone.transform.localRotation = rot;
}
public static void Update(this IPose pose, Odometry odom)
{
pose.X = odom.pose.pose.position.x;
pose.Y = odom.pose.pose.position.y;
pose.Z = odom.pose.pose.position.z;
pose.i = odom.pose.pose.orientation.x;
pose.j = odom.pose.pose.orientation.y;
pose.k = odom.pose.pose.orientation.z;
pose.w = odom.pose.pose.orientation.w;
}
private void CarmenLogParser(string l, ref bool completed)
{
USARParser p = new USARParser(l, "\":", true);
string odo = p.getString("odometry").Replace(" ", "");
odo = "SEN {Type Odometry} {Name Odometry} {Pose " + odo + "}";
mUSARParser.SimulationMessage = odo;
Odometry mOdo = (Odometry)mUSARParser.MessageData;
Pose2D mNewPose = new Pose2D(mOdo.x * m3DScaleFactor, mOdo.y * m3DScaleFactor, mOdo.theta);
mPrevPose = mCurrPose;
mCurrPose = mNewPose;
if (mPrevPose != null)
{
mSeed = new Pose2D(mSeed.Position + (mCurrPose.Position - mPrevPose.Position),
mSeed.Rotation - (mCurrPose.Rotation - mPrevPose.Rotation));
}
else
{
mSeed = new Pose2D();
}
string lsr = p.getString("readings").Replace(" ", "");
string valid = p.getString("valid").Replace(" ", "");
string theta = p.getString("theta").Replace(" ", "");
float nrays = float.Parse(p.getString("nrays"));
float min_theta = float.Parse(p.getString("min_theta"));
float max_theta = float.Parse(p.getString("max_theta"));
float fov = max_theta - min_theta;
float res = fov / nrays;
string range = "SEN {Time 0.0} {Type RangeScanner} {Name Scanner1} {Resolution " + res + "} {FOV " + fov + "} {Range " + lsr + "} {Valid " +
valid + "} {Theta " + theta + "}";
mUSARParser.SimulationMessage = range;
Laser mLaser = (Laser)mUSARParser.MessageData;
if (mPrevLaser == null)
{
mPrevLaser = mLaser;
}
else
{
mCurrLaser = mLaser;
completed = true;
}
}
protected override void Handle(Odometry message)
{
if (Container?.Count == 0)
{
Container.Add(message.GetPose() !.ToTracked());
}
else
{
Container?.Update(message.GetPose() !.ToTracked());
}
}
private void ReportNewAsaRelPose(Odometry relPose)
{
var anchor = missioncontroller.GetAnchorControllerById(relPose.header.frame_id);
if (anchor == null)
{
Debug.Log("Anchor for pose was null");
return;
}
TransformHelper.transform.SetParent(anchor.transform, false);
var position = relPose.pose.pose.position;
var orientation = relPose.pose.pose.orientation;
(var pos, var or) = PoseExtensions.Ros2Unity(position, orientation);
TransformHelper.transform.localPosition = pos;
TransformHelper.transform.localRotation = or;
if (anchor != null)
{
//Calculate the relative position to the current anchor
//pos = anchor.transform.InverseTransformPoint(TransformHelper.transform.position);
//or = (Quaternion.Inverse(anchor.transform.rotation) * TransformHelper.transform.rotation);
robotState.Pose.SetFromPosition(pos);
robotState.Pose.SetFromRotation(or);
}
else
{
//we do not have a current anchor yet, so assume that the anchor is still the one at origin
robotState.Pose.SetFromRosSharp(position, orientation);
}
//Animate the robot's base odometry
if (robotModelActive)
{
RobotBase.transform.SetParent(anchor.transform, false);
RobotBase.transform.localPosition = pos;
RobotBase.transform.localRotation = or;
}
//if(targetPose != null)
//{
// Debug.Log($"{robotState.Pose.X - targetPose.X} | {robotState.Pose.Z - targetPose.Z}");
//}
Robot.Delegates.UpdateRobotState(robotState);
}
void odomCallback (Odometry odom)
{
// calculate acceleration
if ( header_.Stamp.data.toSec () != 0.0 && odom.header.Stamp.data.toSec () != 0.0 )
{
double acceleration_time_constant = 0.1;
double dt = ( ( odom.header.Stamp - header_.Stamp ).data.toSec () );
if ( dt > 0.0 )
{
acceleration_.linear.x = ( ( odom.twist.twist.linear.x - twist_.linear.x ) + acceleration_time_constant * acceleration_.linear.x ) / ( dt + acceleration_time_constant );
acceleration_.linear.y = ( ( odom.twist.twist.linear.y - twist_.linear.y ) + acceleration_time_constant * acceleration_.linear.y ) / ( dt + acceleration_time_constant );
acceleration_.linear.z = ( ( odom.twist.twist.linear.z - twist_.linear.z ) + acceleration_time_constant * acceleration_.linear.z ) / ( dt + acceleration_time_constant );
acceleration_.angular.x = ( ( odom.twist.twist.angular.x - twist_.angular.x ) + acceleration_time_constant * acceleration_.angular.x ) / ( dt + acceleration_time_constant );
acceleration_.angular.y = ( ( odom.twist.twist.angular.y - twist_.angular.y ) + acceleration_time_constant * acceleration_.angular.y ) / ( dt + acceleration_time_constant );
acceleration_.angular.z = ( ( odom.twist.twist.angular.z - twist_.angular.z ) + acceleration_time_constant * acceleration_.angular.z ) / ( dt + acceleration_time_constant );
}
}
header_ = odom.header;
pose_ = odom.pose.pose;
twist_ = odom.twist.twist;
}
public ExampleMessage()
{
a = new int();
str = new String();
odom = new Odometry();
}
/// <summary>
/// Gets the odometry from the odom handler as Odometry message. Uses the reference system indicated in the inspector.
/// </summary>
public void GetOdom()
{
odom = odomHandler.GetOdom(referenceSystem);
}
protected void DigestOdometryStateMessage(Odometry odom)
{
State.Pose.Update(odom);
State.PoseHasChanged = true;
}
/// <summary>
/// Gets the current odometry of the transform either in local or world space and updates the odometry data in the odom handler.
/// </summary>
/// <param name="space">The reference frame of the odometry</param>
/// <returns>The odometry of the transform.</returns>
public Odometry GetOdom(Space space)
{
Odometry odom = (space == Space.Self) ? GetLocalOdom() : GetWorldOdom();
return(odom);
}
/// <summary>
/// Sets the target position and orientation of the transform.
/// </summary>
/// <param name="odom">The odometry data <c>DTStacks.DataType.ROS.Messages.nav_msgs.Odometry</c></param>
public void SetTargetPose(Odometry odom)
{
this.odom = odom;
}
