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