public void PublishData(float rho, float roll, float pitch, float yaw) { if (_publisher != null) { _publisher.PublishData(new StringMsg(string.Format("{0}, {1}, {2}, {3}", rho, roll, pitch, yaw))); } }
protected override void PublishSensorData() { JSONObject json = new JSONObject(); json.AddField(_sensorId, GetSensorData()); _rosUltrasoundPublisher.PublishData(new StringMsg(json.ToString())); }
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 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 void PublishData(Texture2D texture2D, int qualityLevel, int sequenceId) { HeaderMsg header = new HeaderMsg(sequenceId, new TimeMsg(0, 0), "raspicam"); CompressedImageMsg imageMsg = new CompressedImageMsg(header, "jpg", texture2D.EncodeToJPG(qualityLevel)); _publisher.PublishData(imageMsg); }
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); }
private IEnumerator adsjio() { while (true) { yield return(new WaitForSeconds(2)); _rosLogger.PublishData(new StringMsg("Hi!")); } }
private IEnumerator PublishLoop(float interval) { while (_rosBridge != null) { _rosFiducialMapPublisher.PublishData(GetFiducialMapForPublish()); yield return(new WaitForSeconds(interval)); } }
/// <param name="data">X: Angular speed in meter/s, Y: Linear speed in meter/s</param> public void PublishData(GeoPointWGS84 data) { if (_publisher == null) { return; } NavSatFixMsg navSatFix = new NavSatFixMsg(data.latitude, data.longitude, data.altitude); _publisher.PublishData(navSatFix); }
public void PublishData(string data) { if (_publisher == null) { return; } StringMsg state = new StringMsg(data); _publisher.PublishData(state); }
/// <param name="linear">Linear movement speed in meter/second</param> /// <param name="angular">Angular rotational speed in meter/second</param> public void PublishData(float linear, float angular) { if (_publisher == null) { return; } TwistMsg twist = new TwistMsg(new Vector3Msg(linear, 0, 0), new Vector3Msg(0, 0, angular)); _publisher.PublishData(twist); }
protected override void PublishSensorData() { _camera.targetTexture = _renderTexture; _camera.Render(); RenderTexture.active = _renderTexture; _texture2D.ReadPixels(_rect, 0, 0); _camera.targetTexture = null; RenderTexture.active = null; HeaderMsg header = new HeaderMsg(_sequenceId, new TimeMsg(0, 0), "raspicam"); RegionOfInterestMsg roi = new RegionOfInterestMsg(0, 0, 0, 0, false); CameraInfoMsg camInfo = new CameraInfoMsg(header, (uint)_resolutionHeight, (uint)_resolutionWidth, "plumb_bob", Distortion, CameraMatrix, Rectification, Projection, 0, 0, roi); _cameraSensorPublisher.PublishData(_texture2D, _cameraQualityLevel, _sequenceId); _cameraInfoPublisher.PublishData(camInfo); _sequenceId++; }
//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)); } }
/* * 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); }
void Update() { if (_rosBridge == null) { return; } if (!goal_set) { if (!_sentCommand) { _rosLocomotionState.PublishData(new StringMsg(subState)); _sentCommand = true; } return; } else { _sentCommand = false; } angle = Vector3.SignedAngle(goal - transform.position, transform.forward, transform.up) * Mathf.Deg2Rad; distance = Vector3.Distance(transform.position, goal); switch (state) { case "RUNNING": switch (subState) { default: subState = "TURNING"; break; case "TURNING": vel._linear._x = 0; vel._angular._z = k_alpha * angle; if (Mathf.Abs(angle) < 0.2f) { subState = "FORWARDING"; } break; case "FORWARDING": if (angle > Mathf.PI / 2) { vel._linear._x = -k_rho * distance; vel._angular._z = k_alpha * (angle - Mathf.PI); } else if (angle < -Mathf.PI / 2) { vel._linear._x = -k_rho * distance; vel._angular._z = k_alpha * (angle + Mathf.PI); } else { vel._linear._x = k_rho * distance; vel._angular._z = k_alpha * angle; } vel._linear._x = Mathf.Clamp((float)vel._linear._x, -vel_maxlin, vel_maxlin); vel._angular._z = Mathf.Clamp((float)vel._angular._z, -vel_maxang, vel_maxang); PublishVelocity(vel); break; } PublishVelocity(vel); break; case "PARK": subState = "STOP"; vel._linear._x = 0; vel._angular._z = 0; PublishVelocity(vel); break; default: if (prestate == "RUNNING") { subState = "STOP"; vel._linear._x = 0; vel._angular._z = 0; PublishVelocity(vel); } else { subState = "IDLE"; } break; } prestate = state; _publishTimer -= Time.deltaTime; if (_publishTimer <= 0) { _publishTimer = _publishInterval; _rosLocomotionWaypointState.PublishData(subState); } }