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();
}
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");
}
}
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();
}
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();
}
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();
}
/// <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;
}
}
}
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);
}
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();
}
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();
}
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;
}
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");
}