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"]); }