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; }