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