示例#1
0
        private void Window_Loaded(object sender, RoutedEventArgs e)
        {
            ROS.ROS_MASTER_URI = "http://10.0.2.88:11311";
            ROS.ROS_HOSTNAME   = "10.0.2.152";
            ROS.Init(new string[0], NODE_NAME);

            nodeHandle = new NodeHandle();

            //server = nodeHandle.advertiseService<Messages.roscpp_tutorials.TwoInts, Messages.roscpp_tutorials.TwoInts.Request, Messages.roscpp_tutorials.TwoInts.Response>("/add_two_ints", addition);
            client = nodeHandle.serviceClient <Messages.roscpp_tutorials.TwoInts.Request, Messages.roscpp_tutorials.TwoInts.Response>("/add_two_ints");

            new Thread(new ThreadStart(() =>
            {
                Random r = new Random();
                while (!ROS.shutting_down)
                {
                    TwoInts.Request req = new TwoInts.Request()
                    {
                        a = r.Next(100), b = r.Next(100)
                    };
                    TwoInts.Response resp = new TwoInts.Response();
                    if (client.call(req, ref resp))
                    {
                        Dispatcher.Invoke(new Action(() =>
                        {
                            math.Content = "" + req.a + " + " + req.b + " = " + resp.sum;
                        }));
                    }
                    Thread.Sleep(500);
                }
            })).Start();
        }
示例#2
0
        private static void GetIK(NodeHandle node, gm.PoseStamped poseStamped, string group, double ikTimeout = 1.0, int ikAttempts = 0, bool avoidCollisions = false)
        {
            Console.WriteLine("GetIK");
            moveItMsgs.GetPositionIK.Request req = new moveItMsgs.GetPositionIK.Request();

            req.ik_request = new moveItMsgs.PositionIKRequest();
            Console.WriteLine(req.ik_request);
            req.ik_request.group_name       = group;
            req.ik_request.pose_stamped     = poseStamped;
            req.ik_request.timeout.data     = new TimeData(1, 0); // one second
            req.ik_request.attempts         = ikAttempts;
            req.ik_request.avoid_collisions = avoidCollisions;

            moveItMsgs.GetPositionIK.Response resp = new moveItMsgs.GetPositionIK.Response();
            DateTime before = DateTime.Now;

            Console.WriteLine("node.serviceClient");
            bool res = node.serviceClient <moveItMsgs.GetPositionIK.Request, moveItMsgs.GetPositionIK.Response>("/compute_ik").call(req, ref resp);

            if (res)
            {
                Console.WriteLine("got result");
                Console.WriteLine(resp.error_code.val);
            }
            else
            {
                Console.WriteLine("FAILED to receive respond from service");
            }
        }
示例#3
0
         private void Window_Loaded(object sender, RoutedEventArgs e) 
         {
            ROS.Init(new string[0], NODE_NAME+DateTime.Now.Ticks);

            nodeHandle = new NodeHandle();

            new Thread(new ThreadStart(() =>
                {
                    Random r = new Random();
                    while (ROS.ok)
                    {
                        TwoInts.Request req = new TwoInts.Request() { a = r.Next(100), b = r.Next(100) };
                        TwoInts.Response resp = new TwoInts.Response();
                        DateTime before = DateTime.Now;
                        bool res = nodeHandle.serviceClient<TwoInts.Request, TwoInts.Response>("/add_two_ints").call(req, ref resp);
                        TimeSpan dif = DateTime.Now.Subtract(before);
                            Dispatcher.Invoke(new Action(() =>
                                {
                                    string str = "";
                                    if (res)
                                        str = "" + req.a + " + " + req.b + " = " + resp.sum + "\n";
                                    else
                                        str = "call failed after\n";
                                    str += Math.Round(dif.TotalMilliseconds,2) + " ms";
                                    math.Content = str;
                                }));
                        Thread.Sleep(500);
                    }
                })).Start();
        }
