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