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);
}
}
private async void Set_GroundStationMode(object sender, RoutedEventArgs e)
{
BoolMsg yn = new BoolMsg();
yn.value = true;
var err = await DJISDKManager.Instance.ComponentManager.GetFlightControllerHandler(0, 0).SetGroundStationModeEnabledAsync(yn);
Debug.WriteLine("GSM err : " + err.ToString());
DJISDKManager.Instance.WaypointMissionManager.GetWaypointMissionHandler(0).StateChanged += Waypoint_Mission_State;
}
// Update is called once per 0.3 seconds
private IEnumerator WaitAndPrint(float waitTime)
{
BoolMsg msg = new BoolMsg(true);
//string str;
while (true)
{
yield return(new WaitForSeconds(waitTime));
ros.Publish(RobotMovingPublisher.GetMessageTopic(), msg);
//str = UnityEngine.StackTraceUtility.ExtractStackTrace();
Debug.Log("hihi");
ros.Render();
}
}
/*
* 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);
}
public static string ToYAMLString(BoolMsg msg)
{
Debug.Log(msg.ToYAMLString());
return(msg.ToYAMLString());
}
public static string ToString(BoolMsg msg)
{
return(msg.ToString());
}