示例#4
0
         private void Window_Loaded(object sender, RoutedEventArgs e) 
         {
            ROS.ROS_MASTER_URI = "http://10.0.2.88:11311";
            ROS.ROS_HOSTNAME = "10.0.2.152";
            ROS.Init(new string[0], NODE_NAME);

            nodeHandle = new NodeHandle();

            //server = nodeHandle.advertiseService<Messages.roscpp_tutorials.TwoInts, Messages.roscpp_tutorials.TwoInts.Request, Messages.roscpp_tutorials.TwoInts.Response>("/add_two_ints", addition);
            client = nodeHandle.serviceClient<Messages.roscpp_tutorials.TwoInts.Request, Messages.roscpp_tutorials.TwoInts.Response>("/add_two_ints");

            new Thread(new ThreadStart(() =>
                {
                    Random r = new Random();
                    while (!ROS.shutting_down)
                    {
                        TwoInts.Request req = new TwoInts.Request() { a = r.Next(100), b = r.Next(100) };
                        TwoInts.Response resp = new TwoInts.Response();
                        if (client.call(req, ref resp))
                            Dispatcher.Invoke(new Action(() =>
                                {
                                    math.Content = "" + req.a + " + " + req.b + " = " + resp.sum;
                                }));
                        Thread.Sleep(500);
                    }
                })).Start();
        }
示例#5
0
        static void Main(string[] args)
        {
            string NODE_NAME = "ServiceClientTest";

            try
            {
                ROS.Init(ref args, NODE_NAME + DateTime.Now.Ticks);
                var spinner = new AsyncSpinner();
                spinner.Start();
            }
            catch (RosException e)
            {
                ROS.Critical()("ROS.Init failed, shutting down: {0}", e.Message);
                ROS.shutdown();
                ROS.waitForShutdown();
                return;
            }

            try
            {
                var nodeHandle = new NodeHandle();
                while (ROS.ok)
                {
                    Random          r   = new Random();
                    TwoInts.Request req = new TwoInts.Request()
                    {
                        a = r.Next(100), b = r.Next(100)
                    };
                    TwoInts.Response resp   = new TwoInts.Response();
                    DateTime         before = DateTime.Now;
                    bool             res    = nodeHandle.serviceClient <TwoInts.Request, TwoInts.Response>("/add_two_ints").call(req, ref resp);
                    TimeSpan         dif    = DateTime.Now.Subtract(before);

                    string str = "";
                    if (res)
                    {
                        str = "" + req.a + " + " + req.b + " = " + resp.sum + "\n";
                    }
                    else
                    {
                        str = "call failed after ";
                    }

                    str += Math.Round(dif.TotalMilliseconds, 2) + " ms";
                    ROS.Info()(str);
                    Thread.Sleep(1000);
                }
            }
            catch (RosException e)
            {
                ROS.Critical()("Shutting down: {0}", e.Message);
            }

            ROS.shutdown();
            ROS.waitForShutdown();
        }
示例#6
0
        /// <summary>
        ///     Map provider topic
        /// </summary>
        //public string Topic
        //{
        //    get { return __topic as string; }
        //    set
        //    {
        //        __topic = value;
        //        SubscribeToMap(__topic);
        //    }
        //}
        /// <summary>
        /// 采用调用服务方式获取地图数据
        /// </summary>
        /// <param name="serviceName"></param>
        public void GetStaticMap(string serviceName)
        {
            lock (this)
            {
                if (imagehandle == null)
                {
                    imagehandle = new NodeHandle();
                }
                //if(mapServiceClient!=null)
                //{
                //    mapServiceClient.shutdown();
                //    mapServiceClient = null;
                //}
                //if (mapServiceClient != null)
                //    return;
                //mapServiceClient = imagehandle.serviceClient<nm.GetMap>(serviceName);

                //nm.GetMap srv = new nm.GetMap();
                nm.GetMap.Request  req  = new Messages.nav_msgs.GetMap.Request();
                nm.GetMap.Response resp = new Messages.nav_msgs.GetMap.Response();

                if (imagehandle.serviceClient <nm.GetMap.Request, nm.GetMap.Response>(serviceName).call(req, ref resp))
                {
                    nm.OccupancyGrid i = resp.map;
                    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);


                    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;

                    owner.PanX = owner.Width / 2 - (rectangle.Width / 2 + rectangle.Left);
                    owner.PanY = owner.Height / 2 - (rectangle.Height / 2 + rectangle.Top);
                    //owner.Invalidate();
                    owner.LastDataUpdateTime = DateTime.Now;
                }
            }
        }
