static void Main(string[] args) { #if (DEBUG) Environment.SetEnvironmentVariable("ROS_MASTER_URI", "http://localhost:11311/"); #endif Console.WriteLine("Start ROS"); ROS.Init(ref args, "ActionServerClientSlowDummy"); ICallbackQueue callbackQueue = new CallbackQueue(); var asyncSpinner = new AsyncSpinner(callbackQueue); asyncSpinner.Start(); NodeHandle nodeHandle = new NodeHandle(callbackQueue); ActionClient <Messages.actionlib.TestGoal, Messages.actionlib.TestResult, Messages.actionlib.TestFeedback> actionClient = null; // setup action server start Console.WriteLine("Create server"); var actionServer = new ActionServer <Messages.actionlib.TestGoal, Messages.actionlib.TestResult, Messages.actionlib.TestFeedback>(nodeHandle, "test_action"); Param.Set("status_list_timeout", 999.9); actionServer.RegisterGoalCallback((sgoalHandle) => { Thread thread = new Thread(() => serverGoalCallback(sgoalHandle, actionServer, actionClient)); thread.Start(); }); Console.WriteLine("Start Server"); actionServer.Start(); Console.WriteLine("Server Started"); // setup action server finish // setup client actionClient = new ActionClient <Messages.actionlib.TestGoal, Messages.actionlib.TestResult, Messages.actionlib.TestFeedback>("test_action_slow", nodeHandle); // send action request to serverslowdummy Console.WriteLine("Wait for client and server to negotiate connection"); bool started = actionClient.WaitForActionServerToStart(new TimeSpan(0, 0, 3)); if (!started) { Console.WriteLine("Negotiation with server failed!"); } Console.ReadLine(); actionServer.Shutdown(); nodeHandle.shutdown(); ROS.shutdown(); }
/*static void Main(string[] args) * { * Console.WriteLine("Start ROS"); * ROS.Init(new string[0], "ActionClient"); * NodeHandle clientNodeHandle = new NodeHandle(); * * Console.WriteLine("Create client"); * var actionClient = new ActionClient<SingleJointPositionGoal, SingleJointPositionResult, * SingleJointPositionFeedback>("ActionTest", clientNodeHandle); * * Console.WriteLine("Wait for client and server to negotiate connection"); * bool started = actionClient.WaitForActionServerToStart(new TimeSpan(0, 0, 10)); * * * if (started || true) * { * var goal = new SingleJointPositionGoal(); * goal.position = 12.0; * goal.max_velocity = 42.0; * * Console.WriteLine("Send goal from client"); * var transition = false; * actionClient.SendGoal(goal, * (goalHandle) => { Console.WriteLine($"Transition: {goalHandle}"); transition = true; }, * (goalHandle, feedback) => { Console.WriteLine($"Feedback: {feedback}"); }); * * * Console.WriteLine("Wait for action client to receive transition"); * while (!transition) * { * Thread.Sleep(1); * } * } else * { * Console.WriteLine("Negotiation with server failed!"); * } * }*/ static void Main(string[] args) { Environment.SetEnvironmentVariable("ROS_HOSTNAME", "localhost"); Environment.SetEnvironmentVariable("ROS_IP", "127.0.0.1"); Environment.SetEnvironmentVariable("ROS_MASTER_URI", "http://localhost:11311/"); Console.WriteLine("Start ROS"); ROS.Init(ref args, "ActionClient"); ICallbackQueue callbackQueue = new CallbackQueue(); var asyncSpinner = new AsyncSpinner(callbackQueue); asyncSpinner.Start(); NodeHandle clientNodeHandle = new NodeHandle(callbackQueue); //ROS.GlobalNodeHandle; Console.WriteLine("Create client"); var actionClient = new ActionClient <Messages.actionlib.TestGoal, Messages.actionlib.TestResult, Messages.actionlib.TestFeedback>("test_action", clientNodeHandle); Console.WriteLine("Wait for client and server to negotiate connection"); bool started = actionClient.WaitForActionServerToStart(new TimeSpan(0, 0, 60)); if (started) { int counter = 0; var dict = new Dictionary <int, Messages.actionlib.TestGoal>(); while (!Console.KeyAvailable) { var now = DateTime.UtcNow; Console.WriteLine($"Waited: {DateTime.UtcNow - now}"); var goal = new Messages.actionlib.TestGoal { goal = counter }; dict[counter] = goal; counter += 1; Console.WriteLine($"------------------> Send goal {goal.goal} from client"); var cts = new CancellationTokenSource(); var goalTask = actionClient.SendGoalAsync(goal, (goalHandle) => { if (goalHandle.State == CommunicationState.DONE) { int g = goalHandle.Goal.Goal.goal; var resultt = goalHandle.Result; if (resultt != null) { Console.WriteLine($"------------------> Got Result for goal {g}: {resultt.result}"); } else { Console.WriteLine($"Result for goal {g} is NULL!"); } dict.Remove(g); } }, (goalHandle, feedback) => { Console.WriteLine($"------------------> Got Feedback: {feedback.Feedback.feedback}"); }, cts.Token ); var waiter = goalTask.GetAwaiter(); goalTask.Wait(); Console.ReadLine(); while (!waiter.IsCompleted) { Thread.Yield(); } var result = waiter.GetResult(); break; } Console.WriteLine("Wait for 15s for open goals"); var timeOut = new TimeSpan(0, 0, 15); var start = DateTime.UtcNow; while ((DateTime.UtcNow - start <= timeOut) && (dict.Count > 0)) { Thread.Sleep(1); } if (dict.Count == 0) { Console.WriteLine("All goals have been reached!"); } else { Console.WriteLine("TIMEOUT: There are still open goals"); } } else { Console.WriteLine("Negotiation with server failed!"); } Console.WriteLine("Shutdown ROS"); actionClient.Shutdown(); clientNodeHandle.shutdown(); ROS.shutdown(); }
public bool processQueue(Time timestamp, Duration tolerance, Duration delay, Duration wait, CallbackQueue callback_queue) { bool new_command = false; lock ( command_queue_mutex_ ) { Time min = new Time(timestamp.data); Time max = new Time(timestamp.data); try { min = timestamp - delay - tolerance /* - ros::Duration(dt) */; } catch (Exception e) {} try { max = timestamp - delay + tolerance; } catch (Exception e) {} do { while (command_queue_.Count > 0) { MotorPWM new_motor_voltage = command_queue_.Peek(); Time new_time = new_motor_voltage.header.Stamp; if (new_time.data.toSec() == 0.0 || (new_time >= min && new_time <= max)) { setVoltage(new_motor_voltage); command_queue_.Dequeue(); new_command = true; // new motor command is too old: } else if (new_time < min) { ROS.Debug("quadrotor_propulsion", "Command received was %fs too old. Discarding.", (new_time - timestamp).data.toSec()); command_queue_.Dequeue(); // new motor command is too new: } else { break; } } if (command_queue_.Count == 0 && !new_command) { if (!motor_status_.on || wait.data.toSec() == 0.0) { break; } ROS.Debug("quadrotor_propulsion", "Waiting for command at simulation step t = %fs... last update was %fs ago", timestamp.data.toSec(), (timestamp - last_command_time_).data.toSec()); if (callback_queue == null) { if (command_condition_.WaitOne((int)(wait.data.toSec() * 1000))) { continue; } // if (command_condition_.timed_wait(lock, wait.toBoost())) continue; } else { command_condition_.Set(); callback_queue.callAvailable((int)(wait.toSec() * 1000)); command_condition_.Reset(); if (command_queue_.Count > 0) { continue; } } ROS.Error("quadrotor_propulsion", "Command timed out. Disabled motors."); shutdown(); } } while (false); if (new_command) { // ROS.Debug ("quadrotor_propulsion", "Using motor command valid at t = " << last_command_time_.toSec() << "s for simulation step at t = " << timestamp.toSec() << "s (dt = " << (timestamp - last_command_time_).toSec() << "s)"); } } return(new_command); }
static void Main(string[] args) { //#if (DEBUG) // Environment.SetEnvironmentVariable("ROS_HOSTNAME", ""); // Environment.SetEnvironmentVariable("ROS_IP", "192.168.200.32"); // Environment.SetEnvironmentVariable("ROS_MASTER_URI", "http://192.168.200.231:11311/"); //#endif Environment.SetEnvironmentVariable("ROS_HOSTNAME", "localhost"); Environment.SetEnvironmentVariable("ROS_IP", "127.0.0.1"); Environment.SetEnvironmentVariable("ROS_MASTER_URI", "http://localhost:11311/"); Console.WriteLine("Start ROS"); ROS.Init(ref args, "ActionServerSlowDummy"); ICallbackQueue callbackQueue = new CallbackQueue(); var asyncSpinner = new AsyncSpinner(callbackQueue); asyncSpinner.Start(); //var spinner = new SingleThreadSpinner(callbackQueue); NodeHandle serverNodeHandle = new NodeHandle(callbackQueue); Console.WriteLine("Create server"); var actionServer = new ActionServer <Messages.actionlib.TestGoal, Messages.actionlib.TestResult, Messages.actionlib.TestFeedback>(serverNodeHandle, "test_action_slow"); Console.WriteLine("Start Server"); Param.Set("status_list_timeout", 999.9); actionServer.Start(); actionServer.RegisterGoalCallback((goalHandle) => { Console.WriteLine($"Goal registered callback. Goal: {goalHandle.Goal.goal}"); goalHandle.SetAccepted("accepted"); new Thread(() => { for (int x = 0; x < 77; x++) { var fb = new Messages.actionlib.TestFeedback { feedback = x }; goalHandle.PublishFeedback(fb); Thread.Sleep(100); } var result = new Messages.actionlib.TestResult { result = 123 }; goalHandle.SetGoalStatus(Messages.actionlib_msgs.GoalStatus.SUCCEEDED, "done"); actionServer.PublishResult(goalHandle.GoalStatus, result); }).Start(); }); Console.ReadLine(); actionServer.Shutdown(); serverNodeHandle.shutdown(); ROS.shutdown(); }