internal RosOutLogger(string typeName, string nodeId, LogLevel logLevel, bool showLevel, bool showDateTime, bool showLogName,string dateTimeFormat)
: base(typeName, logLevel, showLevel, showDateTime, showLogName, dateTimeFormat)
{
_nodeId = nodeId;
_node = Ros.GetNodes().FirstOrDefault(x => x.NodeId == _nodeId);
if (_node != null)
{
_publisher = _node.LogPubliser;
}
}
protected override void WriteInternal(LogLevel targetLevel, object message, Exception e)
{
var sb = new StringBuilder();
FormatOutput(sb, targetLevel, message, e);
if(_node == null)
{
_node = Ros.GetNodes().FirstOrDefault(x => x.NodeId == _nodeId);
}
LogLevel currentLevel = LogLevel.Info;
string nodeId = "unknown";
if (_node != null)
{
currentLevel = _node.LogLevel.ToLogLevel();
nodeId = _node.NodeId;
}
if (_publisher == null && _node != null)
{
_publisher = _node.LogPubliser;
}
if (_publisher != null)
{
if (targetLevel >= currentLevel)
{
_publisher.OnNext(new Log()
{
name = nodeId,
level = currentLevel.ToLogLevel(),
msg = message.ToString()
});
}
}
}
/// <summary>
/// ROS切断
/// </summary>
public void disconnect()
{
if (null != rosNode)
{
try
{
if (null != pubRE) pubRE.Dispose();
if (null != pubPlot) pubPlot.Dispose();
if (null != pubCompus) pubCompus.Dispose();
if (null != pubGPS) pubGPS.Dispose();
if (null != pubUrg) pubUrg.Dispose();
if (null != pubBenzLED) pubBenzLED.Dispose();
if (null != subVSlam) subVSlam.Dispose();
if (null != subRosif_pub) subRosif_pub.Dispose();
if (null != subUrg) subUrg.Dispose();
if (null != subClock) subClock.Dispose();
} catch { }
try
{
rosNode.Dispose();
Console.WriteLine("ROS Node Dispose");
}
catch (Exception ex)
{
Console.WriteLine(ex.Message);
}
finally
{
rosNode = null;
}
System.Threading.Thread.Sleep(1000);
try
{
Ros.Dispose();
Console.WriteLine("ROS Dispose");
}
catch (Exception ex)
{
Console.WriteLine(ex.Message);
}
System.Threading.Thread.Sleep(3000);
}
}
// ================================================================================================
/// <summary>
/// ROS接続
/// </summary>
/// <param name="myAddr"></param>
/// <param name="tgtAddr"></param>
public void connect( string myAddr, string tgtAddr )
{
// ローカルネットワークのホスト名またはIPアドレス
Ros.HostName = myAddr;
// Masterへの接続URI
Ros.MasterUri = new Uri(tgtAddr);
// ROS TOPICのタイムアウト時間[msec]
Ros.TopicTimeout = 3000;
// XML-RPCのメソッド呼び出しのタイムアウト時間[msec]
Ros.XmlRpcTimeout = 3000;
// Subscriver
try
{
// Nodeの生成。Nodeが生成されるまで待つ。
// Node名
rosNode = Ros.InitNodeAsync(NodeName).Result;
// Subscriberの生成。Subscriberが生成されるまで待つ。
// subVSlam = rosNode.SubscriberAsync<RosSharp.visualization_msgs.Marker>("/svo/points").Result;
subVSlam = rosNode.SubscriberAsync<RosSharp.geometry_msgs.Twist>("/torosif/vslam").Result;
//var subscriber = rosNode.SubscriberAsync<RosSharp.geometry_msgs.Twist>("/turtle1/cmd_vel").Result;
//var subscriber = rosNode.SubscriberAsync<RosSharp.std_msgs.String>("/chatter").Result;
subRosif_pub = rosNode.SubscriberAsync <RosSharp.geometry_msgs.Twist> ("/rosif/base_link").Result;
//subUrg = rosNode.SubscriberAsync<RosSharp.sensor_msgs.LaserScan>("/scan").Result;
//subUrg = rosNode.SubscriberAsync<RosSharp.sensor_msgs.LaserScan>("/last").Result;
subUrg = rosNode.SubscriberAsync<RosSharp.sensor_msgs.LaserScan>("/torosif/scan").Result;
subClock = rosNode.SubscriberAsync<RosSharp.rosgraph_msgs.Clock>("/clock").Result;
// Publisher生成
pubRE = rosNode.PublisherAsync<RosSharp.geometry_msgs.Twist>("/vehiclerunner/re").Result;
pubPlot = rosNode.PublisherAsync<RosSharp.geometry_msgs.Twist>("/vehiclerunner/replot").Result;
pubCompus = rosNode.PublisherAsync<RosSharp.geometry_msgs.Twist>("/vehiclerunner/compus").Result;
pubGPS = rosNode.PublisherAsync<RosSharp.geometry_msgs.Twist>("/vehiclerunner/gpsplot").Result;
//pubUrg = rosNode.PublisherAsync<RosSharp.sensor_msgs.LaserScan>("/scanVR").Result;
//pubClock = rosNode.PublisherAsync<RosSharp.rosgraph_msgs.Clock>("/clock").Result;
pubBenzLED = rosNode.PublisherAsync<RosSharp.std_msgs.String>("/benz/led").Result;
// Subscribe CallBack指定
if (null != subVSlam) subVSlam.Subscribe(cbSubScriber_VSlam);
if( null!= subUrg) subUrg.Subscribe(cbSubScriber_URG);
if (null != subRosif_pub) subRosif_pub.Subscribe(cbSubScriber_rofif_pub);
if (null != subClock) subClock.Subscribe(cbSubScriber_Clock);
}
catch (Exception ex)
{
Console.WriteLine("ROS Connect Error!");
Console.WriteLine(ex.Message);
}
}
