void OnRosInit()
{
nh = ROS.GlobalNodeHandle;
// nh = new NodeHandle ( "~" );
pathSrv = nh.advertiseService <GetPath.Request, GetPath.Response> ("quad_rotor/get_path", GetPathService);
// setOrientSrv = nh.advertiseService<Messages.std_srvs.Empty.Request> ("quad_rotor/reset_orientation", TriggerReset)
// enableMotorSrv = nh.advertiseService<EnableMotors.Request, EnableMotors.Response> ( "enable_motors", OnEnableMotors );
nh.setParam("control_mode", "wrench"); // for now force twist mode
twistSub = nh.subscribe <Twist> ("quad_rotor/cmd_vel", 10, TwistCallback);
wrenchSub = nh.subscribe <Wrench> ("quad_rotor/cmd_force", 10, WrenchCallback);
posePub = nh.advertise <PoseStamped> ("quad_rotor/pose", 10, false);
imuPub = nh.advertise <Imu> ("quad_rotor/imu", 10, false);
// imgPub = nh.advertise<Image> ( "quad_rotor/image", 10, false );
pubThread = new Thread(PublishAll);
pubThread.Start();
gravitySrv = nh.advertiseService <SetBool.Request, SetBool.Response> ("quad_rotor/gravity", GravityService);
constrainForceXSrv = nh.advertiseService <SetBool.Request, SetBool.Response> ("quad_rotor/x_force_constrained", ConstrainForceX);
constrainForceYSrv = nh.advertiseService <SetBool.Request, SetBool.Response> ("quad_rotor/y_force_constrained", ConstrainForceY);
constrainForceZSrv = nh.advertiseService <SetBool.Request, SetBool.Response> ("quad_rotor/z_force_constrained", ConstrainForceZ);
constrainTorqueXSrv = nh.advertiseService <SetBool.Request, SetBool.Response> ("quad_rotor/x_torque_constrained", ConstrainTorqueX);
constrainTorqueYSrv = nh.advertiseService <SetBool.Request, SetBool.Response> ("quad_rotor/y_torque_constrained", ConstrainTorqueY);
constrainTorqueZSrv = nh.advertiseService <SetBool.Request, SetBool.Response> ("quad_rotor/z_torque_constrained", ConstrainTorqueZ);
triggerResetSrv = nh.advertiseService <SetBool.Request, SetBool.Response> ("quad_rotor/reset_orientation", TriggerReset);
// triggerResetSrv = nh.advertiseService<Empty.Request, Empty.Response> ( "quad_rotor/reset_orientation", TriggerReset );
setPoseSrv = nh.advertiseService <SetPose.Request, SetPose.Response> ("quad_rotor/set_pose", SetPoseService);
clearPathSrv = nh.advertiseService <SetBool.Request, SetBool.Response> ("quad_rotor/clear_path", ClearPathService);
setPathSrv = nh.advertiseService <SetPath.Request, SetPath.Response> ("quad_rotor/set_path", SetPathService);
}