protected override void Initialize()
{
quad = new Quad[6];
quad[0] = new Quad(Vector3.Backward, Vector3.Backward, Vector3.Up, 2, 2);
quad[1] = new Quad(Vector3.Left, Vector3.Left, Vector3.Up, 2, 2);
quad[2] = new Quad(Vector3.Right, Vector3.Right, Vector3.Up, 2, 2);
quad[3] = new Quad(Vector3.Forward, Vector3.Forward, Vector3.Up, 2, 2);
quad[4] = new Quad(Vector3.Down, Vector3.Down, Vector3.Right, 2, 2);
quad[5] = new Quad(Vector3.Up, Vector3.Up, Vector3.Left, 2, 2);
nh = new NodeHandle();
imgSub = nh.subscribe<Messages.sensor_msgs.Image>("/camera/rgb/image_rect_color", 1, (img) =>
{
if (padlock.WaitOne(10))
{
if (next_texture == null)
{
next_texture = new Texture2D(GraphicsDevice, (int) img.width, (int) img.height);
}
util.UpdateImage(GraphicsDevice, img.data, new Size((int)img.width, (int)img.height), ref next_texture, img.encoding.data);
padlock.ReleaseMutex();
}
});
base.Initialize();
}
private static void Main(string[] args)
{
ROS.Init(args, "Listener");
NodeHandle node = new NodeHandle();
Subscriber<m.String> Subscriber = node.subscribe<m.String>("/chatter", 1, chatterCallback);
ROS.waitForShutdown();
}
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);
}
public MainWindow()
{
InitializeComponent();
ROS.Init(new string[0], "wpf_listener");
nh = new NodeHandle();
sub = nh.subscribe<Messages.std_msgs.String>("/chatter", 10, subCallback);
}
public void startListening(NodeHandle node)
{
tilt_min = -600;
tilt_max = 600;
pan_min = 0;
pan_max = 5100;
sub = node.subscribe<am.ArmStatus>("/arm/status", 1000, callbackMonitor);
//pub = node.advertise<am.ArmMovement>("/arm/movement", 1000);
}
public SimTime()
{
new Thread(() =>
{
while (!ROS.isStarted() && !ROS.shutting_down)
{
Thread.Sleep(100);
}
nh = new NodeHandle();
if (!ROS.shutting_down)
{
simTimeSubscriber = nh.subscribe<Clock>("/clock", 1, SimTimeCallback);
}
}).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();
}
public SimTime()
{
new Thread(() =>
{
while (!ROS.isStarted() && !ROS.shutting_down)
{
Thread.Sleep(100);
}
Thread.Sleep(1000);
if (!ROS.shutting_down)
{
nh = new NodeHandle();
simTimeSubscriber = nh.subscribe<Messages.rosgraph_msgs.Clock>("/clock", 1, SimTimeCallback);
}
ROS.waitForShutdown();
simTimeSubscriber.shutdown();
}).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);
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();
}
private void SetupTopic(string TopicName)
{
if (Process.GetCurrentProcess().ProcessName == "devenv")
{
return;
}
if (imagehandle == null)
{
imagehandle = new NodeHandle();
}
if (imgsub != null && imgsub.topic != TopicName)
{
imgsub.shutdown();
imgsub = null;
}
Console.WriteLine("IMG TOPIC " + TopicName);
if (imgsub != null)
{
return;
}
imgsub = imagehandle.subscribe(new SubscribeOptions <sm.CompressedImage>(TopicName, 1, i => Dispatcher.Invoke(new Action(() =>
{
UpdateImage(ref i.data);
foreach (SlaveImage si in _slaves)
{
si.UpdateImage(ref i.data);
}
if (ImageReceivedEvent != null)
{
ImageReceivedEvent(this);
}
})))
{
allow_concurrent_callbacks = true
});
}
public ImuSubscriberHelper (NodeHandle nh, string topic, Imu imu)
{
imu_ = imu;
imu_sub_ = nh.subscribe<Imu> ( topic, 1, imuCallback );
}
public PoseSubscriberHelper (NodeHandle nh, string topic)
{
pose_sub_ = nh.subscribe<PoseStamped> ( topic, 1, poseCallback );
}
/// <summary>
/// 订阅更新话题
/// </summary>
/// <param name="topic"></param>
public void SubscribeToPath(string topic)
{
lock (this)
{
if (pathhandle == null)
{
pathhandle = new NodeHandle();
}
if (pathSub != null && pathSub.topic != topic)
{
pathSub.shutdown();
pathSub = null;
}
if (pathSub != null)
{
return;
}
if (tfer == null)
{
tfer = new Transformer(false);
}
Console.WriteLine("Subscribing to path at:= " + topic);
pathSub = pathhandle.subscribe <nm.Path>(topic, 1, i => owner.BeginInvoke(new Action(() =>
{
//if (i.polygon.points.Length == 0) return;
//if (!tfer.waitForTransform("/map", "/base", i.header.stamp, new m.Duration(new Messages.TimeData(1, 0)), null))
// return ;
//tfer.transformVector("/map",)
//emTransform ret = new emTransform();
//if (tfer.lookupTransform("/map", "/base", i.header.stamp, out ret))
//{
pointArray.Clear();
int length = i.poses.Length;
for (int index = 0; index < i.poses.Length; index++)
{
Point worldPoint = owner.Map2World(new PointF((float)i.poses[index].pose.position.x, (float)i.poses[index].pose.position.y));
pointArray.Add(worldPoint);
}
//int length = i.polygon.points.Length;
//for (int index = 0; index < length; index++)
//{
// gm.Point32 p = i.polygon.points[index];
// //Stamped<emVector3> stampVector_in = new Stamped<emVector3>(i.header.stamp, i.header.frame_id, new emVector3(p.x,p.y,p.z));
// //Stamped<emVector3> stamVector_out = new Stamped<emVector3>();
// //tfer.transformVector("map", stampVector_in, ref stamVector_out);
// Point worldPoint = owner.Map2World(new PointF(p.x, p.y));
// pointArray.Add(worldPoint);
//}
//pointArray.Add(pointArray[0]);
if (length > 0)
{
startPoint = (Point)pointArray[0];
endPoint = (Point)pointArray[length - 1];
}
Invalidate();
owner.LastDataUpdateTime = DateTime.Now;
//owner.Invalidate();
//}
})));
}
}
//ROS
private void UserControl_Loaded(object sender, RoutedEventArgs e)
{
new Thread(() =>
{
while (!ROS.isStarted())
{
Thread.Sleep(200);
}
node = new NodeHandle();
//setup up temporary arrays to make setup clean
Slider[][] sliders = new[]
{ new [] { MC1_Brigh_Sl, MC1_Cont_Sl, MC1_Exp_Sl, MC1_Gain_Sl, MC1_Sat_Sl, MC1_WBT_Sl, MC1_Foc_Sl },
new [] { RC_Brigh_Sl, RC_Cont_Sl, RC_Exp_Sl, RC_Gain_Sl, RC_Sat_Sl, RC_WBT_Sl, RC_Foc_Sl },
new [] { MC3_Brigh_Sl, MC3_Cont_Sl, MC3_Exp_Sl, MC3_Gain_Sl, MC3_Sat_Sl, MC3_WBT_Sl, MC3_Foc_Sl },
new [] { MC4_Brigh_Sl, MC4_Cont_Sl, MC4_Exp_Sl, MC4_Gain_Sl, MC4_Sat_Sl, MC4_WBT_Sl, MC4_Foc_Sl } };
Label[][] labels = new[]
{ new[] { MC1_Brigh_Lvl, MC1_Cont_Lvl, MC1_Exp_Lvl, MC1_Gain_Lvl, MC1_Sat_Lvl, MC1_WBT_Lvl, MC1_Foc_Lvl },
new[] { RC_Brigh_Lvl, RC_Cont_Lvl, RC_Exp_Lvl, RC_Gain_Lvl, RC_Sat_Lvl, RC_WBT_Lvl, RC_Foc_Lvl },
new[] { MC3_Brigh_Lvl, MC3_Cont_Lvl, MC3_Exp_Lvl, MC3_Gain_Lvl, MC3_Sat_Lvl, MC3_WBT_Lvl, MC3_Foc_Lvl },
new[] { MC4_Brigh_Lvl, MC4_Cont_Lvl, MC4_Exp_Lvl, MC4_Gain_Lvl, MC4_Sat_Lvl, MC4_WBT_Lvl, MC4_Foc_Lvl } };
int[] defaults = new int[] { 128, 33, 100, 64, 32, 5315, 16 };
string[] info = new[] { "camera0", "camera1", "camera2", "camera3" };
//end setup
//setup persistent array of slider stuff for storage
SUBS = new[] { new SliderStuff[6], new SliderStuff[6], new SliderStuff[6], new SliderStuff[6] };
initials = new[] { new cm[6], new cm[6], new cm[6], new cm[6] };
pub_exposureauto = new Publisher <m.Int32> [4];
//pub_wbtauto = new Publisher<m.Int32>[4];
pub_focusauto = new Publisher <m.Int32> [4];
////// INITIALIZE THIS CAMS PUB AND SUB
pub = new Publisher <cm> [4];
sub = new Subscriber <cm>[] { node.subscribe <cm>("/camera0/sliders_info", 1, cb0),
node.subscribe <cm>("/camera1/sliders_info", 1, cb1),
node.subscribe <cm>("/camera2/sliders_info", 1, cb2),
node.subscribe <cm>("/camera3/sliders_info", 1, cb3) };
for (int i = 0; i < info.Length; i++)
{
//pub_exposureauto[i] = node.advertise<m.Int32>("camera" + i + "/exposureauto", 1);
//pub_wbtauto[i] = node.advertise<m.Int32>("camera" + i + "/wbtauto", 1);
//pub_focusauto[i] = node.advertise<m.Int32>("camera" + i + "/focusauto", 1);
pub[i] = node.advertise <cm>(info[i] + "/sliders", 1);
for (int j = 0; j < 6; j++)
{
Dispatcher.Invoke(new Action(() =>
{
SUBS[i][j] = new SliderStuff(node, j, info[i], sliders[i][j], labels[i][j]);
SUBS[i][j].setFire(fire);
SUBS[i][j].Value = defaults[j];
if (initials[i][j] != null)
{
SUBS[i][j].callback(initials[i][j]);
}
}));
}
}
}).Start();
}
// 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();
}
/// <summary>
/// subscribe status
/// </summary>
void subscribe()
{
Subscriber <jp> Listener = node.subscribe <jp>("status", 10, subCallback);
}
void OnRosInit()
{
nh = ROS.GlobalNodeHandle;
// NodeHandle privateNH = new NodeHandle("~");
// TODO dynamic reconfig
string control_mode = "";
nh.param <string>("control_mode", ref control_mode, "wrench");
// NodeHandle robot_nh = new NodeHandle ("~");
// TODO factor out
// nh.param<string>("base_link_frame", ref baseLinkFrame, "base_link");
// nh.param<string>("world_frame", ref worldFrame, "world");
// nh.param<string>("base_stabilized_frame", ref baseStabilizedFrame, "base_stabilized");
if (control_mode == "wrench")
{
wrenchPub = nh.advertise <Wrench> ("quad_rotor/cmd_force", 10);
poseSub = nh.subscribe <PoseStamped> ("quad_rotor/pose", 10, PoseCallback);
imuSub = nh.subscribe <Imu> ("quad_rotor/imu", 10, ImuCallback);
velocityPublisher = nh.advertise <Twist> ("quad_rotor/cmd_vel", 10);
}
/* else if (control_mode == "attitude")
* {
* // nh.param<double>("pitch_max", ref sAxes.x.factor, 30.0);
* // nh.param<double>("roll_max", ref sAxes.y.factor, 30.0);
* // nh.param<double>("thrust_max", ref sAxes.thrust.factor, 10.0);
* // nh.param<double>("thrust_offset", ref sAxes.thrust.offset, 10.0);
* attitudePublisher = nh.advertise<AttitudeCommand> ( "command/attitude", 10 );
* yawRatePublisher = nh.advertise<YawRateCommand> ( "command/yawrate", 10 );
* thrustPublisher = nh.advertise<ThrustCommand> ( "command/thrust", 10 );
* }
* else if (control_mode == "velocity" || control_mode == "twist")
* {
* // Gazebo uses Y=forward and Z=up
* nh.param<double>("x_velocity_max", ref xVelocityMax, 2.0);
* nh.param<double>("y_velocity_max", ref zVelocityMax, 2.0);
* nh.param<double>("z_velocity_max", ref yVelocityMax, 2.0);
*
* velocityPublisher = nh.advertise<Twist> ( "command/twist", 10 );
* }
* else if (control_mode == "position")
* {
* // Gazebo uses Y=forward and Z=up
* nh.param<double>("x_velocity_max", ref xVelocityMax, 2.0);
* nh.param<double>("y_velocity_max", ref zVelocityMax, 2.0);
* nh.param<double>("z_velocity_max", ref yVelocityMax, 2.0);
*
* pose.pose.position.x = 0;
* pose.pose.position.y = 0;
* pose.pose.position.z = 0;
* pose.pose.orientation.x = 0;
* pose.pose.orientation.y = 0;
* pose.pose.orientation.z = 0;
* pose.pose.orientation.w = 1;
* }*/
else
{
ROS.Error("Unsupported control mode: " + control_mode);
}
motorEnableService = nh.serviceClient <EnableMotors> ("enable_motors");
resetService = nh.serviceClient <SetBool.Request, SetBool.Response> ("quad_rotor/reset_orientation");
gravityService = nh.serviceClient <SetBool.Request, SetBool.Response> ("quad_rotor/gravity");
// takeoffClient = new TakeoffClient ( nh, "action/takeoff" );
// landingClient = new LandingClient ( nh, "action/landing" );
// poseClient = new PoseClient ( nh, "action/pose" );
init = true;
}
public void startListening(NodeHandle node)
{
sub = node.subscribe <m.Float32>("/Voltage", 1000, callbackVoltMonitor);
}
public void startListening(NodeHandle node)
{
telemsub[0] = node.subscribe <m.String>("/mc1/telemetry", 1, cb0);
telemsub[1] = node.subscribe <m.String>("/mc2/telemetry", 1, cb1);
}
/// <summary>
/// 订阅更新话题
/// </summary>
/// <param name="topic"></param>
public void SubscribeToPostion(string topic)
{
lock (this)
{
if (foothandle == null)
{
foothandle = new NodeHandle();
}
if (footSub != null && footSub.topic != topic)
{
footSub.shutdown();
footSub = null;
}
if (footSub != null)
{
return;
}
if (tfer == null)
{
tfer = new Transformer(false);
}
Console.WriteLine("Subscribing to robot_baselink at:= " + topic);
footSub = foothandle.subscribe <gm.PoseStamped>(topic, 1, i => owner.BeginInvoke(new Action(() =>
{
//if (i.polygon.points.Length == 0) return;
//if (!tfer.waitForTransform("/map", "/base", i.header.stamp, new m.Duration(new Messages.TimeData(1, 0)), null))
// return ;
//tfer.transformVector("/map",)
//emTransform ret = new emTransform();
//if (tfer.lookupTransform("/map", "/base", i.header.stamp, out ret))
//{
pointArray.Clear();
PointF[] vecs = new PointF[3];
emVector3 orien = (new emQuaternion(i.pose.orientation)).getRPY();
double d1 = 0.3, d2 = 0.1;
vecs[0] = new PointF((float)(i.pose.position.x + d1 * Math.Cos(orien.z)), (float)(i.pose.position.y + d1 * Math.Sin(orien.z)));
vecs[1] = new PointF((float)(i.pose.position.x + d2 * Math.Cos(orien.z + Math.PI / 2)), (float)(i.pose.position.y + d2 * Math.Sin(orien.z + Math.PI / 2)));
vecs[2] = new PointF((float)(i.pose.position.x + d2 * Math.Cos(orien.z - Math.PI / 2)), (float)(i.pose.position.y + d2 * Math.Sin(orien.z - Math.PI / 2)));
int length = vecs.Length;
for (int index = 0; index < length; index++)
{
//Stamped<emVector3> stampVector_in = new Stamped<emVector3>(i.header.stamp, i.header.frame_id, new emVector3(p.x,p.y,p.z));
//Stamped<emVector3> stamVector_out = new Stamped<emVector3>();
//tfer.transformVector("map", stampVector_in, ref stamVector_out);
Point worldPoint = owner.Map2World(vecs[index]);
pointArray.Add(worldPoint);
}
pointArray.Add(pointArray[0]);
if (length > 0)
{
startPoint = (Point)pointArray[0];
endPoint = (Point)pointArray[length - 1];
}
Invalidate();
owner.LastDataUpdateTime = DateTime.Now;
//owner.Invalidate();
//}
})));
}
}
public void SubscribeToLaserScan(string topic)
{
if (laserhandle == null)
{
laserhandle = new NodeHandle();
}
while (!laserhandle.ok)
{
System.Threading.Thread.Sleep(10);
}
if (laserSub != null && laserSub.topic != topic)
{
laserSub.shutdown();
laserSub = null;
}
if (laserSub != null)
{
return;
}
if (tfer == null)
{
tfer = new Transformer(false);
}
Console.WriteLine("Subscribing to laser_pointCloud at:= " + topic);
laserSub = laserhandle.subscribe <sm.PointCloud>(topic, 1, i => owner.BeginInvoke(new Action(() =>
{
try
{
pointList.Clear();
for (int j = 0; j < i.points.Length; j++)
{
Point worldPoint = owner.Map2World(new PointF((float)i.points[j].x, (float)i.points[j].y));
pointList.Add(worldPoint);
}
//if (!tfer.waitForTransform("map", "base_laser", i.header.stamp, new m.Duration(new Messages.TimeData(1, 0)), null))
//{
// Console.WriteLine("drawLaserScan:wait for tf timeout");
// return;
//}
//emTransform ret = new emTransform();
//if (tfer.lookupTransform("map", i.header.frame_id, i.header.stamp, out ret))
//{
// pointList.Clear();
// int index = 0;
// double angle = i.angle_min;
// if (i.angle_min >= i.angle_max) return;
// do
// {
// angle += i.angle_increment;
// if (i.ranges[index] >= i.range_min && i.ranges[index] <= i.range_max)
// {
// emVector3 vector = new emVector3(i.ranges[index] * Math.Cos(angle), i.ranges[index] * Math.Sin(angle), 0.0);
// emVector3 origin = new emVector3(0, 0, 0);
// emVector3 output = (ret * vector) - (ret * origin);
// //emVector3 output = ret * vector;
// Point worldPoint = owner.Map2World(new PointF((float)output.x, (float)output.y));
// pointList.Add(worldPoint);
// //pointList.Add(new Point((int)vector.x*,(int)vector.y*10));
// }
// index++;
// } while (angle < i.range_max && index < i.ranges.Length);
//}
Invalidate();
//owner.Invalidate();
owner.LastDataUpdateTime = DateTime.Now;
}
catch (Exception ex)
{
}
})));
}
public void startListening(NodeHandle nh, string TopicName)
{
imusub = nh.subscribe<sm.Imu>(TopicName, 1, imu_callback);
}
public DetectionHelper(NodeHandle node, int cameraNumber, MainWindow w)
{
sub = node.subscribe<imgDataArray>("/camera" + cameraNumber + "/detects", 1000, detectCallback);
this.cameraNumber = cameraNumber;
primary = w.mainImages[cameraNumber];
secondary = w.subImages[cameraNumber];
}
public void startListening(NodeHandle nh, string TopicName)
{
imusub = nh.subscribe <sm.Imu>(TopicName, 1, imu_callback);
}
public void Resubscribe()
{
Desubscribe();
imgSub = imagehandle.subscribe <sm.Image>(Topic, 1, updateImage);
}
public void startListening(NodeHandle node)
{
telemsub[0] = node.subscribe<m.String>("/mc1/telemetry", 1, cb0);
telemsub[1] = node.subscribe<m.String>("/mc2/telemetry", 1, cb1);
}
public void startListening(NodeHandle node)
{
sub = node.subscribe <m.Bool>("/estopState", 1000, callbackEStop);
pub = node.advertise <m.Bool>("/setEstop", 1000);
}
public void startListening(NodeHandle node)
{
sub = node.subscribe<m.Bool>("/estopState", 1000, callbackEStop);
pub = node.advertise<m.Bool>("/setEstop", 1000);
}
void OnRosInit()
{
nh = ROS.GlobalNodeHandle;
// NodeHandle privateNH = new NodeHandle("~");
nh.param <int>("x_axis", ref sAxes.x.axis, 5);
nh.param <int>("y_axis", ref sAxes.y.axis, 4);
nh.param <int>("z_axis", ref sAxes.z.axis, 2);
nh.param <int>("thrust_axis", ref sAxes.thrust.axis, -3);
nh.param <int>("yaw_axis", ref sAxes.yaw.axis, 1);
nh.param <double>("yaw_velocity_max", ref sAxes.yaw.factor, 90.0);
nh.param <int>("slow_button", ref sButtons.slow.button, 4);
nh.param <int>("go_button", ref sButtons.go.button, 1);
nh.param <int>("stop_button", ref sButtons.stop.button, 2);
nh.param <int>("interrupt_button", ref sButtons.interrupt.button, 3);
nh.param <double>("slow_factor", ref slowFactor, 0.2);
// TODO dynamic reconfig
string control_mode = "";
nh.param <string>("control_mode", ref control_mode, "twist");
NodeHandle robot_nh = ROS.GlobalNodeHandle;
// NodeHandle robot_nh = new NodeHandle ();
// TODO factor out
robot_nh.param <string>("base_link_frame", ref baseLinkFrame, "base_link");
robot_nh.param <string>("world_frame", ref worldFrame, "world");
robot_nh.param <string>("base_stabilized_frame", ref baseStabilizedFrame, "base_stabilized");
if (control_mode == "attitude")
{
nh.param <double>("pitch_max", ref sAxes.x.factor, 30.0);
nh.param <double>("roll_max", ref sAxes.y.factor, 30.0);
nh.param <double>("thrust_max", ref sAxes.thrust.factor, 10.0);
nh.param <double>("thrust_offset", ref sAxes.thrust.offset, 10.0);
joySubscriber = nh.subscribe <Joy> ("joy", 1, joyAttitudeCallback);
attitudePublisher = robot_nh.advertise <AttitudeCommand> ("command/attitude", 10);
yawRatePublisher = robot_nh.advertise <YawRateCommand> ("command/yawrate", 10);
thrustPublisher = robot_nh.advertise <ThrustCommand> ("command/thrust", 10);
}
else if (control_mode == "velocity")
{
nh.param <double>("x_velocity_max", ref sAxes.x.factor, 2.0);
nh.param <double>("y_velocity_max", ref sAxes.y.factor, 2.0);
nh.param <double>("z_velocity_max", ref sAxes.z.factor, 2.0);
joySubscriber = nh.subscribe <Joy> ("joy", 1, joyTwistCallback);
velocityPublisher = robot_nh.advertise <TwistStamped> ("command/twist", 10);
}
else if (control_mode == "position")
{
nh.param <double>("x_velocity_max", ref sAxes.x.factor, 2.0);
nh.param <double>("y_velocity_max", ref sAxes.y.factor, 2.0);
nh.param <double>("z_velocity_max", ref sAxes.z.factor, 2.0);
joySubscriber = nh.subscribe <Joy> ("joy", 1, joyPoseCallback);
pose.pose.position.x = 0;
pose.pose.position.y = 0;
pose.pose.position.z = 0;
pose.pose.orientation.x = 0;
pose.pose.orientation.y = 0;
pose.pose.orientation.z = 0;
pose.pose.orientation.w = 1;
}
else
{
ROS.Error("Unsupported control mode: " + control_mode);
}
motorEnableService = robot_nh.serviceClient <EnableMotors> ("enable_motors");
takeoffClient = new TakeoffClient(robot_nh, "action/takeoff");
landingClient = new LandingClient(robot_nh, "action/landing");
poseClient = new PoseClient(robot_nh, "action/pose");
}
void Start()
{
nh = rosmaster.getNodeHandle();
sub = nh.subscribe <Messages.std_msgs.String>(topic_name, 1, callback);
}
public void startListening(NodeHandle nh)
{
sub = nh.subscribe <Messages.rosgraph_msgs.Log>("/rosout_agg", 100, callback);
}
private void InitNodeHandle()
{
nh = new NodeHandle();
nh.subscribe <tfMessage>("/tf", 0, Update);
nh.subscribe <tfMessage>("/tf_static", 0, Update);
}
private void InitNH()
{
nh = ROS.GlobalNodeHandle;
// nh = new NodeHandle();
nh.subscribe <tfMessage>("/tf", 0, Update);
}
public void startListening(NodeHandle node)
{
sub = node.subscribe<m.Float32>("/Voltage", 1000, callbackVoltMonitor);
}
/// <summary>
/// 订阅更新话题
/// </summary>
/// <param name="topic"></param>
public void SubscribeToMap(string topic)
{
lock (this)
{
if (imagehandle == null)
{
imagehandle = new NodeHandle();
}
if (mapSub != null && mapSub.topic != topic)
{
mapSub.shutdown();
mapSub = null;
}
if (mapSub != null)
{
return;
}
Console.WriteLine("Subscribing to map at:= " + topic);
lastUpdateTime = DateTime.Now;
mapSub = imagehandle.subscribe <nm.OccupancyGrid>(topic, 1, i => owner.BeginInvoke(new Action(() =>
{
//if (lastUpdateTime.AddMilliseconds(1000) > DateTime.Now) return;
lastUpdateTime = DateTime.Now;
mapResolution = i.info.resolution;
owner.MapResolution = mapResolution;
mapHeight = i.info.height;
mapWidth = i.info.width;
origin = new PointF((float)i.info.origin.position.x, (float)i.info.origin.position.y);
Point left_bottomPoint = owner.Map2World(origin);
left_bottomPoint.Offset(-(int)MapHeight, -(int)mapWidth);
SetRectangle(left_bottomPoint.X, left_bottomPoint.Y, (int)mapHeight, (int)MapWidth);
//if (Width != 0)
// actualResolution = (mapWidth / (float)Width) * mapResolution;
//else
// actualResolution = (mapWidth / (float)ActualWidth) * mapResolution;
//if (float.IsNaN(actualResolution) || float.IsInfinity(actualResolution))
// actualResolution = 0;
//else
//{
// MatchAspectRatio();
//}
Size size = new Size((int)i.info.width, (int)i.info.height);
byte[] data = createRGBA(i.data);
UpdateImage(data, size, false);
_originalImage = _image;
_image.RotateFlip(RotateFlipType.Rotate270FlipNone);
data = null;
if (occupancyGrid == null)
{
owner.PanX = owner.Width / 2 - (rectangle.Width / 2 + rectangle.Left);
owner.PanY = owner.Height / 2 - (rectangle.Height / 2 + rectangle.Top);
}
this.occupancyGrid = i;
//owner.Invalidate();
owner.LastDataUpdateTime = DateTime.Now;
})));
}
}
