public void Run()
{
metadata_pub = rosNodeHandle.advertise <nm.MapMetaData>("map_metadata", 1, true);
if (metadata_pub != null)
{
metadata_pub.publish(mete_data_message_);
}
else
{
throw new Exception("map_metadata话题发布失败！");
}
map_pub = rosNodeHandle.advertise <nm.OccupancyGrid>("map", 1, true);
if (map_pub != null)
{
map_pub.publish(map_resp_.map);
}
else
{
throw new Exception("map话题发布失败！");
}
server = rosNodeHandle.advertiseService <TwoInts.Request, TwoInts.Response>("/add_two_ints", addition);
//可能rosnet 框架问题，服务不能够发布，后面解决
service = rosNodeHandle.advertiseService <nm.GetMap.Request, nm.GetMap.Response>("/static_map", mapCallback);
}
public void init()
{
NodeHandle nh = new NodeHandle();
if (OnInitProcess != null)
{
OnInitProcess(60);
}
imagel = nh.advertise <Messages.sensor_msgs.Image>("/imagel", 1, true);
imager = nh.advertise <Messages.sensor_msgs.Image>("/imager", 1, true);
chaminfo = nh.advertise <Messages.cham_msgs.ChamInfo>("/chaminfo", 1, true);
//test = nh.advertise<Messages.std_msgs.String>("/Top2Slam_name", 1, true);
if (OnInitProcess != null)
{
OnInitProcess(80);
}
//pose_sub = nh.subscribe<Messages.geometry_msgs.Point>("/Slam2Top_pos", 1, pos_subCallback, true);
isConnect = true;
if (OnInitProcess != null)
{
OnInitProcess(100);
}
}
public ActionClient(string name, NodeHandle parentNodeHandle, int queueSize = 50)
{
this.Name = name;
this.QueueSize = queueSize;
this.nodeHandle = new NodeHandle(parentNodeHandle, name);
this.statusReceived = false;
this.goalHandles = new Dictionary <string, ClientGoalHandle <TGoal, TResult, TFeedback> >();
this.goalSubscriberCount = new Dictionary <string, int>();
this.cancelSubscriberCount = new Dictionary <string, int>();
statusSubscriber = nodeHandle.subscribe <GoalStatusArray>("status", queueSize, OnStatusMessage);
feedbackSubscriber = nodeHandle.subscribe <FeedbackActionMessage <TFeedback> >("feedback", queueSize, OnFeedbackMessage);
resultSubscriber = nodeHandle.subscribe <ResultActionMessage <TResult> >("result", queueSize, OnResultMessage);
GoalPublisher = nodeHandle.advertise <GoalActionMessage <TGoal> >(
"goal",
queueSize,
OnGoalConnectCallback,
OnGoalDisconnectCallback
);
CancelPublisher = nodeHandle.advertise <GoalID>(
"cancel",
queueSize,
OnCancelConnectCallback,
OnCancelDisconnectCallback
);
}
void OnRosInit()
{
nh = ROS.GlobalNodeHandle;
// nh = new NodeHandle ( "~" );
pathSrv = nh.advertiseService <GetPath.Request, GetPath.Response> ("quad_rotor/get_path", GetPathService);
// setOrientSrv = nh.advertiseService<Messages.std_srvs.Empty.Request> ("quad_rotor/reset_orientation", TriggerReset)
// enableMotorSrv = nh.advertiseService<EnableMotors.Request, EnableMotors.Response> ( "enable_motors", OnEnableMotors );
nh.setParam("control_mode", "wrench"); // for now force twist mode
twistSub = nh.subscribe <Twist> ("quad_rotor/cmd_vel", 10, TwistCallback);
wrenchSub = nh.subscribe <Wrench> ("quad_rotor/cmd_force", 10, WrenchCallback);
posePub = nh.advertise <PoseStamped> ("quad_rotor/pose", 10, false);
imuPub = nh.advertise <Imu> ("quad_rotor/imu", 10, false);
// imgPub = nh.advertise<Image> ( "quad_rotor/image", 10, false );
pubThread = new Thread(PublishAll);
pubThread.Start();
gravitySrv = nh.advertiseService <SetBool.Request, SetBool.Response> ("quad_rotor/gravity", GravityService);
constrainForceXSrv = nh.advertiseService <SetBool.Request, SetBool.Response> ("quad_rotor/x_force_constrained", ConstrainForceX);
constrainForceYSrv = nh.advertiseService <SetBool.Request, SetBool.Response> ("quad_rotor/y_force_constrained", ConstrainForceY);
constrainForceZSrv = nh.advertiseService <SetBool.Request, SetBool.Response> ("quad_rotor/z_force_constrained", ConstrainForceZ);
constrainTorqueXSrv = nh.advertiseService <SetBool.Request, SetBool.Response> ("quad_rotor/x_torque_constrained", ConstrainTorqueX);
constrainTorqueYSrv = nh.advertiseService <SetBool.Request, SetBool.Response> ("quad_rotor/y_torque_constrained", ConstrainTorqueY);
constrainTorqueZSrv = nh.advertiseService <SetBool.Request, SetBool.Response> ("quad_rotor/z_torque_constrained", ConstrainTorqueZ);
triggerResetSrv = nh.advertiseService <SetBool.Request, SetBool.Response> ("quad_rotor/reset_orientation", TriggerReset);
// triggerResetSrv = nh.advertiseService<Empty.Request, Empty.Response> ( "quad_rotor/reset_orientation", TriggerReset );
setPoseSrv = nh.advertiseService <SetPose.Request, SetPose.Response> ("quad_rotor/set_pose", SetPoseService);
clearPathSrv = nh.advertiseService <SetBool.Request, SetBool.Response> ("quad_rotor/clear_path", ClearPathService);
setPathSrv = nh.advertiseService <SetPath.Request, SetPath.Response> ("quad_rotor/set_path", SetPathService);
}
public MainWindow()
{
InitializeComponent();
ROS.Init(new string[0], "wpf_talker");
nh = new NodeHandle();
pub = nh.advertise<Messages.std_msgs.String>("/chatter", 1, false);
pubthread = new Thread(() =>
{
int i = 0;
Messages.std_msgs.String msg;
while (ROS.ok && !closing)
{
msg = new Messages.std_msgs.String("foo " + (i++));
pub.publish(msg);
Dispatcher.Invoke(new Action(() =>
{
l.Content = "Sending: " + msg.data;
}),new TimeSpan(0,0,1));
Thread.Sleep(100);
}
});
pubthread.Start();
}
public KeyControl(DrawArea owner)
{
this.owner = owner;
nh = new NodeHandle();
pub = nh.advertise <Messages.geometry_msgs.Twist>("/cmd_vel", 1, false);
}
public MainWindow()
{
InitializeComponent();
string[] sParam = new string[1];
//sParam[0] = "__hostname:=192.168.0.8";
sParam[0] = "__master:=http://192.168.1.108:11311";
ROS.Init(sParam, "wpf_talker");
nh = new NodeHandle();
pub = nh.advertise <Messages.std_msgs.String>("/chatter", 1, true);
pubthread = new Thread(() =>
{
int i = 0;
Messages.std_msgs.String msg;
while (ROS.ok && !closing)
{
msg = new Messages.std_msgs.String("foo " + (i++));
pub.publish(msg);
Dispatcher.Invoke(new Action(() =>
{
l.Content = "Sending: " + msg.data;
}), new TimeSpan(0, 0, 1));
Thread.Sleep(100);
}
});
pubthread.Start();
}
public MainWindow()
{
InitializeComponent();
ROS.Init(new string[0], "simplePublisher");
nh = new NodeHandle();
pub = nh.advertise <Messages.tf.tfMessage>("/tf_test", 1000, true);
new Thread(() =>
{
int i = 0;
while (ROS.ok)
{
Messages.tf.tfMessage msg = new Messages.tf.tfMessage();
msg.transforms = new Messages.geometry_msgs.TransformStamped[1];
msg.transforms[0] = new Messages.geometry_msgs.TransformStamped();
msg.transforms[0].header.seq = (uint)i++;
pub.publish(msg);
Thread.Sleep(100);
Dispatcher.Invoke(new Action(() =>
{
l.Content = "Sending: " + msg.transforms[0].header.seq;
}));
}
}).Start();
}
/// <summary>
/// Initializes ROS
/// </summary>
public void InitROS()
{
if (!ROS.initialized)
{
if (string.IsNullOrEmpty(rosUri))
{
UnityEngine.Debug.LogError("ROS Master URI is empty. This must be populated with the Master address of your target roscore instance.");
return;
}
ROS.Init(new string[] { "__master:=" + rosUri }, primaryNodeName);
NodeHandle nodeHandle = new NodeHandle();
// Setup publishers
pubVector3 = nodeHandle.advertise <Messages.geometry_msgs.Vector3>(pubVector3Topic, 1, false);
// Setup subscribers
subVector3 = nodeHandle.subscribe <Messages.geometry_msgs.Vector3>(pubVector3Topic, 1, OnReceiveVector3);
subCustomNode = nodeHandle.subscribe <Messages.stanford_msgs.ExampleCustom>(subCustomNodeTopic, 1, OnReceiveCustomNodeData);
}
else
{
Debug.LogError("ROS can only be initialized once per application launch. To re-initialize ROS you must close and re-launch the application.");
}
}
void Start()
{
interactableItem = GetComponent <InteractableItem>();
nh = rosmaster.getNodeHandle();
pub = nh.advertise <Messages.ihmc_msgs.HandTrajectoryRosMessage>("/ihmc_ros/valkyrie/control/hand_trajectory", 10);
}
public MainWindow()
{
InitializeComponent();
ROS.Init(new string[0], "wpf_talker");
nh = new NodeHandle();
pub = nh.advertise <Messages.std_msgs.String>("/chatter", 1, false);
pubthread = new Thread(() =>
{
int i = 0;
Messages.std_msgs.String msg;
while (ROS.ok && !closing)
{
msg = new Messages.std_msgs.String("foo " + (i++));
pub.publish(msg);
Dispatcher.Invoke(new Action(() =>
{
l.Content = "Sending: " + msg.data;
}), new TimeSpan(0, 0, 1));
Thread.Sleep(100);
}
});
pubthread.Start();
}
public void startListening(NodeHandle nh)
{
updater = new DispatcherTimer() { Interval = new TimeSpan(0, 0, 0, 0, 100) };
updater.Tick += Link;
updater.Start();
sub = nh.subscribe<m.Int32>("/camera1/tilt_info", 1, callback);
pub = nh.advertise<m.Int32>("/camera1/tilt", 1);
}
void doConnect()
{
//开启键盘控制节点
ROS.Init(new string[0], "tele_key");
nh = new NodeHandle();
pub = nh.advertise <Messages.geometry_msgs.Twist>("/cmd_vel", 1, false);
}
Publisher <M> queue_advertise <M> (string topic, uint queue_size, SubscriberStatusCallback connect_cb, SubscriberStatusCallback disconnect_cb, CallbackQueueInterface queue) where M : IRosMessage, new()
{
AdvertiseOptions <M> ops = new AdvertiseOptions <M> (topic, (int)queue_size, connect_cb, disconnect_cb);
// ops.tracked_object = ros::VoidPtr();
ops.latch = false;
ops.callback_queue = queue;
return(nodeHandle.advertise(ops));
}
// Use this for initialization
void Start()
{
rosmaster.StartROS("http://mjolnir.nrv:11311", "kraken-soft.nrv", "UnityProject1");
nh = rosmaster.getNodeHandle();
pub = nh.advertise <Messages.std_msgs.String>(topic_name, 10);
// msg = new Messages.std_msgs.String();
// msg.data = "HELLO!";
// msg.Serialized = null;
}
public TouchControl(DrawArea owner)
{
this.owner = owner;
nh = new NodeHandle();
pub = nh.advertise <Messages.geometry_msgs.Twist>("/cmd_vel", 1, false);
engineCtlForm = new WpfApplication1.MainWindow(this);
directCtrlForm = new WpfApplication1.Window1(this);
}
// Use this for initialization
void Start()
{
setROS_HOST();
string[] dummy = { "" };
ROS.Init(dummy, "unity"); //TODO:set your own node name
n = new NodeHandle();
tf = new Transformer(false /* do Interprate ?*/); /* for TF lookup*/
tf_pub = n.advertise <Messages.tf.tfMessage>("/tf", 100); /* for TF broad cast */
Debug.LogError("ROS my HostIP :" + ROS.ROS_IP + "\nMASTER_URI" + ROS.ROS_MASTER_URI);
}
public void startListening(NodeHandle nh)
{
updater = new DispatcherTimer()
{
Interval = new TimeSpan(0, 0, 0, 0, 100)
};
updater.Tick += Link;
updater.Start();
sub = nh.subscribe <m.Int32>("/camera1/tilt_info", 1, callback);
pub = nh.advertise <m.Int32>("/camera1/tilt", 1);
}
/// <summary>
/// publish goal
/// </summary>
static void publish()
{
Publisher <m.String> Talker = node.advertise <m.String>("goal", 10);
while (true)//不可用ROS.ok
{
vehicle1.calc();
String strSend = new String(vehicle1.sendmsg);
Talker.publish(strSend);
Thread.Sleep(100);
}
}
// Use this for initialization
void Start()
{
ROS.ROS_MASTER_URI = "http://127.0.0.1:11311/";
string[] args = new string[0];
ROS.Init(args, "Talker");
node = new NodeHandle();
Talker = node.advertise <m.String>("/chatter", 1000);
sub = node.subscribe <Messages.std_msgs.String>("/chatter_to_unity", 10, subCallback);
StartCoroutine(Coro());
}
private void loadThread()
{
while (!closing)
{
if (!ROS.ok)
{
string[] sParam = new string[2];
sParam[0] = "__hostname:=" + GetLocalIP();
//sParam[0] = "__hostname:=192.168.0.30";
sParam[1] = "__master:=http://" + host + ":11311";
ROS.Init(sParam, "Image_Test2");
nh = new NodeHandle();
VoicePlay.EasyPlayer_Init();
vp.host = host;
vp.micro = micro;
vp.EasyPlayer_OpenStream("rtmp://" + host + ":2017/live/test");
pub_camera = nh.advertise <Messages.std_msgs.UInt8MultiArray>("/EOD_camera", 1, true);
pub_control = nh.advertise <Messages.std_msgs.UInt8MultiArray>("/platform_msg", 1, true);
// sub_control = nh.subscribe<Messages.std_msgs.UInt8MultiArray>("/robot_state", 1, subCallback);
//sub_control = nh.subscribe<Messages.std_msgs.UInt8MultiArray>("/platform_msg", 1, subCallback);
publishCameraThread();
publishControlThread();
}
if (this.keypress != null)
{
loadJoy();
}
Thread.Sleep(100);
}
}
public MainForm()
{
InitializeComponent();
MouseWheel += new MouseEventHandler(MainForm_MouseWheel);
Ros_CSharp.ROS.Init(new string[0], "drawTool_TestNode");
while (!Ros_CSharp.ROS.ok)
{
System.Threading.Thread.Sleep(100);
}
ROSNodeHandle = new NodeHandle();
goalCancelPub = ROSNodeHandle.advertise <am.GoalID>("/move_base/cancel", 1);
}
public static void start()
{
ROS.Init(new string[0], "rosout");
nh = new NodeHandle();
sub = nh.subscribe <Messages.rosgraph_msgs.Log>("/rosout", 10, rosoutCallback);
pub = nh.advertise <Messages.rosgraph_msgs.Log>("/rosout_agg", 1000, true);
new Thread(() =>
{
while (!ROS.ok)
{
Thread.Sleep(10);
}
}).Start();
}
public static void start()
{
ROS.Init(new string[0], "rosout");
nh = new NodeHandle();
sub = nh.subscribe<Messages.rosgraph_msgs.Log>("/rosout", 10, rosoutCallback);
pub = nh.advertise<Messages.rosgraph_msgs.Log>("/rosout_agg", 1000, true);
new Thread(() =>
{
while (!ROS.ok)
{
Thread.Sleep(10);
}
}).Start();
}
private static void Main(string[] args)
{
ROS.Init(args, "Talker");
NodeHandle node = new NodeHandle();
Publisher<m.String> Talker = node.advertise<m.String>("/chatter", 1);
int count = 0;
while (ROS.ok)
{
ROS.Info("Publishing a chatter message: \"Blah blah blah " + count + "\"");
String pow = new String("Blah blah blah " + (count++));
Talker.publish(pow);
Thread.Sleep(1000);
}
ROS.shutdown();
ROS.waitForShutdown();
}
private static void Main(string[] args)
{
ROS.Init(args, "Talker");
NodeHandle node = new NodeHandle();
Publisher <m.String> Talker = node.advertise <m.String>("/Chatter", 1);
//Subscriber<m.String> Subscriber = node.subscribe<m.String>("/Chatter", 1, chatterCallback);
int count = 0;
while (ROS.ok)
{
ROS.Info("Publishing a chatter message: \"Blah blah blah " + count + "\"");
String pow = new String("Blah blah blah " + (count++));
Talker.publish(pow);
Thread.Sleep(1000);
}
ROS.shutdown();
}
public WindowScraper(string topic, Window w, TimeSpan period)
{
w.Dispatcher.Invoke(new Action(() =>
{
w.LocationChanged += (s, a) =>
{
window_left = (int)w.Left;
window_top = (int)w.Top;
};
}));
hwnd = new WindowInteropHelper(w).Handle;
period_ms = (int)Math.Floor(period.TotalMilliseconds);
timer = new Timer(callback, null, Timeout.Infinite, period_ms);
nh = new NodeHandle();
if (!topic.EndsWith("/compressed"))
{
Console.WriteLine("APPENDING /compressed TO TOPIC NAME TO MAKE IMAGE TRANSPORT HAPPY");
topic += "/compressed";
}
pub = nh.advertise <CompressedImage>(topic, 10);
}
public WindowScraper(string topic, Window w, TimeSpan period)
{
w.Dispatcher.Invoke(new Action(() =>
{
w.LocationChanged += (s, a) =>
{
window_left = (int) w.Left;
window_top = (int) w.Top;
};
}));
hwnd = new WindowInteropHelper(w).Handle;
period_ms = (int) Math.Floor(period.TotalMilliseconds);
timer = new Timer(callback, null, Timeout.Infinite, period_ms);
nh = new NodeHandle();
if (!topic.EndsWith("/compressed"))
{
Console.WriteLine("APPENDING /compressed TO TOPIC NAME TO MAKE IMAGE TRANSPORT HAPPY");
topic += "/compressed";
}
pub = nh.advertise<CompressedImage>(topic, 10);
}
private static void Main(string[] args)
{
Trace.Listeners.Add(new TextWriterTraceListener(Console.Out));
ROS.Init(args, "Talker");
var spinner = new SingleThreadSpinner();
NodeHandle node = new NodeHandle();
Publisher <std_msgs.String> Talker = node.advertise <std_msgs.String>("/chatter", 1);
int count = 0;
while (ROS.ok && !Console.KeyAvailable)
{
Console.WriteLine("publishing message");
ROS.Info()("Publishing a chatter message: \"Blah blah blah " + count + "\"");
String pow = new String("Blah blah blah " + (count++));
Talker.publish(pow);
spinner.SpinOnce();
Thread.Sleep(1000);
}
ROS.shutdown();
}
private static void Main(string[] args)
{
ROS.Init(args, "Talker");
NodeHandle node = new NodeHandle();
Publisher <m.String> Talker = node.advertise <m.String>("/Chatter", 1);
Subscriber <m.String> Subscriber = node.subscribe <m.String>("/Chatter", 1, chatterCallback);
int count = 0;
Console.WriteLine("PRESS ENTER TO QUIT!");
new Thread(() =>
{
while (ROS.ok)
{
ROS.Info("Publishing a chatter message: \"Blah blah blah " + count + "\"");
String pow = new String("Blah blah blah " + (count++));
Talker.publish(pow);
Thread.Sleep(1000);
}
}).Start();
Console.ReadLine();
ROS.shutdown();
}
public PassiveControl(DrawArea owner)
{
this.owner = owner;
nh = new NodeHandle();
pub = nh.advertise <Messages.geometry_msgs.Twist>("/cmd_vel", 1, false);
timer = new Timer((obj) =>
{
double linear = 0.0, angular = 0.0;
if (block_up && !block_down)
{
linear = -0.15;
}
else if (!block_up && block_down)
{
linear = 0.15;
}
else if (block_up && block_down)
{
angular = 0.9;
}
pubCmdMsg(linear, angular, speed);
}, null, 1000, 500);
}
// Use this for initialization
void Start()
{
NodeHandle nh = rosmaster.getNodeHandle();
tfPub = nh.advertise <Messages.tf.tfMessage>("/tf", 100);
}
private void Window_Loaded(object sender, RoutedEventArgs e)
{
controllerUpdater = new DispatcherTimer() { Interval = new TimeSpan(0, 0, 0, 0, 10) };
controllerUpdater.Tick += Link;
controllerUpdater.Start();
new Thread(() =>
{
// ROS stuff
ROS.ROS_MASTER_URI = "http://10.0.3.5:11311";
ROS.Init(new string[0], "The_Cam_Tester_" + System.Environment.MachineName.Replace("-", "__"));
nh = new NodeHandle();
tilt_pub = new Publisher<m.Int32>[4];
recalPub0 = nh.advertise<Messages.rock_publisher.recalibrateMsg>("/camera0/recalibrate", 4);
recalPub1 = nh.advertise<Messages.rock_publisher.recalibrateMsg>("/camera1/recalibrate", 4);
recalPub2 = nh.advertise<Messages.rock_publisher.recalibrateMsg>("/camera2/recalibrate", 4);
recalPub3 = nh.advertise<Messages.rock_publisher.recalibrateMsg>("/camera3/recalibrate", 4);
for (int i = 0; i < 4; i++)
{
tilt_pub[i] = nh.advertise<m.Int32>("camera" + i + "/tilt", 1);
}
Dispatcher.Invoke(new Action(() =>
{
mainCameras = new TabItem[] { MainCamera1, MainCamera2, MainCamera3, MainCamera4 };
subCameras = new TabItem[] { SubCamera1, SubCamera2, SubCamera3, SubCamera4 };
mainImages = new ROS_ImageWPF.CompressedImageControl[] { camImage0, camImage1, camImage2, camImage3 };
subImages = new ROS_ImageWPF.SlaveImage[] { camImageSlave0, camImageSlave1, camImageSlave2, camImageSlave3 };
for (int i = 0; i < mainCameras.Length; i++)
{
mainImages[i].AddSlave(subImages[i]);
}
subCameras[1].Focus();
// instantiating some global helpers
detectors = new DetectionHelper[4];
for (int i = 0; i < 4; ++i)
{
detectors[i] = new DetectionHelper(nh, i, this);
}
}));
while (ROS.ok)
{
Dispatcher.Invoke(new Action(() =>
{
for (int i = 0; i < 4; ++i)
{
detectors[i].churnAndBurn();
}
}));
Thread.Sleep(100);
}
}).Start();
}
public void startListening(NodeHandle node)
{
sub = node.subscribe <m.Bool>("/estopState", 1000, callbackEStop);
pub = node.advertise <m.Bool>("/setEstop", 1000);
}
private void Window_Loaded(object sender, RoutedEventArgs e)
{
controllerUpdater = new DispatcherTimer() { Interval = new TimeSpan(0, 0, 0, 0, 10) };
controllerUpdater.Tick += Link;
controllerUpdater.Start();
new Thread(() =>
{
XmlRpcUtil.ShowOutputFromXmlRpcPInvoke();
// ROS stuff
ROS.Init(new string[0], "The_UI_" + System.Environment.MachineName.Replace("-", "__"));
nh = new NodeHandle();
Dispatcher.Invoke(new Action(() =>
{
battvolt.startListening(nh);
EStop.startListening(nh);
MotorGraph.startListening(nh);
rosout_control.startListening(nh);
spinningstuff.startListening(nh, "/imu/data");
}));
velPub = nh.advertise<gm.Twist>("/cmd_vel", 1);
multiplexPub = nh.advertise<m.Byte>("/cam_select", 1);
armPub = nh.advertise<am.ArmMovement>("/arm/movement", 1);
ArmON = nh.advertise<m.Bool>("arm/on", 1);
mast_pub = nh.advertise<m.Bool>("raise_camera_mast", 1);
tilt_pub = new Publisher<m.Int32>[4];
for (int i = 0; i < 4; i++)
{
tilt_pub[i] = nh.advertise<m.Int32>("camera" + i + "/tilt", 1);
}
Dispatcher.Invoke(new Action(() =>
{
mainCameras = new TabItem[] { MainCamera1, MainCamera2, MainCamera3, MainCamera4 };
subCameras = new TabItem[] { SubCamera1, SubCamera2, SubCamera3, SubCamera4 };
mainImages = new ROS_ImageWPF.CompressedImageControl[] { camImage0, camImage1, camImage2, camImage3 };
subImages = new ROS_ImageWPF.SlaveImage[] { camImageSlave0, camImageSlave1, camImageSlave2, camImageSlave3 };
for (int i = 0; i < mainCameras.Length; i++)
{
mainImages[i].AddSlave(subImages[i]);
}
subCameras[1].Focus();
adr(false);
// instantiating some global helpers
/*detectors = new DetectionHelper[4];
for (int i = 0; i < 4; ++i)
{
detectors[i] = new DetectionHelper(nh, i, this);
}*/
}));
#if !INSTANT_DETECTION_DEATH
while (ROS.ok)
{
Dispatcher.Invoke(new Action(() =>
{
for (int i = 0; i < 4; ++i)
{
detectors[i].churnAndBurn();
}
}));
Thread.Sleep(100);
}
#endif
}).Start();
}
public void startListening(NodeHandle node)
{
sub = node.subscribe<m.Bool>("/estopState", 1000, callbackEStop);
pub = node.advertise<m.Bool>("/setEstop", 1000);
}
// Use this for initialization
void Start()
{
print("Starting...");
TfTreeManager.Instance.AddListener(vis =>
{
//Debug.LogWarning("LaserMeshView has a tfvisualizer now!");
tfvisualizer = vis;
});
if (is_static)
{
theCloud = new mesh_pcloud[1];
GameObject theCloudObject = new GameObject("pointCloudContainer");
theCloudObject.transform.parent = gameObject.transform;
theCloudObject.AddComponent <mesh_pcloud>();
theCloud[0] = theCloudObject.GetComponent <mesh_pcloud>();
registerMeshPclouds();
print("Starting to load static cloud from " + Mesh_File.name);
string[] pointLocations = Mesh_File.text.Split('\n');
string Version = pointLocations[1].Split(' ')[1];
string[] Fields = pointLocations[2].Split(' ');
string[] Sizes = pointLocations[3].Split(' ');
string[] Type = pointLocations[4].Split(' ');
string[] Count = pointLocations[5].Split(' ');
int Width = Int32.Parse(pointLocations[6].Split(' ')[1]);
int Height = Int32.Parse(pointLocations[7].Split(' ')[1]);
string[] Viewpoint = pointLocations[8].Split(' ');
int Points = Int32.Parse(pointLocations[9].Split(' ')[1]);
string Data = pointLocations[10].Split(' ')[1];
if (Data != "ascii")
{
print("Only ascii pcd format is supported...");
print(Data);
return;
}
//theCloud.vertexBuffers[theCloud.whichBuffer] = new Vector3[10 * vertexCount[pointShape]];//[(pointLocations.Length / 6) * vertexCount[pointShape]];
//theCloud.colorBuffers[theCloud.whichBuffer] = new Color[10 * vertexCount[pointShape]];//[(pointLocations.Length / 6) * vertexCount[pointShape]];
//theCloud.recalculateTriangles(10, indices[pointShape], vertexCount[pointShape]);
theCloud[0].vertexBuffers[theCloud[0].whichBuffer] = new Vector3[Points * vertexCount[pointShape]];
theCloud[0].colorBuffers[theCloud[0].whichBuffer] = new Color[Points * vertexCount[pointShape]];
theCloud[0].recalculateTriangles(Points, indices[pointShape], vertexCount[pointShape]);
for (int i = 11; i < Points + 11; i++)
{
try
{
string[] thisPoint = pointLocations[i].Split(' ');
float x = float.Parse(thisPoint[0]);
float y = float.Parse(thisPoint[1]);
float z = float.Parse(thisPoint[2]);
Color c;
if (thisPoint[3][0] != 'n')
{
byte[] color = System.BitConverter.GetBytes(float.Parse(thisPoint[3]));
c = new Color(color[2] * 0.00392156862F, color[1] * 0.00392156862F, color[0] * 0.00392156862F, 1);
}
else
{
c = new Color(0, 0, 0, 1);
}
addPoint(x, y, z, c);
}
catch (Exception e)
{
print(e);
print(pointLocations[i]);
}
}
print("number of points loaded: " + pointNum);
theCloud[0].whichBuffer = (theCloud[0].whichBuffer + 1) % 2;
theCloud[0].updateVertexData = true;
theCloud[0].updateColorData = true;
theCloud[0].updateTriangleData = true;
}
else
{
ROS.ROS_HOSTNAME = "asgard";
ROS.ROS_MASTER_URI = "http://thor:11311";
ROS.Init(new String[0], "mesh_pcloud_renderer");
nh = new NodeHandle();
//nh= rosmaster.getNodeHandle();
print("Subscribing");
theCloud = new mesh_pcloud[accumulate_data && max_acc_frames > 0 ? max_acc_frames : 1];
for (int i = 0; i < theCloud.Length; i++)
{
GameObject theCloudObject = new GameObject("pointCloudContainer" + i);
theCloudObject.transform.parent = gameObject.transform;
theCloudObject.AddComponent <mesh_pcloud>();
theCloud[i] = theCloudObject.GetComponent <mesh_pcloud>();
}
sub = nh.subscribe <Messages.sensor_msgs.PointCloud2>(topic_name, 2, subCallback);
unity_tf_msg = new Messages.geometry_msgs.PolygonStamped();
unity_tf_msg.header = new Messages.std_msgs.Header();
unity_tf_msg.polygon = new Messages.geometry_msgs.Polygon();
unity_tf_msg.header.frame_id = (is_static ? Mesh_File.name : topic_name);
unity_tf_msg.polygon.points = new Messages.geometry_msgs.Point32[3];
unity_tf_pub = nh.advertise <Messages.geometry_msgs.PolygonStamped>("/unity_tf", 10);
ThreadStart pub_thread_start = new ThreadStart(UnityTFPublisher);
pub_thread = new Thread(pub_thread_start);
pub_thread.Start();
}
print("Started");
//theCloud.Initialize();
}
// Use this for initialization
void Start()
{
interactableItem = GetComponent <InteractableItem>();
nh = rosmaster.getNodeHandle();
tfPub = nh.advertise <Messages.tf.tfMessage>("/tf", 10);
}
