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 void Initialise()
{
ros = new ROSBridgeWebSocketConnection("ws://192.168.255.40", 9090, "Test");
_genericPub = new ROSGenericPublisher(ros, "/raspicam_node/camera_info", CameraInfoMsg.GetMessageType());
_genericSub = new ROSGenericSubscriber <CameraInfoMsg>(ros, "/raspicam_node/camera_info", CameraInfoMsg.GetMessageType(), msg => new CameraInfoMsg(msg));
_genericSub.OnDataReceived += OnDataReceived;
ros.Connect();
ros.Render();
StartCoroutine(WaitForRunning());
}
/// <summary>
/// Register rosbridge and start publishing FiducialMapEntryArrayMsg to bridge.
/// </summary>
public void Register(ROSBridgeWebSocketConnection rosBridge)
{
if (_rosBridge != null)
{
Unregister(_rosBridge);
}
_rosBridge = rosBridge;
_rosFiducialMapPublisher = new ROSGenericPublisher(rosBridge, "/fiducial_map_gps", FiducialMapEntryArrayMsg.GetMessageType());
_publishLoop = StartCoroutine(PublishLoop(_publishInterval));
}
public override void Initialise(ROSBridgeWebSocketConnection rosBridge)
{
_texture2D = new Texture2D(_resolutionWidth, _resolutionHeight, TextureFormat.RGB24, false);
_rect = new Rect(0, 0, _resolutionWidth, _resolutionHeight);
_renderTexture = new RenderTexture(_resolutionWidth, _resolutionHeight, 24);
_cameraInfoPublisher = new ROSGenericPublisher(rosBridge, "/raspicam_node/camera_info", CameraInfoMsg.GetMessageType());
_cameraSensorPublisher = new ROSCameraSensorPublisher(rosBridge, "/raspicam_node/image/compressed");
base.Initialise(rosBridge);
}
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;
}
public ROSLocomotionDirect(AgentJob job, ROSBridgeWebSocketConnection rosConnection, string topicName)
{
if (job == AgentJob.Publisher)
{
_publisher = new ROSGenericPublisher(rosConnection, topicName, TwistMsg.GetMessageType());
rosConnection.AddPublisher(_publisher);
}
else if (job == AgentJob.Subscriber)
{
_subscriber = new ROSGenericSubscriber <TwistMsg>(rosConnection, topicName, TwistMsg.GetMessageType(), (msg) => new TwistMsg(msg));
_subscriber.OnDataReceived += (data) =>
{
if (OnDataReceived != null)
{
OnDataReceived(data);
}
};
rosConnection.AddSubscriber(_subscriber);
}
}
/*
* 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 override void Initialise(ROSBridgeWebSocketConnection rosBridge)
{
_rosUltrasoundPublisher = new ROSGenericPublisher(rosBridge, "/ultrasonic_data", StringMsg.GetMessageType());
base.Initialise(rosBridge);
}
public ROSCameraSensorPublisher(ROSBridgeWebSocketConnection rosConnection, string topicName)
{
_publisher = new ROSGenericPublisher(rosConnection, topicName, CompressedImageMsg.GetMessageType());
rosConnection.AddPublisher(_publisher);
}
