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