public new static void CallBack(ROSBridgeMsg msg)
{
Float32Msg speedMessage = (Float32Msg)msg;
playerSpeed = speedMessage.GetData();
Debug.Log("speedMessage: " + playerSpeed);
}
public void SetNavigationMovementParameters(Float32Msg maxLinearVel = null, Float32Msg maxAngularVel = null)
{
if (maxLinearVel != null)
{
vel_maxlin = maxLinearVel._data;
}
if (maxAngularVel != null)
{
vel_maxang = maxAngularVel._data;
}
}
public NavStsMsg(JSONNode msg)
{
_header = new HeaderMsg(msg["header"]);
_global_position = new DecimalLatLonMsg(msg["global_position"]);
_origin = new DecimalLatLonMsg(msg["origin"]);
_position = new NEDMsg(msg["position"]);
_altitude = new Float32Msg(msg["altitude"]);
_body_velocity = new PointMsg(msg["body_velocity"]);
_orientation = new RPYMsg(msg["orientation"]);
_orientation_rate = new RPYMsg(msg["orientation_rate"]);
_position_variance = new NEDMsg(msg["position_variance"]);
_orientation_variance = new RPYMsg(msg["orientation_variance"]);
_status = new UInt8Msg(msg["status"]);
}
void Update()
{
try
{
float p = PingerAngle();
pingermsg = new Float32Msg(p);
obj.GetComponent <ROS_Initialize>().ros.Publish(PingerPublisher.GetMessageTopic(), pingermsg);
/* Debug.Log("Sending: PingerAngle = " + p);
* Debug.Log("Sending to topic: " + PingerPublisher.GetMessageTopic());
*/
}
catch (Exception e)
{
Debug.Log("Socket error: " + e);
}
}
void Update()
{
try
{
float d = FlareAngle();
flaremsg = new Float32Msg(d);
obj.GetComponent <ROS_Initialize>().ros.Publish(FlarePublisher.GetMessageTopic(), flaremsg);
/* Debug.Log("Sending: FlareAngle = " + d);s
* Debug.Log("Sending to topic: " + FlarePublisher.GetMessageTopic());
*/
}
catch (Exception e)
{
Debug.Log("Socket error: " + e);
}
}
public NavStsMsg(JSONNode msg)
{
//Debug.Log("NavStsMsg with " + msg.ToString());
_header = new HeaderMsg(msg["header"]);
_global_position = new DecimalLatLonMsg(msg["global_position"]);
_origin = new DecimalLatLonMsg(msg["origin"]);
_position = new NEDMsg(msg["position"]);
_altitude = new Float32Msg(msg["altitude"]);
_body_velocity = new PointMsg(msg["body_velocity"]);
_orientation = new RPYMsg(msg["orientation"]);
_orientation_rate = new RPYMsg(msg["orientation_rate"]);
_position_variance = new NEDMsg(msg["position_variance"]);
_orientation_variance = new RPYMsg(msg["orientation_variance"]);
_status = new UInt8Msg(msg["status"]);
//Debug.Log("NavStsMsg done and it looks like " + this.ToString());
}
public NavStsMsg(HeaderMsg header,
DecimalLatLonMsg global_position,
DecimalLatLonMsg origin,
NEDMsg position,
Float32Msg altitude,
PointMsg body_velocity,
RPYMsg orientation,
RPYMsg orientation_rate,
NEDMsg position_variance,
RPYMsg orientation_variance,
UInt8Msg status)
{
_header = header;
_global_position = global_position;
_origin = origin;
_position = position;
_altitude = altitude;
_body_velocity = body_velocity;
_orientation = orientation;
_orientation_rate = orientation_rate;
_position_variance = position_variance;
_orientation_variance = orientation_variance;
_status = status;
}
public new static void CallBack(ROSBridgeMsg msg)
{
GameObject model = GameObject.Find("Model");
GameObject robot = GameObject.Find("Robot");
Robot r = (Robot)model.GetComponent(typeof(Robot));
if (model == null)
{
Debug.Log("<color=red>ERROR:</color> Can't find GameObject Robot. Create it in Unity");
}
else
{
NavStsMsg nav_sts = (NavStsMsg)msg;
NEDMsg p = nav_sts.GetPosition();
RPYMsg o = nav_sts.GetOrientation();
Float32Msg altitude = nav_sts.GetAltitude();
// Debug.Log("position" + p + "altitude" + altitude);
robot.transform.position = new Vector3(-p.GetNorth(), 0, p.GetEast());
Vector3 next_position = new Vector3(-p.GetNorth(), -p.GetDepth(), p.GetEast());
Quaternion next_orientation = Quaternion.Euler(o.GetRollDegrees() + 90f, o.GetYawDegrees(), o.GetPitchDegrees());
r.AddWaypoint(next_position, next_orientation);
//model.transform.position = new Vector3(-p.GetNorth(), -p.GetDepth(), p.GetEast());
//model.transform.rotation = Quaternion.Euler(o.GetRollDegrees() + 90f, o.GetYawDegrees(), o.GetPitchDegrees());
GameObject ned_canvas = GameObject.Find("NED");
Text NED_text = ned_canvas.GetComponent <Text>();
int dp = 2;
string format = string.Format("#.{0} m;-#.{0} m", new string('#', dp));
NED_text.text = p.GetNorth().ToString(format) +
"\n" + p.GetEast().ToString(format) +
"\n" + p.GetDepth().ToString(format) +
"\n" + nav_sts.GetAltitude().GetData().ToString(format);
}
}
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);
}
public static string ToYAMLString(Float32Msg msg)
{
return(msg.ToYAMLString());
}
private void ReceivedNavigationAngularSpeedParameter(ROSBridgeMsg parameter)
{
Float32Msg data = (Float32Msg)parameter;
vel_maxang = data._data;
}
public override void Initialise(ROSBridgeWebSocketConnection rosBridge)
{
base.Initialise(rosBridge);
_rosLocomotionDirect = new ROSLocomotionDirect(ROSAgent.AgentJob.Publisher, rosBridge, "/cmd_vel");
_rosLocomotionState = new ROSGenericPublisher(rosBridge, "/waypoint/robot_state", StringMsg.GetMessageType());
_rosLocomotionWaypoint = new ROSLocomotionWaypoint(ROSAgent.AgentJob.Subscriber, rosBridge, "/waypoint");
_rosLocomotionWaypoint.OnDataReceived += ReceivedWaypoint;
_rosLocomotionWaypointState = new ROSLocomotionWaypointState(ROSAgent.AgentJob.Subscriber, rosBridge, "/waypoint/state");
_rosLocomotionWaypointState.OnDataReceived += ReceivedNavigationState;
_rosLocomotionControlParams = new ROSLocomotionControlParams(ROSAgent.AgentJob.Subscriber, rosBridge, "/waypoint/control_parameters");
_rosLocomotionControlParams.OnDataReceived += ReceivedNavigationParameters;
_rosLocomotionAngular = new ROSGenericSubscriber <Float32Msg>(rosBridge, "/waypoint/max_angular_speed", Float32Msg.GetMessageType(), (msg) => new Float32Msg(msg));
_rosLocomotionAngular.OnDataReceived += ReceivedNavigationAngularSpeedParameter;
_rosLocomotionLinear = new ROSGenericSubscriber <Float32Msg>(rosBridge, "/waypoint/max_linear_speed", Float32Msg.GetMessageType(), (msg) => new Float32Msg(msg));
_rosLocomotionLinear.OnDataReceived += ReceivedNavigationLinearSpeedParameter;
}
/*
* 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);
}