示例#7
0
 public void Set(string key, string value)
 {
     new Action(() =>
     {
         lock (padlock)
         {
             Reconfigure.Request req = new Reconfigure.Request {
                 config = new Config {
                     strs = new[] { new StrParameter {
                                        name = new String(key), value = new String(value)
                                    } }
                 }
             };
             Reconfigure.Response resp = new Reconfigure.Response();
             if (!nh.serviceClient <Reconfigure.Request, Reconfigure.Response>(names.resolve(name, "set_parameters")).call(req, ref resp))
             {
                 Console.WriteLine("SET FAILED!");
             }
         }
     }).BeginInvoke(iar => { }, null);
 }
示例#8
0
        private void Window_Loaded(object sender, RoutedEventArgs e)
        {
            ROS.Init(new string[0], NODE_NAME + DateTime.Now.Ticks);

            nodeHandle = new NodeHandle();

            new Thread(new ThreadStart(() =>
            {
                Random r = new Random();
                while (ROS.ok)
                {
                    TwoInts.Request req = new TwoInts.Request()
                    {
                        a = r.Next(100), b = r.Next(100)
                    };
                    TwoInts.Response resp = new TwoInts.Response();
                    DateTime before       = DateTime.Now;
                    bool res     = nodeHandle.serviceClient <TwoInts.Request, TwoInts.Response>("/add_two_ints").call(req, ref resp);
                    TimeSpan dif = DateTime.Now.Subtract(before);
                    Dispatcher.Invoke(new Action(() =>
                    {
                        string str = "";
                        if (res)
                        {
                            str = "" + req.a + " + " + req.b + " = " + resp.sum + "\n";
                        }
                        else
                        {
                            str = "call failed after\n";
                        }
                        str         += Math.Round(dif.TotalMilliseconds, 2) + " ms";
                        math.Content = str;
                    }));
                    Thread.Sleep(500);
                }
            })).Start();
        }
示例#9
0
        static void Main(string[] args)
        {
            string NODE_NAME = "ServiceClientTest";

            var loggerFactory = new LoggerFactory();

            loggerFactory.AddProvider(
                new ConsoleLoggerProvider(
                    (string text, LogLevel logLevel) => { return(logLevel >= LogLevel.Debug); }, true)
                );
            Logger = ApplicationLogging.CreateLogger(nameof(Main));
            ROS.SetLoggerFactory(loggerFactory);


            try
            {
                ROS.Init(new string[0], NODE_NAME + DateTime.Now.Ticks);
            }
            catch (RosException e)
            {
                Logger.LogCritical("ROS.Init failed, shutting down: {0}", e.Message);
                ROS.shutdown();
                ROS.waitForShutdown();
                return;
            }

            try
            {
                var nodeHandle = new NodeHandle();
                while (ROS.ok)
                {
                    Random          r   = new Random();
                    TwoInts.Request req = new TwoInts.Request()
                    {
                        a = r.Next(100), b = r.Next(100)
                    };
                    TwoInts.Response resp   = new TwoInts.Response();
                    DateTime         before = DateTime.Now;
                    bool             res    = nodeHandle.serviceClient <TwoInts.Request, TwoInts.Response>("/add_two_ints").call(req, ref resp);
                    TimeSpan         dif    = DateTime.Now.Subtract(before);

                    string str = "";
                    if (res)
                    {
                        str = "" + req.a + " + " + req.b + " = " + resp.sum + "\n";
                    }
                    else
                    {
                        str = "call failed after ";
                    }

                    str += Math.Round(dif.TotalMilliseconds, 2) + " ms";
                    Logger.LogInformation(str);
                    Thread.Sleep(1000);
                }
            }
            catch (RosException e)
            {
                Logger.LogCritical("Shutting down: {0}", e.Message);
            }

            ROS.shutdown();
            ROS.waitForShutdown();
        }
示例#10
0
    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;
    }
示例#11
0
    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");
    }