diff --git a/.nojekyll b/.nojekyll new file mode 100644 index 00000000..e69de29b diff --git a/404.html b/404.html new file mode 100644 index 00000000..1f2ac578 --- /dev/null +++ b/404.html @@ -0,0 +1,2580 @@ + + + + + + + + + + + + + + + + + + + AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ +

404 - Not found

+ +
+
+ + + +
+ + + +
+ + + +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/CNAME b/CNAME new file mode 100644 index 00000000..3f5e3774 --- /dev/null +++ b/CNAME @@ -0,0 +1 @@ +docs.theairlab.org diff --git a/CODE_OF_CONDUCT/index.html b/CODE_OF_CONDUCT/index.html new file mode 100644 index 00000000..a0c58595 --- /dev/null +++ b/CODE_OF_CONDUCT/index.html @@ -0,0 +1,2660 @@ + + + + + + + + + + + + + + + + + + + Contributor Covenant Code of Conduct - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + Skip to content + + +
+
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

Contributor Covenant Code of Conduct

+

Our Pledge

+

In the interest of fostering an open and welcoming environment, we as +contributors and maintainers pledge to making participation in our project and +our community a harassment-free experience for everyone, regardless of age, body +size, disability, ethnicity, gender identity and expression, level of experience, +education, socio-economic status, nationality, personal appearance, race, +religion, or sexual identity and orientation.

+

Our Standards

+

Examples of behavior that contributes to creating a positive environment +include:

+
    +
  • Using welcoming and inclusive language
    • +
  • Being respectful of differing viewpoints and experiences
    • +
  • Gracefully accepting constructive criticism
    • +
  • Focusing on what is best for the community
    • +
  • Showing empathy towards other community members
    • +
+

Examples of unacceptable behavior by participants include:

+
    +
  • The use of sexualized language or imagery and unwelcome sexual attention or + advances
    • +
  • Trolling, insulting/derogatory comments, and personal or political attacks
    • +
  • Public or private harassment
    • +
  • Publishing others' private information, such as a physical or electronic + address, without explicit permission
    • +
  • Other conduct which could reasonably be considered inappropriate in a + professional setting
    • +
+

Our Responsibilities

+

Project maintainers are responsible for clarifying the standards of acceptable +behavior and are expected to take appropriate and fair corrective action in +response to any instances of unacceptable behavior.

+

Project maintainers have the right and responsibility to remove, edit, or +reject comments, commits, code, wiki edits, issues, and other contributions +that are not aligned to this Code of Conduct, or to ban temporarily or +permanently any contributor for other behaviors that they deem inappropriate, +threatening, offensive, or harmful.

+

Scope

+

This Code of Conduct applies both within project spaces and in public spaces +when an individual is representing the project or its community. Examples of +representing a project or community include using an official project e-mail +address, posting via an official social media account, or acting as an appointed +representative at an online or offline event. Representation of a project may be +further defined and clarified by project maintainers.

+

Enforcement

+

Instances of abusive, harassing, or otherwise unacceptable behavior may be +reported by contacting the project team at {{ email }}. All +complaints will be reviewed and investigated and will result in a response that +is deemed necessary and appropriate to the circumstances. The project team is +obligated to maintain confidentiality with regard to the reporter of an incident. +Further details of specific enforcement policies may be posted separately.

+

Project maintainers who do not follow or enforce the Code of Conduct in good +faith may face temporary or permanent repercussions as determined by other +members of the project's leadership.

+

Attribution

+

This Code of Conduct is adapted from the Contributor Covenant, version 1.4, +available at https://www.contributor-covenant.org/version/1/4/code-of-conduct.html

+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ + + +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/assets/images/favicon.png b/assets/images/favicon.png new file mode 100644 index 00000000..1cf13b9f Binary files /dev/null and b/assets/images/favicon.png differ diff --git a/assets/javascripts/bundle.83f73b43.min.js b/assets/javascripts/bundle.83f73b43.min.js new file mode 100644 index 00000000..43d8b70f --- /dev/null +++ b/assets/javascripts/bundle.83f73b43.min.js @@ -0,0 +1,16 @@ +"use strict";(()=>{var Wi=Object.create;var gr=Object.defineProperty;var Di=Object.getOwnPropertyDescriptor;var Vi=Object.getOwnPropertyNames,Vt=Object.getOwnPropertySymbols,Ni=Object.getPrototypeOf,yr=Object.prototype.hasOwnProperty,ao=Object.prototype.propertyIsEnumerable;var io=(e,t,r)=>t in e?gr(e,t,{enumerable:!0,configurable:!0,writable:!0,value:r}):e[t]=r,$=(e,t)=>{for(var r in t||(t={}))yr.call(t,r)&&io(e,r,t[r]);if(Vt)for(var r of Vt(t))ao.call(t,r)&&io(e,r,t[r]);return e};var so=(e,t)=>{var r={};for(var o in e)yr.call(e,o)&&t.indexOf(o)<0&&(r[o]=e[o]);if(e!=null&&Vt)for(var o of Vt(e))t.indexOf(o)<0&&ao.call(e,o)&&(r[o]=e[o]);return r};var xr=(e,t)=>()=>(t||e((t={exports:{}}).exports,t),t.exports);var zi=(e,t,r,o)=>{if(t&&typeof t=="object"||typeof t=="function")for(let n of Vi(t))!yr.call(e,n)&&n!==r&&gr(e,n,{get:()=>t[n],enumerable:!(o=Di(t,n))||o.enumerable});return e};var Mt=(e,t,r)=>(r=e!=null?Wi(Ni(e)):{},zi(t||!e||!e.__esModule?gr(r,"default",{value:e,enumerable:!0}):r,e));var co=(e,t,r)=>new Promise((o,n)=>{var i=p=>{try{s(r.next(p))}catch(c){n(c)}},a=p=>{try{s(r.throw(p))}catch(c){n(c)}},s=p=>p.done?o(p.value):Promise.resolve(p.value).then(i,a);s((r=r.apply(e,t)).next())});var lo=xr((Er,po)=>{(function(e,t){typeof Er=="object"&&typeof po!="undefined"?t():typeof define=="function"&&define.amd?define(t):t()})(Er,function(){"use strict";function e(r){var o=!0,n=!1,i=null,a={text:!0,search:!0,url:!0,tel:!0,email:!0,password:!0,number:!0,date:!0,month:!0,week:!0,time:!0,datetime:!0,"datetime-local":!0};function s(k){return!!(k&&k!==document&&k.nodeName!=="HTML"&&k.nodeName!=="BODY"&&"classList"in k&&"contains"in k.classList)}function p(k){var ft=k.type,qe=k.tagName;return!!(qe==="INPUT"&&a[ft]&&!k.readOnly||qe==="TEXTAREA"&&!k.readOnly||k.isContentEditable)}function c(k){k.classList.contains("focus-visible")||(k.classList.add("focus-visible"),k.setAttribute("data-focus-visible-added",""))}function l(k){k.hasAttribute("data-focus-visible-added")&&(k.classList.remove("focus-visible"),k.removeAttribute("data-focus-visible-added"))}function f(k){k.metaKey||k.altKey||k.ctrlKey||(s(r.activeElement)&&c(r.activeElement),o=!0)}function u(k){o=!1}function d(k){s(k.target)&&(o||p(k.target))&&c(k.target)}function y(k){s(k.target)&&(k.target.classList.contains("focus-visible")||k.target.hasAttribute("data-focus-visible-added"))&&(n=!0,window.clearTimeout(i),i=window.setTimeout(function(){n=!1},100),l(k.target))}function L(k){document.visibilityState==="hidden"&&(n&&(o=!0),X())}function X(){document.addEventListener("mousemove",J),document.addEventListener("mousedown",J),document.addEventListener("mouseup",J),document.addEventListener("pointermove",J),document.addEventListener("pointerdown",J),document.addEventListener("pointerup",J),document.addEventListener("touchmove",J),document.addEventListener("touchstart",J),document.addEventListener("touchend",J)}function te(){document.removeEventListener("mousemove",J),document.removeEventListener("mousedown",J),document.removeEventListener("mouseup",J),document.removeEventListener("pointermove",J),document.removeEventListener("pointerdown",J),document.removeEventListener("pointerup",J),document.removeEventListener("touchmove",J),document.removeEventListener("touchstart",J),document.removeEventListener("touchend",J)}function J(k){k.target.nodeName&&k.target.nodeName.toLowerCase()==="html"||(o=!1,te())}document.addEventListener("keydown",f,!0),document.addEventListener("mousedown",u,!0),document.addEventListener("pointerdown",u,!0),document.addEventListener("touchstart",u,!0),document.addEventListener("visibilitychange",L,!0),X(),r.addEventListener("focus",d,!0),r.addEventListener("blur",y,!0),r.nodeType===Node.DOCUMENT_FRAGMENT_NODE&&r.host?r.host.setAttribute("data-js-focus-visible",""):r.nodeType===Node.DOCUMENT_NODE&&(document.documentElement.classList.add("js-focus-visible"),document.documentElement.setAttribute("data-js-focus-visible",""))}if(typeof window!="undefined"&&typeof document!="undefined"){window.applyFocusVisiblePolyfill=e;var t;try{t=new CustomEvent("focus-visible-polyfill-ready")}catch(r){t=document.createEvent("CustomEvent"),t.initCustomEvent("focus-visible-polyfill-ready",!1,!1,{})}window.dispatchEvent(t)}typeof document!="undefined"&&e(document)})});var qr=xr((hy,On)=>{"use strict";/*! + * escape-html + * Copyright(c) 2012-2013 TJ Holowaychuk + * Copyright(c) 2015 Andreas Lubbe + * Copyright(c) 2015 Tiancheng "Timothy" Gu + * MIT Licensed + */var $a=/["'&<>]/;On.exports=Pa;function Pa(e){var t=""+e,r=$a.exec(t);if(!r)return t;var o,n="",i=0,a=0;for(i=r.index;i{/*! + * clipboard.js v2.0.11 + * https://clipboardjs.com/ + * + * Licensed MIT © Zeno Rocha + */(function(t,r){typeof It=="object"&&typeof Yr=="object"?Yr.exports=r():typeof define=="function"&&define.amd?define([],r):typeof It=="object"?It.ClipboardJS=r():t.ClipboardJS=r()})(It,function(){return function(){var e={686:function(o,n,i){"use strict";i.d(n,{default:function(){return Ui}});var a=i(279),s=i.n(a),p=i(370),c=i.n(p),l=i(817),f=i.n(l);function u(V){try{return document.execCommand(V)}catch(A){return!1}}var d=function(A){var M=f()(A);return u("cut"),M},y=d;function L(V){var A=document.documentElement.getAttribute("dir")==="rtl",M=document.createElement("textarea");M.style.fontSize="12pt",M.style.border="0",M.style.padding="0",M.style.margin="0",M.style.position="absolute",M.style[A?"right":"left"]="-9999px";var F=window.pageYOffset||document.documentElement.scrollTop;return M.style.top="".concat(F,"px"),M.setAttribute("readonly",""),M.value=V,M}var X=function(A,M){var F=L(A);M.container.appendChild(F);var D=f()(F);return u("copy"),F.remove(),D},te=function(A){var M=arguments.length>1&&arguments[1]!==void 0?arguments[1]:{container:document.body},F="";return typeof A=="string"?F=X(A,M):A instanceof HTMLInputElement&&!["text","search","url","tel","password"].includes(A==null?void 0:A.type)?F=X(A.value,M):(F=f()(A),u("copy")),F},J=te;function k(V){"@babel/helpers - typeof";return typeof Symbol=="function"&&typeof Symbol.iterator=="symbol"?k=function(M){return typeof M}:k=function(M){return M&&typeof Symbol=="function"&&M.constructor===Symbol&&M!==Symbol.prototype?"symbol":typeof M},k(V)}var ft=function(){var A=arguments.length>0&&arguments[0]!==void 0?arguments[0]:{},M=A.action,F=M===void 0?"copy":M,D=A.container,Y=A.target,$e=A.text;if(F!=="copy"&&F!=="cut")throw new Error('Invalid "action" value, use either "copy" or "cut"');if(Y!==void 0)if(Y&&k(Y)==="object"&&Y.nodeType===1){if(F==="copy"&&Y.hasAttribute("disabled"))throw new Error('Invalid "target" attribute. Please use "readonly" instead of "disabled" attribute');if(F==="cut"&&(Y.hasAttribute("readonly")||Y.hasAttribute("disabled")))throw new Error(`Invalid "target" attribute. You can't cut text from elements with "readonly" or "disabled" attributes`)}else throw new Error('Invalid "target" value, use a valid Element');if($e)return J($e,{container:D});if(Y)return F==="cut"?y(Y):J(Y,{container:D})},qe=ft;function Fe(V){"@babel/helpers - typeof";return typeof Symbol=="function"&&typeof Symbol.iterator=="symbol"?Fe=function(M){return typeof M}:Fe=function(M){return M&&typeof Symbol=="function"&&M.constructor===Symbol&&M!==Symbol.prototype?"symbol":typeof M},Fe(V)}function ki(V,A){if(!(V instanceof A))throw new TypeError("Cannot call a class as a function")}function no(V,A){for(var M=0;M0&&arguments[0]!==void 0?arguments[0]:{};this.action=typeof D.action=="function"?D.action:this.defaultAction,this.target=typeof D.target=="function"?D.target:this.defaultTarget,this.text=typeof D.text=="function"?D.text:this.defaultText,this.container=Fe(D.container)==="object"?D.container:document.body}},{key:"listenClick",value:function(D){var Y=this;this.listener=c()(D,"click",function($e){return Y.onClick($e)})}},{key:"onClick",value:function(D){var Y=D.delegateTarget||D.currentTarget,$e=this.action(Y)||"copy",Dt=qe({action:$e,container:this.container,target:this.target(Y),text:this.text(Y)});this.emit(Dt?"success":"error",{action:$e,text:Dt,trigger:Y,clearSelection:function(){Y&&Y.focus(),window.getSelection().removeAllRanges()}})}},{key:"defaultAction",value:function(D){return vr("action",D)}},{key:"defaultTarget",value:function(D){var Y=vr("target",D);if(Y)return document.querySelector(Y)}},{key:"defaultText",value:function(D){return vr("text",D)}},{key:"destroy",value:function(){this.listener.destroy()}}],[{key:"copy",value:function(D){var Y=arguments.length>1&&arguments[1]!==void 0?arguments[1]:{container:document.body};return J(D,Y)}},{key:"cut",value:function(D){return y(D)}},{key:"isSupported",value:function(){var D=arguments.length>0&&arguments[0]!==void 0?arguments[0]:["copy","cut"],Y=typeof D=="string"?[D]:D,$e=!!document.queryCommandSupported;return Y.forEach(function(Dt){$e=$e&&!!document.queryCommandSupported(Dt)}),$e}}]),M}(s()),Ui=Fi},828:function(o){var n=9;if(typeof Element!="undefined"&&!Element.prototype.matches){var i=Element.prototype;i.matches=i.matchesSelector||i.mozMatchesSelector||i.msMatchesSelector||i.oMatchesSelector||i.webkitMatchesSelector}function a(s,p){for(;s&&s.nodeType!==n;){if(typeof s.matches=="function"&&s.matches(p))return s;s=s.parentNode}}o.exports=a},438:function(o,n,i){var a=i(828);function s(l,f,u,d,y){var L=c.apply(this,arguments);return l.addEventListener(u,L,y),{destroy:function(){l.removeEventListener(u,L,y)}}}function p(l,f,u,d,y){return typeof l.addEventListener=="function"?s.apply(null,arguments):typeof u=="function"?s.bind(null,document).apply(null,arguments):(typeof l=="string"&&(l=document.querySelectorAll(l)),Array.prototype.map.call(l,function(L){return s(L,f,u,d,y)}))}function c(l,f,u,d){return function(y){y.delegateTarget=a(y.target,f),y.delegateTarget&&d.call(l,y)}}o.exports=p},879:function(o,n){n.node=function(i){return i!==void 0&&i instanceof HTMLElement&&i.nodeType===1},n.nodeList=function(i){var a=Object.prototype.toString.call(i);return i!==void 0&&(a==="[object NodeList]"||a==="[object HTMLCollection]")&&"length"in i&&(i.length===0||n.node(i[0]))},n.string=function(i){return typeof i=="string"||i instanceof String},n.fn=function(i){var a=Object.prototype.toString.call(i);return a==="[object Function]"}},370:function(o,n,i){var a=i(879),s=i(438);function p(u,d,y){if(!u&&!d&&!y)throw new Error("Missing required arguments");if(!a.string(d))throw new TypeError("Second argument must be a String");if(!a.fn(y))throw new TypeError("Third argument must be a Function");if(a.node(u))return c(u,d,y);if(a.nodeList(u))return l(u,d,y);if(a.string(u))return f(u,d,y);throw new TypeError("First argument must be a String, HTMLElement, HTMLCollection, or NodeList")}function c(u,d,y){return u.addEventListener(d,y),{destroy:function(){u.removeEventListener(d,y)}}}function l(u,d,y){return Array.prototype.forEach.call(u,function(L){L.addEventListener(d,y)}),{destroy:function(){Array.prototype.forEach.call(u,function(L){L.removeEventListener(d,y)})}}}function f(u,d,y){return s(document.body,u,d,y)}o.exports=p},817:function(o){function n(i){var a;if(i.nodeName==="SELECT")i.focus(),a=i.value;else if(i.nodeName==="INPUT"||i.nodeName==="TEXTAREA"){var s=i.hasAttribute("readonly");s||i.setAttribute("readonly",""),i.select(),i.setSelectionRange(0,i.value.length),s||i.removeAttribute("readonly"),a=i.value}else{i.hasAttribute("contenteditable")&&i.focus();var p=window.getSelection(),c=document.createRange();c.selectNodeContents(i),p.removeAllRanges(),p.addRange(c),a=p.toString()}return a}o.exports=n},279:function(o){function n(){}n.prototype={on:function(i,a,s){var p=this.e||(this.e={});return(p[i]||(p[i]=[])).push({fn:a,ctx:s}),this},once:function(i,a,s){var p=this;function c(){p.off(i,c),a.apply(s,arguments)}return c._=a,this.on(i,c,s)},emit:function(i){var a=[].slice.call(arguments,1),s=((this.e||(this.e={}))[i]||[]).slice(),p=0,c=s.length;for(p;p0&&i[i.length-1])&&(c[0]===6||c[0]===2)){r=0;continue}if(c[0]===3&&(!i||c[1]>i[0]&&c[1]=e.length&&(e=void 0),{value:e&&e[o++],done:!e}}};throw new TypeError(t?"Object is not iterable.":"Symbol.iterator is not defined.")}function N(e,t){var r=typeof Symbol=="function"&&e[Symbol.iterator];if(!r)return e;var o=r.call(e),n,i=[],a;try{for(;(t===void 0||t-- >0)&&!(n=o.next()).done;)i.push(n.value)}catch(s){a={error:s}}finally{try{n&&!n.done&&(r=o.return)&&r.call(o)}finally{if(a)throw a.error}}return i}function q(e,t,r){if(r||arguments.length===2)for(var o=0,n=t.length,i;o1||p(d,L)})},y&&(n[d]=y(n[d])))}function p(d,y){try{c(o[d](y))}catch(L){u(i[0][3],L)}}function c(d){d.value instanceof nt?Promise.resolve(d.value.v).then(l,f):u(i[0][2],d)}function l(d){p("next",d)}function f(d){p("throw",d)}function u(d,y){d(y),i.shift(),i.length&&p(i[0][0],i[0][1])}}function uo(e){if(!Symbol.asyncIterator)throw new TypeError("Symbol.asyncIterator is not defined.");var t=e[Symbol.asyncIterator],r;return t?t.call(e):(e=typeof he=="function"?he(e):e[Symbol.iterator](),r={},o("next"),o("throw"),o("return"),r[Symbol.asyncIterator]=function(){return this},r);function o(i){r[i]=e[i]&&function(a){return new Promise(function(s,p){a=e[i](a),n(s,p,a.done,a.value)})}}function n(i,a,s,p){Promise.resolve(p).then(function(c){i({value:c,done:s})},a)}}function H(e){return typeof e=="function"}function ut(e){var t=function(o){Error.call(o),o.stack=new Error().stack},r=e(t);return r.prototype=Object.create(Error.prototype),r.prototype.constructor=r,r}var zt=ut(function(e){return function(r){e(this),this.message=r?r.length+` errors occurred during unsubscription: +`+r.map(function(o,n){return n+1+") "+o.toString()}).join(` + `):"",this.name="UnsubscriptionError",this.errors=r}});function Qe(e,t){if(e){var r=e.indexOf(t);0<=r&&e.splice(r,1)}}var Ue=function(){function e(t){this.initialTeardown=t,this.closed=!1,this._parentage=null,this._finalizers=null}return e.prototype.unsubscribe=function(){var t,r,o,n,i;if(!this.closed){this.closed=!0;var a=this._parentage;if(a)if(this._parentage=null,Array.isArray(a))try{for(var s=he(a),p=s.next();!p.done;p=s.next()){var c=p.value;c.remove(this)}}catch(L){t={error:L}}finally{try{p&&!p.done&&(r=s.return)&&r.call(s)}finally{if(t)throw t.error}}else a.remove(this);var l=this.initialTeardown;if(H(l))try{l()}catch(L){i=L instanceof zt?L.errors:[L]}var f=this._finalizers;if(f){this._finalizers=null;try{for(var u=he(f),d=u.next();!d.done;d=u.next()){var y=d.value;try{ho(y)}catch(L){i=i!=null?i:[],L instanceof zt?i=q(q([],N(i)),N(L.errors)):i.push(L)}}}catch(L){o={error:L}}finally{try{d&&!d.done&&(n=u.return)&&n.call(u)}finally{if(o)throw o.error}}}if(i)throw new zt(i)}},e.prototype.add=function(t){var r;if(t&&t!==this)if(this.closed)ho(t);else{if(t instanceof e){if(t.closed||t._hasParent(this))return;t._addParent(this)}(this._finalizers=(r=this._finalizers)!==null&&r!==void 0?r:[]).push(t)}},e.prototype._hasParent=function(t){var r=this._parentage;return r===t||Array.isArray(r)&&r.includes(t)},e.prototype._addParent=function(t){var r=this._parentage;this._parentage=Array.isArray(r)?(r.push(t),r):r?[r,t]:t},e.prototype._removeParent=function(t){var r=this._parentage;r===t?this._parentage=null:Array.isArray(r)&&Qe(r,t)},e.prototype.remove=function(t){var r=this._finalizers;r&&Qe(r,t),t instanceof e&&t._removeParent(this)},e.EMPTY=function(){var t=new e;return t.closed=!0,t}(),e}();var Tr=Ue.EMPTY;function qt(e){return e instanceof Ue||e&&"closed"in e&&H(e.remove)&&H(e.add)&&H(e.unsubscribe)}function ho(e){H(e)?e():e.unsubscribe()}var Pe={onUnhandledError:null,onStoppedNotification:null,Promise:void 0,useDeprecatedSynchronousErrorHandling:!1,useDeprecatedNextContext:!1};var dt={setTimeout:function(e,t){for(var r=[],o=2;o0},enumerable:!1,configurable:!0}),t.prototype._trySubscribe=function(r){return this._throwIfClosed(),e.prototype._trySubscribe.call(this,r)},t.prototype._subscribe=function(r){return this._throwIfClosed(),this._checkFinalizedStatuses(r),this._innerSubscribe(r)},t.prototype._innerSubscribe=function(r){var o=this,n=this,i=n.hasError,a=n.isStopped,s=n.observers;return i||a?Tr:(this.currentObservers=null,s.push(r),new Ue(function(){o.currentObservers=null,Qe(s,r)}))},t.prototype._checkFinalizedStatuses=function(r){var o=this,n=o.hasError,i=o.thrownError,a=o.isStopped;n?r.error(i):a&&r.complete()},t.prototype.asObservable=function(){var r=new j;return r.source=this,r},t.create=function(r,o){return new To(r,o)},t}(j);var To=function(e){oe(t,e);function t(r,o){var n=e.call(this)||this;return n.destination=r,n.source=o,n}return t.prototype.next=function(r){var o,n;(n=(o=this.destination)===null||o===void 0?void 0:o.next)===null||n===void 0||n.call(o,r)},t.prototype.error=function(r){var o,n;(n=(o=this.destination)===null||o===void 0?void 0:o.error)===null||n===void 0||n.call(o,r)},t.prototype.complete=function(){var r,o;(o=(r=this.destination)===null||r===void 0?void 0:r.complete)===null||o===void 0||o.call(r)},t.prototype._subscribe=function(r){var o,n;return(n=(o=this.source)===null||o===void 0?void 0:o.subscribe(r))!==null&&n!==void 0?n:Tr},t}(g);var _r=function(e){oe(t,e);function t(r){var o=e.call(this)||this;return o._value=r,o}return Object.defineProperty(t.prototype,"value",{get:function(){return this.getValue()},enumerable:!1,configurable:!0}),t.prototype._subscribe=function(r){var o=e.prototype._subscribe.call(this,r);return!o.closed&&r.next(this._value),o},t.prototype.getValue=function(){var r=this,o=r.hasError,n=r.thrownError,i=r._value;if(o)throw n;return this._throwIfClosed(),i},t.prototype.next=function(r){e.prototype.next.call(this,this._value=r)},t}(g);var At={now:function(){return(At.delegate||Date).now()},delegate:void 0};var Ct=function(e){oe(t,e);function t(r,o,n){r===void 0&&(r=1/0),o===void 0&&(o=1/0),n===void 0&&(n=At);var i=e.call(this)||this;return i._bufferSize=r,i._windowTime=o,i._timestampProvider=n,i._buffer=[],i._infiniteTimeWindow=!0,i._infiniteTimeWindow=o===1/0,i._bufferSize=Math.max(1,r),i._windowTime=Math.max(1,o),i}return t.prototype.next=function(r){var o=this,n=o.isStopped,i=o._buffer,a=o._infiniteTimeWindow,s=o._timestampProvider,p=o._windowTime;n||(i.push(r),!a&&i.push(s.now()+p)),this._trimBuffer(),e.prototype.next.call(this,r)},t.prototype._subscribe=function(r){this._throwIfClosed(),this._trimBuffer();for(var o=this._innerSubscribe(r),n=this,i=n._infiniteTimeWindow,a=n._buffer,s=a.slice(),p=0;p0?e.prototype.schedule.call(this,r,o):(this.delay=o,this.state=r,this.scheduler.flush(this),this)},t.prototype.execute=function(r,o){return o>0||this.closed?e.prototype.execute.call(this,r,o):this._execute(r,o)},t.prototype.requestAsyncId=function(r,o,n){return n===void 0&&(n=0),n!=null&&n>0||n==null&&this.delay>0?e.prototype.requestAsyncId.call(this,r,o,n):(r.flush(this),0)},t}(gt);var Lo=function(e){oe(t,e);function t(){return e!==null&&e.apply(this,arguments)||this}return t}(yt);var kr=new Lo(Oo);var Mo=function(e){oe(t,e);function t(r,o){var n=e.call(this,r,o)||this;return n.scheduler=r,n.work=o,n}return t.prototype.requestAsyncId=function(r,o,n){return n===void 0&&(n=0),n!==null&&n>0?e.prototype.requestAsyncId.call(this,r,o,n):(r.actions.push(this),r._scheduled||(r._scheduled=vt.requestAnimationFrame(function(){return r.flush(void 0)})))},t.prototype.recycleAsyncId=function(r,o,n){var i;if(n===void 0&&(n=0),n!=null?n>0:this.delay>0)return e.prototype.recycleAsyncId.call(this,r,o,n);var a=r.actions;o!=null&&((i=a[a.length-1])===null||i===void 0?void 0:i.id)!==o&&(vt.cancelAnimationFrame(o),r._scheduled=void 0)},t}(gt);var _o=function(e){oe(t,e);function t(){return e!==null&&e.apply(this,arguments)||this}return t.prototype.flush=function(r){this._active=!0;var o=this._scheduled;this._scheduled=void 0;var n=this.actions,i;r=r||n.shift();do if(i=r.execute(r.state,r.delay))break;while((r=n[0])&&r.id===o&&n.shift());if(this._active=!1,i){for(;(r=n[0])&&r.id===o&&n.shift();)r.unsubscribe();throw i}},t}(yt);var me=new _o(Mo);var S=new j(function(e){return e.complete()});function Yt(e){return e&&H(e.schedule)}function Hr(e){return e[e.length-1]}function Xe(e){return H(Hr(e))?e.pop():void 0}function ke(e){return Yt(Hr(e))?e.pop():void 0}function Bt(e,t){return typeof Hr(e)=="number"?e.pop():t}var xt=function(e){return e&&typeof e.length=="number"&&typeof e!="function"};function Gt(e){return H(e==null?void 0:e.then)}function Jt(e){return H(e[bt])}function Xt(e){return Symbol.asyncIterator&&H(e==null?void 0:e[Symbol.asyncIterator])}function Zt(e){return new TypeError("You provided "+(e!==null&&typeof e=="object"?"an invalid object":"'"+e+"'")+" where a stream was expected. You can provide an Observable, Promise, ReadableStream, Array, AsyncIterable, or Iterable.")}function Zi(){return typeof Symbol!="function"||!Symbol.iterator?"@@iterator":Symbol.iterator}var er=Zi();function tr(e){return H(e==null?void 0:e[er])}function rr(e){return fo(this,arguments,function(){var r,o,n,i;return Nt(this,function(a){switch(a.label){case 0:r=e.getReader(),a.label=1;case 1:a.trys.push([1,,9,10]),a.label=2;case 2:return[4,nt(r.read())];case 3:return o=a.sent(),n=o.value,i=o.done,i?[4,nt(void 0)]:[3,5];case 4:return[2,a.sent()];case 5:return[4,nt(n)];case 6:return[4,a.sent()];case 7:return a.sent(),[3,2];case 8:return[3,10];case 9:return r.releaseLock(),[7];case 10:return[2]}})})}function or(e){return H(e==null?void 0:e.getReader)}function U(e){if(e instanceof j)return e;if(e!=null){if(Jt(e))return ea(e);if(xt(e))return ta(e);if(Gt(e))return ra(e);if(Xt(e))return Ao(e);if(tr(e))return oa(e);if(or(e))return na(e)}throw Zt(e)}function ea(e){return new j(function(t){var r=e[bt]();if(H(r.subscribe))return r.subscribe(t);throw new TypeError("Provided object does not correctly implement Symbol.observable")})}function ta(e){return new j(function(t){for(var r=0;r=2;return function(o){return o.pipe(e?b(function(n,i){return e(n,i,o)}):le,Te(1),r?De(t):Qo(function(){return new ir}))}}function jr(e){return e<=0?function(){return S}:E(function(t,r){var o=[];t.subscribe(T(r,function(n){o.push(n),e=2,!0))}function pe(e){e===void 0&&(e={});var t=e.connector,r=t===void 0?function(){return new g}:t,o=e.resetOnError,n=o===void 0?!0:o,i=e.resetOnComplete,a=i===void 0?!0:i,s=e.resetOnRefCountZero,p=s===void 0?!0:s;return function(c){var l,f,u,d=0,y=!1,L=!1,X=function(){f==null||f.unsubscribe(),f=void 0},te=function(){X(),l=u=void 0,y=L=!1},J=function(){var k=l;te(),k==null||k.unsubscribe()};return E(function(k,ft){d++,!L&&!y&&X();var qe=u=u!=null?u:r();ft.add(function(){d--,d===0&&!L&&!y&&(f=Ur(J,p))}),qe.subscribe(ft),!l&&d>0&&(l=new at({next:function(Fe){return qe.next(Fe)},error:function(Fe){L=!0,X(),f=Ur(te,n,Fe),qe.error(Fe)},complete:function(){y=!0,X(),f=Ur(te,a),qe.complete()}}),U(k).subscribe(l))})(c)}}function Ur(e,t){for(var r=[],o=2;oe.next(document)),e}function P(e,t=document){return Array.from(t.querySelectorAll(e))}function R(e,t=document){let r=fe(e,t);if(typeof r=="undefined")throw new ReferenceError(`Missing element: expected "${e}" to be present`);return r}function fe(e,t=document){return t.querySelector(e)||void 0}function Ie(){var e,t,r,o;return(o=(r=(t=(e=document.activeElement)==null?void 0:e.shadowRoot)==null?void 0:t.activeElement)!=null?r:document.activeElement)!=null?o:void 0}var wa=O(h(document.body,"focusin"),h(document.body,"focusout")).pipe(_e(1),Q(void 0),m(()=>Ie()||document.body),G(1));function et(e){return wa.pipe(m(t=>e.contains(t)),K())}function $t(e,t){return C(()=>O(h(e,"mouseenter").pipe(m(()=>!0)),h(e,"mouseleave").pipe(m(()=>!1))).pipe(t?Ht(r=>Le(+!r*t)):le,Q(e.matches(":hover"))))}function Jo(e,t){if(typeof t=="string"||typeof t=="number")e.innerHTML+=t.toString();else if(t instanceof Node)e.appendChild(t);else if(Array.isArray(t))for(let r of t)Jo(e,r)}function x(e,t,...r){let o=document.createElement(e);if(t)for(let n of Object.keys(t))typeof t[n]!="undefined"&&(typeof t[n]!="boolean"?o.setAttribute(n,t[n]):o.setAttribute(n,""));for(let n of r)Jo(o,n);return o}function sr(e){if(e>999){let t=+((e-950)%1e3>99);return`${((e+1e-6)/1e3).toFixed(t)}k`}else return e.toString()}function Tt(e){let t=x("script",{src:e});return C(()=>(document.head.appendChild(t),O(h(t,"load"),h(t,"error").pipe(v(()=>$r(()=>new ReferenceError(`Invalid script: ${e}`))))).pipe(m(()=>{}),_(()=>document.head.removeChild(t)),Te(1))))}var Xo=new g,Ta=C(()=>typeof ResizeObserver=="undefined"?Tt("https://unpkg.com/resize-observer-polyfill"):I(void 0)).pipe(m(()=>new ResizeObserver(e=>e.forEach(t=>Xo.next(t)))),v(e=>O(Ye,I(e)).pipe(_(()=>e.disconnect()))),G(1));function ce(e){return{width:e.offsetWidth,height:e.offsetHeight}}function ge(e){let t=e;for(;t.clientWidth===0&&t.parentElement;)t=t.parentElement;return Ta.pipe(w(r=>r.observe(t)),v(r=>Xo.pipe(b(o=>o.target===t),_(()=>r.unobserve(t)))),m(()=>ce(e)),Q(ce(e)))}function St(e){return{width:e.scrollWidth,height:e.scrollHeight}}function cr(e){let t=e.parentElement;for(;t&&(e.scrollWidth<=t.scrollWidth&&e.scrollHeight<=t.scrollHeight);)t=(e=t).parentElement;return t?e:void 0}function Zo(e){let t=[],r=e.parentElement;for(;r;)(e.clientWidth>r.clientWidth||e.clientHeight>r.clientHeight)&&t.push(r),r=(e=r).parentElement;return t.length===0&&t.push(document.documentElement),t}function Ve(e){return{x:e.offsetLeft,y:e.offsetTop}}function en(e){let t=e.getBoundingClientRect();return{x:t.x+window.scrollX,y:t.y+window.scrollY}}function tn(e){return O(h(window,"load"),h(window,"resize")).pipe(Me(0,me),m(()=>Ve(e)),Q(Ve(e)))}function pr(e){return{x:e.scrollLeft,y:e.scrollTop}}function Ne(e){return O(h(e,"scroll"),h(window,"scroll"),h(window,"resize")).pipe(Me(0,me),m(()=>pr(e)),Q(pr(e)))}var rn=new g,Sa=C(()=>I(new IntersectionObserver(e=>{for(let t of e)rn.next(t)},{threshold:0}))).pipe(v(e=>O(Ye,I(e)).pipe(_(()=>e.disconnect()))),G(1));function tt(e){return Sa.pipe(w(t=>t.observe(e)),v(t=>rn.pipe(b(({target:r})=>r===e),_(()=>t.unobserve(e)),m(({isIntersecting:r})=>r))))}function on(e,t=16){return Ne(e).pipe(m(({y:r})=>{let o=ce(e),n=St(e);return r>=n.height-o.height-t}),K())}var lr={drawer:R("[data-md-toggle=drawer]"),search:R("[data-md-toggle=search]")};function nn(e){return lr[e].checked}function Je(e,t){lr[e].checked!==t&&lr[e].click()}function ze(e){let t=lr[e];return h(t,"change").pipe(m(()=>t.checked),Q(t.checked))}function Oa(e,t){switch(e.constructor){case HTMLInputElement:return e.type==="radio"?/^Arrow/.test(t):!0;case HTMLSelectElement:case HTMLTextAreaElement:return!0;default:return e.isContentEditable}}function La(){return O(h(window,"compositionstart").pipe(m(()=>!0)),h(window,"compositionend").pipe(m(()=>!1))).pipe(Q(!1))}function an(){let e=h(window,"keydown").pipe(b(t=>!(t.metaKey||t.ctrlKey)),m(t=>({mode:nn("search")?"search":"global",type:t.key,claim(){t.preventDefault(),t.stopPropagation()}})),b(({mode:t,type:r})=>{if(t==="global"){let o=Ie();if(typeof o!="undefined")return!Oa(o,r)}return!0}),pe());return La().pipe(v(t=>t?S:e))}function ye(){return new URL(location.href)}function lt(e,t=!1){if(B("navigation.instant")&&!t){let r=x("a",{href:e.href});document.body.appendChild(r),r.click(),r.remove()}else location.href=e.href}function sn(){return new g}function cn(){return location.hash.slice(1)}function pn(e){let t=x("a",{href:e});t.addEventListener("click",r=>r.stopPropagation()),t.click()}function Ma(e){return O(h(window,"hashchange"),e).pipe(m(cn),Q(cn()),b(t=>t.length>0),G(1))}function ln(e){return Ma(e).pipe(m(t=>fe(`[id="${t}"]`)),b(t=>typeof t!="undefined"))}function Pt(e){let t=matchMedia(e);return ar(r=>t.addListener(()=>r(t.matches))).pipe(Q(t.matches))}function mn(){let e=matchMedia("print");return O(h(window,"beforeprint").pipe(m(()=>!0)),h(window,"afterprint").pipe(m(()=>!1))).pipe(Q(e.matches))}function Nr(e,t){return e.pipe(v(r=>r?t():S))}function zr(e,t){return new j(r=>{let o=new XMLHttpRequest;return o.open("GET",`${e}`),o.responseType="blob",o.addEventListener("load",()=>{o.status>=200&&o.status<300?(r.next(o.response),r.complete()):r.error(new Error(o.statusText))}),o.addEventListener("error",()=>{r.error(new Error("Network error"))}),o.addEventListener("abort",()=>{r.complete()}),typeof(t==null?void 0:t.progress$)!="undefined"&&(o.addEventListener("progress",n=>{var i;if(n.lengthComputable)t.progress$.next(n.loaded/n.total*100);else{let a=(i=o.getResponseHeader("Content-Length"))!=null?i:0;t.progress$.next(n.loaded/+a*100)}}),t.progress$.next(5)),o.send(),()=>o.abort()})}function je(e,t){return zr(e,t).pipe(v(r=>r.text()),m(r=>JSON.parse(r)),G(1))}function fn(e,t){let r=new DOMParser;return zr(e,t).pipe(v(o=>o.text()),m(o=>r.parseFromString(o,"text/html")),G(1))}function un(e,t){let r=new DOMParser;return zr(e,t).pipe(v(o=>o.text()),m(o=>r.parseFromString(o,"text/xml")),G(1))}function dn(){return{x:Math.max(0,scrollX),y:Math.max(0,scrollY)}}function hn(){return O(h(window,"scroll",{passive:!0}),h(window,"resize",{passive:!0})).pipe(m(dn),Q(dn()))}function bn(){return{width:innerWidth,height:innerHeight}}function vn(){return h(window,"resize",{passive:!0}).pipe(m(bn),Q(bn()))}function gn(){return z([hn(),vn()]).pipe(m(([e,t])=>({offset:e,size:t})),G(1))}function mr(e,{viewport$:t,header$:r}){let o=t.pipe(ee("size")),n=z([o,r]).pipe(m(()=>Ve(e)));return z([r,t,n]).pipe(m(([{height:i},{offset:a,size:s},{x:p,y:c}])=>({offset:{x:a.x-p,y:a.y-c+i},size:s})))}function _a(e){return h(e,"message",t=>t.data)}function Aa(e){let t=new g;return t.subscribe(r=>e.postMessage(r)),t}function yn(e,t=new Worker(e)){let r=_a(t),o=Aa(t),n=new g;n.subscribe(o);let i=o.pipe(Z(),ie(!0));return n.pipe(Z(),Re(r.pipe(W(i))),pe())}var Ca=R("#__config"),Ot=JSON.parse(Ca.textContent);Ot.base=`${new URL(Ot.base,ye())}`;function xe(){return Ot}function B(e){return Ot.features.includes(e)}function Ee(e,t){return typeof t!="undefined"?Ot.translations[e].replace("#",t.toString()):Ot.translations[e]}function Se(e,t=document){return R(`[data-md-component=${e}]`,t)}function ae(e,t=document){return P(`[data-md-component=${e}]`,t)}function ka(e){let t=R(".md-typeset > :first-child",e);return h(t,"click",{once:!0}).pipe(m(()=>R(".md-typeset",e)),m(r=>({hash:__md_hash(r.innerHTML)})))}function xn(e){if(!B("announce.dismiss")||!e.childElementCount)return S;if(!e.hidden){let t=R(".md-typeset",e);__md_hash(t.innerHTML)===__md_get("__announce")&&(e.hidden=!0)}return C(()=>{let t=new g;return t.subscribe(({hash:r})=>{e.hidden=!0,__md_set("__announce",r)}),ka(e).pipe(w(r=>t.next(r)),_(()=>t.complete()),m(r=>$({ref:e},r)))})}function Ha(e,{target$:t}){return t.pipe(m(r=>({hidden:r!==e})))}function En(e,t){let r=new g;return r.subscribe(({hidden:o})=>{e.hidden=o}),Ha(e,t).pipe(w(o=>r.next(o)),_(()=>r.complete()),m(o=>$({ref:e},o)))}function Rt(e,t){return t==="inline"?x("div",{class:"md-tooltip md-tooltip--inline",id:e,role:"tooltip"},x("div",{class:"md-tooltip__inner md-typeset"})):x("div",{class:"md-tooltip",id:e,role:"tooltip"},x("div",{class:"md-tooltip__inner md-typeset"}))}function wn(...e){return x("div",{class:"md-tooltip2",role:"tooltip"},x("div",{class:"md-tooltip2__inner md-typeset"},e))}function Tn(e,t){if(t=t?`${t}_annotation_${e}`:void 0,t){let r=t?`#${t}`:void 0;return x("aside",{class:"md-annotation",tabIndex:0},Rt(t),x("a",{href:r,class:"md-annotation__index",tabIndex:-1},x("span",{"data-md-annotation-id":e})))}else return x("aside",{class:"md-annotation",tabIndex:0},Rt(t),x("span",{class:"md-annotation__index",tabIndex:-1},x("span",{"data-md-annotation-id":e})))}function Sn(e){return x("button",{class:"md-clipboard md-icon",title:Ee("clipboard.copy"),"data-clipboard-target":`#${e} > code`})}var Ln=Mt(qr());function Qr(e,t){let r=t&2,o=t&1,n=Object.keys(e.terms).filter(p=>!e.terms[p]).reduce((p,c)=>[...p,x("del",null,(0,Ln.default)(c))," "],[]).slice(0,-1),i=xe(),a=new URL(e.location,i.base);B("search.highlight")&&a.searchParams.set("h",Object.entries(e.terms).filter(([,p])=>p).reduce((p,[c])=>`${p} ${c}`.trim(),""));let{tags:s}=xe();return x("a",{href:`${a}`,class:"md-search-result__link",tabIndex:-1},x("article",{class:"md-search-result__article md-typeset","data-md-score":e.score.toFixed(2)},r>0&&x("div",{class:"md-search-result__icon md-icon"}),r>0&&x("h1",null,e.title),r<=0&&x("h2",null,e.title),o>0&&e.text.length>0&&e.text,e.tags&&x("nav",{class:"md-tags"},e.tags.map(p=>{let c=s?p in s?`md-tag-icon md-tag--${s[p]}`:"md-tag-icon":"";return x("span",{class:`md-tag ${c}`},p)})),o>0&&n.length>0&&x("p",{class:"md-search-result__terms"},Ee("search.result.term.missing"),": ",...n)))}function Mn(e){let t=e[0].score,r=[...e],o=xe(),n=r.findIndex(l=>!`${new URL(l.location,o.base)}`.includes("#")),[i]=r.splice(n,1),a=r.findIndex(l=>l.scoreQr(l,1)),...p.length?[x("details",{class:"md-search-result__more"},x("summary",{tabIndex:-1},x("div",null,p.length>0&&p.length===1?Ee("search.result.more.one"):Ee("search.result.more.other",p.length))),...p.map(l=>Qr(l,1)))]:[]];return x("li",{class:"md-search-result__item"},c)}function _n(e){return x("ul",{class:"md-source__facts"},Object.entries(e).map(([t,r])=>x("li",{class:`md-source__fact md-source__fact--${t}`},typeof r=="number"?sr(r):r)))}function Kr(e){let t=`tabbed-control tabbed-control--${e}`;return x("div",{class:t,hidden:!0},x("button",{class:"tabbed-button",tabIndex:-1,"aria-hidden":"true"}))}function An(e){return x("div",{class:"md-typeset__scrollwrap"},x("div",{class:"md-typeset__table"},e))}function Ra(e){var o;let t=xe(),r=new URL(`../${e.version}/`,t.base);return x("li",{class:"md-version__item"},x("a",{href:`${r}`,class:"md-version__link"},e.title,((o=t.version)==null?void 0:o.alias)&&e.aliases.length>0&&x("span",{class:"md-version__alias"},e.aliases[0])))}function Cn(e,t){var o;let r=xe();return e=e.filter(n=>{var i;return!((i=n.properties)!=null&&i.hidden)}),x("div",{class:"md-version"},x("button",{class:"md-version__current","aria-label":Ee("select.version")},t.title,((o=r.version)==null?void 0:o.alias)&&t.aliases.length>0&&x("span",{class:"md-version__alias"},t.aliases[0])),x("ul",{class:"md-version__list"},e.map(Ra)))}var Ia=0;function ja(e){let t=z([et(e),$t(e)]).pipe(m(([o,n])=>o||n),K()),r=C(()=>Zo(e)).pipe(ne(Ne),pt(1),He(t),m(()=>en(e)));return t.pipe(Ae(o=>o),v(()=>z([t,r])),m(([o,n])=>({active:o,offset:n})),pe())}function Fa(e,t){let{content$:r,viewport$:o}=t,n=`__tooltip2_${Ia++}`;return C(()=>{let i=new g,a=new _r(!1);i.pipe(Z(),ie(!1)).subscribe(a);let s=a.pipe(Ht(c=>Le(+!c*250,kr)),K(),v(c=>c?r:S),w(c=>c.id=n),pe());z([i.pipe(m(({active:c})=>c)),s.pipe(v(c=>$t(c,250)),Q(!1))]).pipe(m(c=>c.some(l=>l))).subscribe(a);let p=a.pipe(b(c=>c),re(s,o),m(([c,l,{size:f}])=>{let u=e.getBoundingClientRect(),d=u.width/2;if(l.role==="tooltip")return{x:d,y:8+u.height};if(u.y>=f.height/2){let{height:y}=ce(l);return{x:d,y:-16-y}}else return{x:d,y:16+u.height}}));return z([s,i,p]).subscribe(([c,{offset:l},f])=>{c.style.setProperty("--md-tooltip-host-x",`${l.x}px`),c.style.setProperty("--md-tooltip-host-y",`${l.y}px`),c.style.setProperty("--md-tooltip-x",`${f.x}px`),c.style.setProperty("--md-tooltip-y",`${f.y}px`),c.classList.toggle("md-tooltip2--top",f.y<0),c.classList.toggle("md-tooltip2--bottom",f.y>=0)}),a.pipe(b(c=>c),re(s,(c,l)=>l),b(c=>c.role==="tooltip")).subscribe(c=>{let l=ce(R(":scope > *",c));c.style.setProperty("--md-tooltip-width",`${l.width}px`),c.style.setProperty("--md-tooltip-tail","0px")}),a.pipe(K(),ve(me),re(s)).subscribe(([c,l])=>{l.classList.toggle("md-tooltip2--active",c)}),z([a.pipe(b(c=>c)),s]).subscribe(([c,l])=>{l.role==="dialog"?(e.setAttribute("aria-controls",n),e.setAttribute("aria-haspopup","dialog")):e.setAttribute("aria-describedby",n)}),a.pipe(b(c=>!c)).subscribe(()=>{e.removeAttribute("aria-controls"),e.removeAttribute("aria-describedby"),e.removeAttribute("aria-haspopup")}),ja(e).pipe(w(c=>i.next(c)),_(()=>i.complete()),m(c=>$({ref:e},c)))})}function mt(e,{viewport$:t},r=document.body){return Fa(e,{content$:new j(o=>{let n=e.title,i=wn(n);return o.next(i),e.removeAttribute("title"),r.append(i),()=>{i.remove(),e.setAttribute("title",n)}}),viewport$:t})}function Ua(e,t){let r=C(()=>z([tn(e),Ne(t)])).pipe(m(([{x:o,y:n},i])=>{let{width:a,height:s}=ce(e);return{x:o-i.x+a/2,y:n-i.y+s/2}}));return et(e).pipe(v(o=>r.pipe(m(n=>({active:o,offset:n})),Te(+!o||1/0))))}function kn(e,t,{target$:r}){let[o,n]=Array.from(e.children);return C(()=>{let i=new g,a=i.pipe(Z(),ie(!0));return i.subscribe({next({offset:s}){e.style.setProperty("--md-tooltip-x",`${s.x}px`),e.style.setProperty("--md-tooltip-y",`${s.y}px`)},complete(){e.style.removeProperty("--md-tooltip-x"),e.style.removeProperty("--md-tooltip-y")}}),tt(e).pipe(W(a)).subscribe(s=>{e.toggleAttribute("data-md-visible",s)}),O(i.pipe(b(({active:s})=>s)),i.pipe(_e(250),b(({active:s})=>!s))).subscribe({next({active:s}){s?e.prepend(o):o.remove()},complete(){e.prepend(o)}}),i.pipe(Me(16,me)).subscribe(({active:s})=>{o.classList.toggle("md-tooltip--active",s)}),i.pipe(pt(125,me),b(()=>!!e.offsetParent),m(()=>e.offsetParent.getBoundingClientRect()),m(({x:s})=>s)).subscribe({next(s){s?e.style.setProperty("--md-tooltip-0",`${-s}px`):e.style.removeProperty("--md-tooltip-0")},complete(){e.style.removeProperty("--md-tooltip-0")}}),h(n,"click").pipe(W(a),b(s=>!(s.metaKey||s.ctrlKey))).subscribe(s=>{s.stopPropagation(),s.preventDefault()}),h(n,"mousedown").pipe(W(a),re(i)).subscribe(([s,{active:p}])=>{var c;if(s.button!==0||s.metaKey||s.ctrlKey)s.preventDefault();else if(p){s.preventDefault();let l=e.parentElement.closest(".md-annotation");l instanceof HTMLElement?l.focus():(c=Ie())==null||c.blur()}}),r.pipe(W(a),b(s=>s===o),Ge(125)).subscribe(()=>e.focus()),Ua(e,t).pipe(w(s=>i.next(s)),_(()=>i.complete()),m(s=>$({ref:e},s)))})}function Wa(e){return e.tagName==="CODE"?P(".c, .c1, .cm",e):[e]}function Da(e){let t=[];for(let r of Wa(e)){let o=[],n=document.createNodeIterator(r,NodeFilter.SHOW_TEXT);for(let i=n.nextNode();i;i=n.nextNode())o.push(i);for(let i of o){let a;for(;a=/(\(\d+\))(!)?/.exec(i.textContent);){let[,s,p]=a;if(typeof p=="undefined"){let c=i.splitText(a.index);i=c.splitText(s.length),t.push(c)}else{i.textContent=s,t.push(i);break}}}}return t}function Hn(e,t){t.append(...Array.from(e.childNodes))}function fr(e,t,{target$:r,print$:o}){let n=t.closest("[id]"),i=n==null?void 0:n.id,a=new Map;for(let s of Da(t)){let[,p]=s.textContent.match(/\((\d+)\)/);fe(`:scope > li:nth-child(${p})`,e)&&(a.set(p,Tn(p,i)),s.replaceWith(a.get(p)))}return a.size===0?S:C(()=>{let s=new g,p=s.pipe(Z(),ie(!0)),c=[];for(let[l,f]of a)c.push([R(".md-typeset",f),R(`:scope > li:nth-child(${l})`,e)]);return o.pipe(W(p)).subscribe(l=>{e.hidden=!l,e.classList.toggle("md-annotation-list",l);for(let[f,u]of c)l?Hn(f,u):Hn(u,f)}),O(...[...a].map(([,l])=>kn(l,t,{target$:r}))).pipe(_(()=>s.complete()),pe())})}function $n(e){if(e.nextElementSibling){let t=e.nextElementSibling;if(t.tagName==="OL")return t;if(t.tagName==="P"&&!t.children.length)return $n(t)}}function Pn(e,t){return C(()=>{let r=$n(e);return typeof r!="undefined"?fr(r,e,t):S})}var Rn=Mt(Br());var Va=0;function In(e){if(e.nextElementSibling){let t=e.nextElementSibling;if(t.tagName==="OL")return t;if(t.tagName==="P"&&!t.children.length)return In(t)}}function Na(e){return ge(e).pipe(m(({width:t})=>({scrollable:St(e).width>t})),ee("scrollable"))}function jn(e,t){let{matches:r}=matchMedia("(hover)"),o=C(()=>{let n=new g,i=n.pipe(jr(1));n.subscribe(({scrollable:c})=>{c&&r?e.setAttribute("tabindex","0"):e.removeAttribute("tabindex")});let a=[];if(Rn.default.isSupported()&&(e.closest(".copy")||B("content.code.copy")&&!e.closest(".no-copy"))){let c=e.closest("pre");c.id=`__code_${Va++}`;let l=Sn(c.id);c.insertBefore(l,e),B("content.tooltips")&&a.push(mt(l,{viewport$}))}let s=e.closest(".highlight");if(s instanceof HTMLElement){let c=In(s);if(typeof c!="undefined"&&(s.classList.contains("annotate")||B("content.code.annotate"))){let l=fr(c,e,t);a.push(ge(s).pipe(W(i),m(({width:f,height:u})=>f&&u),K(),v(f=>f?l:S)))}}return P(":scope > span[id]",e).length&&e.classList.add("md-code__content"),Na(e).pipe(w(c=>n.next(c)),_(()=>n.complete()),m(c=>$({ref:e},c)),Re(...a))});return B("content.lazy")?tt(e).pipe(b(n=>n),Te(1),v(()=>o)):o}function za(e,{target$:t,print$:r}){let o=!0;return O(t.pipe(m(n=>n.closest("details:not([open])")),b(n=>e===n),m(()=>({action:"open",reveal:!0}))),r.pipe(b(n=>n||!o),w(()=>o=e.open),m(n=>({action:n?"open":"close"}))))}function Fn(e,t){return C(()=>{let r=new g;return r.subscribe(({action:o,reveal:n})=>{e.toggleAttribute("open",o==="open"),n&&e.scrollIntoView()}),za(e,t).pipe(w(o=>r.next(o)),_(()=>r.complete()),m(o=>$({ref:e},o)))})}var Un=".node circle,.node ellipse,.node path,.node polygon,.node rect{fill:var(--md-mermaid-node-bg-color);stroke:var(--md-mermaid-node-fg-color)}marker{fill:var(--md-mermaid-edge-color)!important}.edgeLabel .label rect{fill:#0000}.label{color:var(--md-mermaid-label-fg-color);font-family:var(--md-mermaid-font-family)}.label foreignObject{line-height:normal;overflow:visible}.label div .edgeLabel{color:var(--md-mermaid-label-fg-color)}.edgeLabel,.edgeLabel p,.label div .edgeLabel{background-color:var(--md-mermaid-label-bg-color)}.edgeLabel,.edgeLabel p{fill:var(--md-mermaid-label-bg-color);color:var(--md-mermaid-edge-color)}.edgePath .path,.flowchart-link{stroke:var(--md-mermaid-edge-color);stroke-width:.05rem}.edgePath .arrowheadPath{fill:var(--md-mermaid-edge-color);stroke:none}.cluster rect{fill:var(--md-default-fg-color--lightest);stroke:var(--md-default-fg-color--lighter)}.cluster span{color:var(--md-mermaid-label-fg-color);font-family:var(--md-mermaid-font-family)}g #flowchart-circleEnd,g #flowchart-circleStart,g #flowchart-crossEnd,g #flowchart-crossStart,g #flowchart-pointEnd,g #flowchart-pointStart{stroke:none}g.classGroup line,g.classGroup rect{fill:var(--md-mermaid-node-bg-color);stroke:var(--md-mermaid-node-fg-color)}g.classGroup text{fill:var(--md-mermaid-label-fg-color);font-family:var(--md-mermaid-font-family)}.classLabel .box{fill:var(--md-mermaid-label-bg-color);background-color:var(--md-mermaid-label-bg-color);opacity:1}.classLabel .label{fill:var(--md-mermaid-label-fg-color);font-family:var(--md-mermaid-font-family)}.node .divider{stroke:var(--md-mermaid-node-fg-color)}.relation{stroke:var(--md-mermaid-edge-color)}.cardinality{fill:var(--md-mermaid-label-fg-color);font-family:var(--md-mermaid-font-family)}.cardinality text{fill:inherit!important}defs #classDiagram-compositionEnd,defs #classDiagram-compositionStart,defs #classDiagram-dependencyEnd,defs #classDiagram-dependencyStart,defs #classDiagram-extensionEnd,defs #classDiagram-extensionStart{fill:var(--md-mermaid-edge-color)!important;stroke:var(--md-mermaid-edge-color)!important}defs #classDiagram-aggregationEnd,defs #classDiagram-aggregationStart{fill:var(--md-mermaid-label-bg-color)!important;stroke:var(--md-mermaid-edge-color)!important}g.stateGroup rect{fill:var(--md-mermaid-node-bg-color);stroke:var(--md-mermaid-node-fg-color)}g.stateGroup .state-title{fill:var(--md-mermaid-label-fg-color)!important;font-family:var(--md-mermaid-font-family)}g.stateGroup .composit{fill:var(--md-mermaid-label-bg-color)}.nodeLabel,.nodeLabel p{color:var(--md-mermaid-label-fg-color);font-family:var(--md-mermaid-font-family)}a .nodeLabel{text-decoration:underline}.node circle.state-end,.node circle.state-start,.start-state{fill:var(--md-mermaid-edge-color);stroke:none}.end-state-inner,.end-state-outer{fill:var(--md-mermaid-edge-color)}.end-state-inner,.node circle.state-end{stroke:var(--md-mermaid-label-bg-color)}.transition{stroke:var(--md-mermaid-edge-color)}[id^=state-fork] rect,[id^=state-join] rect{fill:var(--md-mermaid-edge-color)!important;stroke:none!important}.statediagram-cluster.statediagram-cluster .inner{fill:var(--md-default-bg-color)}.statediagram-cluster rect{fill:var(--md-mermaid-node-bg-color);stroke:var(--md-mermaid-node-fg-color)}.statediagram-state rect.divider{fill:var(--md-default-fg-color--lightest);stroke:var(--md-default-fg-color--lighter)}defs #statediagram-barbEnd{stroke:var(--md-mermaid-edge-color)}.attributeBoxEven,.attributeBoxOdd{fill:var(--md-mermaid-node-bg-color);stroke:var(--md-mermaid-node-fg-color)}.entityBox{fill:var(--md-mermaid-label-bg-color);stroke:var(--md-mermaid-node-fg-color)}.entityLabel{fill:var(--md-mermaid-label-fg-color);font-family:var(--md-mermaid-font-family)}.relationshipLabelBox{fill:var(--md-mermaid-label-bg-color);fill-opacity:1;background-color:var(--md-mermaid-label-bg-color);opacity:1}.relationshipLabel{fill:var(--md-mermaid-label-fg-color)}.relationshipLine{stroke:var(--md-mermaid-edge-color)}defs #ONE_OR_MORE_END *,defs #ONE_OR_MORE_START *,defs #ONLY_ONE_END *,defs #ONLY_ONE_START *,defs #ZERO_OR_MORE_END *,defs #ZERO_OR_MORE_START *,defs #ZERO_OR_ONE_END *,defs #ZERO_OR_ONE_START *{stroke:var(--md-mermaid-edge-color)!important}defs #ZERO_OR_MORE_END circle,defs #ZERO_OR_MORE_START circle{fill:var(--md-mermaid-label-bg-color)}.actor{fill:var(--md-mermaid-sequence-actor-bg-color);stroke:var(--md-mermaid-sequence-actor-border-color)}text.actor>tspan{fill:var(--md-mermaid-sequence-actor-fg-color);font-family:var(--md-mermaid-font-family)}line{stroke:var(--md-mermaid-sequence-actor-line-color)}.actor-man circle,.actor-man line{fill:var(--md-mermaid-sequence-actorman-bg-color);stroke:var(--md-mermaid-sequence-actorman-line-color)}.messageLine0,.messageLine1{stroke:var(--md-mermaid-sequence-message-line-color)}.note{fill:var(--md-mermaid-sequence-note-bg-color);stroke:var(--md-mermaid-sequence-note-border-color)}.loopText,.loopText>tspan,.messageText,.noteText>tspan{stroke:none;font-family:var(--md-mermaid-font-family)!important}.messageText{fill:var(--md-mermaid-sequence-message-fg-color)}.loopText,.loopText>tspan{fill:var(--md-mermaid-sequence-loop-fg-color)}.noteText>tspan{fill:var(--md-mermaid-sequence-note-fg-color)}#arrowhead path{fill:var(--md-mermaid-sequence-message-line-color);stroke:none}.loopLine{fill:var(--md-mermaid-sequence-loop-bg-color);stroke:var(--md-mermaid-sequence-loop-border-color)}.labelBox{fill:var(--md-mermaid-sequence-label-bg-color);stroke:none}.labelText,.labelText>span{fill:var(--md-mermaid-sequence-label-fg-color);font-family:var(--md-mermaid-font-family)}.sequenceNumber{fill:var(--md-mermaid-sequence-number-fg-color)}rect.rect{fill:var(--md-mermaid-sequence-box-bg-color);stroke:none}rect.rect+text.text{fill:var(--md-mermaid-sequence-box-fg-color)}defs #sequencenumber{fill:var(--md-mermaid-sequence-number-bg-color)!important}";var Gr,Qa=0;function Ka(){return typeof mermaid=="undefined"||mermaid instanceof Element?Tt("https://unpkg.com/mermaid@11/dist/mermaid.min.js"):I(void 0)}function Wn(e){return e.classList.remove("mermaid"),Gr||(Gr=Ka().pipe(w(()=>mermaid.initialize({startOnLoad:!1,themeCSS:Un,sequence:{actorFontSize:"16px",messageFontSize:"16px",noteFontSize:"16px"}})),m(()=>{}),G(1))),Gr.subscribe(()=>co(this,null,function*(){e.classList.add("mermaid");let t=`__mermaid_${Qa++}`,r=x("div",{class:"mermaid"}),o=e.textContent,{svg:n,fn:i}=yield mermaid.render(t,o),a=r.attachShadow({mode:"closed"});a.innerHTML=n,e.replaceWith(r),i==null||i(a)})),Gr.pipe(m(()=>({ref:e})))}var Dn=x("table");function Vn(e){return e.replaceWith(Dn),Dn.replaceWith(An(e)),I({ref:e})}function Ya(e){let t=e.find(r=>r.checked)||e[0];return O(...e.map(r=>h(r,"change").pipe(m(()=>R(`label[for="${r.id}"]`))))).pipe(Q(R(`label[for="${t.id}"]`)),m(r=>({active:r})))}function Nn(e,{viewport$:t,target$:r}){let o=R(".tabbed-labels",e),n=P(":scope > input",e),i=Kr("prev");e.append(i);let a=Kr("next");return e.append(a),C(()=>{let s=new g,p=s.pipe(Z(),ie(!0));z([s,ge(e),tt(e)]).pipe(W(p),Me(1,me)).subscribe({next([{active:c},l]){let f=Ve(c),{width:u}=ce(c);e.style.setProperty("--md-indicator-x",`${f.x}px`),e.style.setProperty("--md-indicator-width",`${u}px`);let d=pr(o);(f.xd.x+l.width)&&o.scrollTo({left:Math.max(0,f.x-16),behavior:"smooth"})},complete(){e.style.removeProperty("--md-indicator-x"),e.style.removeProperty("--md-indicator-width")}}),z([Ne(o),ge(o)]).pipe(W(p)).subscribe(([c,l])=>{let f=St(o);i.hidden=c.x<16,a.hidden=c.x>f.width-l.width-16}),O(h(i,"click").pipe(m(()=>-1)),h(a,"click").pipe(m(()=>1))).pipe(W(p)).subscribe(c=>{let{width:l}=ce(o);o.scrollBy({left:l*c,behavior:"smooth"})}),r.pipe(W(p),b(c=>n.includes(c))).subscribe(c=>c.click()),o.classList.add("tabbed-labels--linked");for(let c of n){let l=R(`label[for="${c.id}"]`);l.replaceChildren(x("a",{href:`#${l.htmlFor}`,tabIndex:-1},...Array.from(l.childNodes))),h(l.firstElementChild,"click").pipe(W(p),b(f=>!(f.metaKey||f.ctrlKey)),w(f=>{f.preventDefault(),f.stopPropagation()})).subscribe(()=>{history.replaceState({},"",`#${l.htmlFor}`),l.click()})}return B("content.tabs.link")&&s.pipe(Ce(1),re(t)).subscribe(([{active:c},{offset:l}])=>{let f=c.innerText.trim();if(c.hasAttribute("data-md-switching"))c.removeAttribute("data-md-switching");else{let u=e.offsetTop-l.y;for(let y of P("[data-tabs]"))for(let L of P(":scope > input",y)){let X=R(`label[for="${L.id}"]`);if(X!==c&&X.innerText.trim()===f){X.setAttribute("data-md-switching",""),L.click();break}}window.scrollTo({top:e.offsetTop-u});let d=__md_get("__tabs")||[];__md_set("__tabs",[...new Set([f,...d])])}}),s.pipe(W(p)).subscribe(()=>{for(let c of P("audio, video",e))c.pause()}),Ya(n).pipe(w(c=>s.next(c)),_(()=>s.complete()),m(c=>$({ref:e},c)))}).pipe(Ke(se))}function zn(e,{viewport$:t,target$:r,print$:o}){return O(...P(".annotate:not(.highlight)",e).map(n=>Pn(n,{target$:r,print$:o})),...P("pre:not(.mermaid) > code",e).map(n=>jn(n,{target$:r,print$:o})),...P("pre.mermaid",e).map(n=>Wn(n)),...P("table:not([class])",e).map(n=>Vn(n)),...P("details",e).map(n=>Fn(n,{target$:r,print$:o})),...P("[data-tabs]",e).map(n=>Nn(n,{viewport$:t,target$:r})),...P("[title]",e).filter(()=>B("content.tooltips")).map(n=>mt(n,{viewport$:t})))}function Ba(e,{alert$:t}){return t.pipe(v(r=>O(I(!0),I(!1).pipe(Ge(2e3))).pipe(m(o=>({message:r,active:o})))))}function qn(e,t){let r=R(".md-typeset",e);return C(()=>{let o=new g;return o.subscribe(({message:n,active:i})=>{e.classList.toggle("md-dialog--active",i),r.textContent=n}),Ba(e,t).pipe(w(n=>o.next(n)),_(()=>o.complete()),m(n=>$({ref:e},n)))})}var Ga=0;function Ja(e,t){document.body.append(e);let{width:r}=ce(e);e.style.setProperty("--md-tooltip-width",`${r}px`),e.remove();let o=cr(t),n=typeof o!="undefined"?Ne(o):I({x:0,y:0}),i=O(et(t),$t(t)).pipe(K());return z([i,n]).pipe(m(([a,s])=>{let{x:p,y:c}=Ve(t),l=ce(t),f=t.closest("table");return f&&t.parentElement&&(p+=f.offsetLeft+t.parentElement.offsetLeft,c+=f.offsetTop+t.parentElement.offsetTop),{active:a,offset:{x:p-s.x+l.width/2-r/2,y:c-s.y+l.height+8}}}))}function Qn(e){let t=e.title;if(!t.length)return S;let r=`__tooltip_${Ga++}`,o=Rt(r,"inline"),n=R(".md-typeset",o);return n.innerHTML=t,C(()=>{let i=new g;return i.subscribe({next({offset:a}){o.style.setProperty("--md-tooltip-x",`${a.x}px`),o.style.setProperty("--md-tooltip-y",`${a.y}px`)},complete(){o.style.removeProperty("--md-tooltip-x"),o.style.removeProperty("--md-tooltip-y")}}),O(i.pipe(b(({active:a})=>a)),i.pipe(_e(250),b(({active:a})=>!a))).subscribe({next({active:a}){a?(e.insertAdjacentElement("afterend",o),e.setAttribute("aria-describedby",r),e.removeAttribute("title")):(o.remove(),e.removeAttribute("aria-describedby"),e.setAttribute("title",t))},complete(){o.remove(),e.removeAttribute("aria-describedby"),e.setAttribute("title",t)}}),i.pipe(Me(16,me)).subscribe(({active:a})=>{o.classList.toggle("md-tooltip--active",a)}),i.pipe(pt(125,me),b(()=>!!e.offsetParent),m(()=>e.offsetParent.getBoundingClientRect()),m(({x:a})=>a)).subscribe({next(a){a?o.style.setProperty("--md-tooltip-0",`${-a}px`):o.style.removeProperty("--md-tooltip-0")},complete(){o.style.removeProperty("--md-tooltip-0")}}),Ja(o,e).pipe(w(a=>i.next(a)),_(()=>i.complete()),m(a=>$({ref:e},a)))}).pipe(Ke(se))}function Xa({viewport$:e}){if(!B("header.autohide"))return I(!1);let t=e.pipe(m(({offset:{y:n}})=>n),Be(2,1),m(([n,i])=>[nMath.abs(i-n.y)>100),m(([,[n]])=>n),K()),o=ze("search");return z([e,o]).pipe(m(([{offset:n},i])=>n.y>400&&!i),K(),v(n=>n?r:I(!1)),Q(!1))}function Kn(e,t){return C(()=>z([ge(e),Xa(t)])).pipe(m(([{height:r},o])=>({height:r,hidden:o})),K((r,o)=>r.height===o.height&&r.hidden===o.hidden),G(1))}function Yn(e,{header$:t,main$:r}){return C(()=>{let o=new g,n=o.pipe(Z(),ie(!0));o.pipe(ee("active"),He(t)).subscribe(([{active:a},{hidden:s}])=>{e.classList.toggle("md-header--shadow",a&&!s),e.hidden=s});let i=ue(P("[title]",e)).pipe(b(()=>B("content.tooltips")),ne(a=>Qn(a)));return r.subscribe(o),t.pipe(W(n),m(a=>$({ref:e},a)),Re(i.pipe(W(n))))})}function Za(e,{viewport$:t,header$:r}){return mr(e,{viewport$:t,header$:r}).pipe(m(({offset:{y:o}})=>{let{height:n}=ce(e);return{active:o>=n}}),ee("active"))}function Bn(e,t){return C(()=>{let r=new g;r.subscribe({next({active:n}){e.classList.toggle("md-header__title--active",n)},complete(){e.classList.remove("md-header__title--active")}});let o=fe(".md-content h1");return typeof o=="undefined"?S:Za(o,t).pipe(w(n=>r.next(n)),_(()=>r.complete()),m(n=>$({ref:e},n)))})}function Gn(e,{viewport$:t,header$:r}){let o=r.pipe(m(({height:i})=>i),K()),n=o.pipe(v(()=>ge(e).pipe(m(({height:i})=>({top:e.offsetTop,bottom:e.offsetTop+i})),ee("bottom"))));return z([o,n,t]).pipe(m(([i,{top:a,bottom:s},{offset:{y:p},size:{height:c}}])=>(c=Math.max(0,c-Math.max(0,a-p,i)-Math.max(0,c+p-s)),{offset:a-i,height:c,active:a-i<=p})),K((i,a)=>i.offset===a.offset&&i.height===a.height&&i.active===a.active))}function es(e){let t=__md_get("__palette")||{index:e.findIndex(o=>matchMedia(o.getAttribute("data-md-color-media")).matches)},r=Math.max(0,Math.min(t.index,e.length-1));return I(...e).pipe(ne(o=>h(o,"change").pipe(m(()=>o))),Q(e[r]),m(o=>({index:e.indexOf(o),color:{media:o.getAttribute("data-md-color-media"),scheme:o.getAttribute("data-md-color-scheme"),primary:o.getAttribute("data-md-color-primary"),accent:o.getAttribute("data-md-color-accent")}})),G(1))}function Jn(e){let t=P("input",e),r=x("meta",{name:"theme-color"});document.head.appendChild(r);let o=x("meta",{name:"color-scheme"});document.head.appendChild(o);let n=Pt("(prefers-color-scheme: light)");return C(()=>{let i=new g;return i.subscribe(a=>{if(document.body.setAttribute("data-md-color-switching",""),a.color.media==="(prefers-color-scheme)"){let s=matchMedia("(prefers-color-scheme: light)"),p=document.querySelector(s.matches?"[data-md-color-media='(prefers-color-scheme: light)']":"[data-md-color-media='(prefers-color-scheme: dark)']");a.color.scheme=p.getAttribute("data-md-color-scheme"),a.color.primary=p.getAttribute("data-md-color-primary"),a.color.accent=p.getAttribute("data-md-color-accent")}for(let[s,p]of Object.entries(a.color))document.body.setAttribute(`data-md-color-${s}`,p);for(let s=0;sa.key==="Enter"),re(i,(a,s)=>s)).subscribe(({index:a})=>{a=(a+1)%t.length,t[a].click(),t[a].focus()}),i.pipe(m(()=>{let a=Se("header"),s=window.getComputedStyle(a);return o.content=s.colorScheme,s.backgroundColor.match(/\d+/g).map(p=>(+p).toString(16).padStart(2,"0")).join("")})).subscribe(a=>r.content=`#${a}`),i.pipe(ve(se)).subscribe(()=>{document.body.removeAttribute("data-md-color-switching")}),es(t).pipe(W(n.pipe(Ce(1))),ct(),w(a=>i.next(a)),_(()=>i.complete()),m(a=>$({ref:e},a)))})}function Xn(e,{progress$:t}){return C(()=>{let r=new g;return r.subscribe(({value:o})=>{e.style.setProperty("--md-progress-value",`${o}`)}),t.pipe(w(o=>r.next({value:o})),_(()=>r.complete()),m(o=>({ref:e,value:o})))})}var Jr=Mt(Br());function ts(e){e.setAttribute("data-md-copying","");let t=e.closest("[data-copy]"),r=t?t.getAttribute("data-copy"):e.innerText;return e.removeAttribute("data-md-copying"),r.trimEnd()}function Zn({alert$:e}){Jr.default.isSupported()&&new j(t=>{new Jr.default("[data-clipboard-target], [data-clipboard-text]",{text:r=>r.getAttribute("data-clipboard-text")||ts(R(r.getAttribute("data-clipboard-target")))}).on("success",r=>t.next(r))}).pipe(w(t=>{t.trigger.focus()}),m(()=>Ee("clipboard.copied"))).subscribe(e)}function ei(e,t){return e.protocol=t.protocol,e.hostname=t.hostname,e}function rs(e,t){let r=new Map;for(let o of P("url",e)){let n=R("loc",o),i=[ei(new URL(n.textContent),t)];r.set(`${i[0]}`,i);for(let a of P("[rel=alternate]",o)){let s=a.getAttribute("href");s!=null&&i.push(ei(new URL(s),t))}}return r}function ur(e){return un(new URL("sitemap.xml",e)).pipe(m(t=>rs(t,new URL(e))),de(()=>I(new Map)))}function os(e,t){if(!(e.target instanceof Element))return S;let r=e.target.closest("a");if(r===null)return S;if(r.target||e.metaKey||e.ctrlKey)return S;let o=new URL(r.href);return o.search=o.hash="",t.has(`${o}`)?(e.preventDefault(),I(new URL(r.href))):S}function ti(e){let t=new Map;for(let r of P(":scope > *",e.head))t.set(r.outerHTML,r);return t}function ri(e){for(let t of P("[href], [src]",e))for(let r of["href","src"]){let o=t.getAttribute(r);if(o&&!/^(?:[a-z]+:)?\/\//i.test(o)){t[r]=t[r];break}}return I(e)}function ns(e){for(let o of["[data-md-component=announce]","[data-md-component=container]","[data-md-component=header-topic]","[data-md-component=outdated]","[data-md-component=logo]","[data-md-component=skip]",...B("navigation.tabs.sticky")?["[data-md-component=tabs]"]:[]]){let n=fe(o),i=fe(o,e);typeof n!="undefined"&&typeof i!="undefined"&&n.replaceWith(i)}let t=ti(document);for(let[o,n]of ti(e))t.has(o)?t.delete(o):document.head.appendChild(n);for(let o of t.values()){let n=o.getAttribute("name");n!=="theme-color"&&n!=="color-scheme"&&o.remove()}let r=Se("container");return We(P("script",r)).pipe(v(o=>{let n=e.createElement("script");if(o.src){for(let i of o.getAttributeNames())n.setAttribute(i,o.getAttribute(i));return o.replaceWith(n),new j(i=>{n.onload=()=>i.complete()})}else return n.textContent=o.textContent,o.replaceWith(n),S}),Z(),ie(document))}function oi({location$:e,viewport$:t,progress$:r}){let o=xe();if(location.protocol==="file:")return S;let n=ur(o.base);I(document).subscribe(ri);let i=h(document.body,"click").pipe(He(n),v(([p,c])=>os(p,c)),pe()),a=h(window,"popstate").pipe(m(ye),pe());i.pipe(re(t)).subscribe(([p,{offset:c}])=>{history.replaceState(c,""),history.pushState(null,"",p)}),O(i,a).subscribe(e);let s=e.pipe(ee("pathname"),v(p=>fn(p,{progress$:r}).pipe(de(()=>(lt(p,!0),S)))),v(ri),v(ns),pe());return O(s.pipe(re(e,(p,c)=>c)),s.pipe(v(()=>e),ee("pathname"),v(()=>e),ee("hash")),e.pipe(K((p,c)=>p.pathname===c.pathname&&p.hash===c.hash),v(()=>i),w(()=>history.back()))).subscribe(p=>{var c,l;history.state!==null||!p.hash?window.scrollTo(0,(l=(c=history.state)==null?void 0:c.y)!=null?l:0):(history.scrollRestoration="auto",pn(p.hash),history.scrollRestoration="manual")}),e.subscribe(()=>{history.scrollRestoration="manual"}),h(window,"beforeunload").subscribe(()=>{history.scrollRestoration="auto"}),t.pipe(ee("offset"),_e(100)).subscribe(({offset:p})=>{history.replaceState(p,"")}),s}var ni=Mt(qr());function ii(e){let t=e.separator.split("|").map(n=>n.replace(/(\(\?[!=<][^)]+\))/g,"").length===0?"\uFFFD":n).join("|"),r=new RegExp(t,"img"),o=(n,i,a)=>`${i}${a}`;return n=>{n=n.replace(/[\s*+\-:~^]+/g," ").trim();let i=new RegExp(`(^|${e.separator}|)(${n.replace(/[|\\{}()[\]^$+*?.-]/g,"\\$&").replace(r,"|")})`,"img");return a=>(0,ni.default)(a).replace(i,o).replace(/<\/mark>(\s+)]*>/img,"$1")}}function jt(e){return e.type===1}function dr(e){return e.type===3}function ai(e,t){let r=yn(e);return O(I(location.protocol!=="file:"),ze("search")).pipe(Ae(o=>o),v(()=>t)).subscribe(({config:o,docs:n})=>r.next({type:0,data:{config:o,docs:n,options:{suggest:B("search.suggest")}}})),r}function si(e){var l;let{selectedVersionSitemap:t,selectedVersionBaseURL:r,currentLocation:o,currentBaseURL:n}=e,i=(l=Xr(n))==null?void 0:l.pathname;if(i===void 0)return;let a=ss(o.pathname,i);if(a===void 0)return;let s=ps(t.keys());if(!t.has(s))return;let p=Xr(a,s);if(!p||!t.has(p.href))return;let c=Xr(a,r);if(c)return c.hash=o.hash,c.search=o.search,c}function Xr(e,t){try{return new URL(e,t)}catch(r){return}}function ss(e,t){if(e.startsWith(t))return e.slice(t.length)}function cs(e,t){let r=Math.min(e.length,t.length),o;for(o=0;oS)),o=r.pipe(m(n=>{let[,i]=t.base.match(/([^/]+)\/?$/);return n.find(({version:a,aliases:s})=>a===i||s.includes(i))||n[0]}));r.pipe(m(n=>new Map(n.map(i=>[`${new URL(`../${i.version}/`,t.base)}`,i]))),v(n=>h(document.body,"click").pipe(b(i=>!i.metaKey&&!i.ctrlKey),re(o),v(([i,a])=>{if(i.target instanceof Element){let s=i.target.closest("a");if(s&&!s.target&&n.has(s.href)){let p=s.href;return!i.target.closest(".md-version")&&n.get(p)===a?S:(i.preventDefault(),I(new URL(p)))}}return S}),v(i=>ur(i).pipe(m(a=>{var s;return(s=si({selectedVersionSitemap:a,selectedVersionBaseURL:i,currentLocation:ye(),currentBaseURL:t.base}))!=null?s:i})))))).subscribe(n=>lt(n,!0)),z([r,o]).subscribe(([n,i])=>{R(".md-header__topic").appendChild(Cn(n,i))}),e.pipe(v(()=>o)).subscribe(n=>{var a;let i=__md_get("__outdated",sessionStorage);if(i===null){i=!0;let s=((a=t.version)==null?void 0:a.default)||"latest";Array.isArray(s)||(s=[s]);e:for(let p of s)for(let c of n.aliases.concat(n.version))if(new RegExp(p,"i").test(c)){i=!1;break e}__md_set("__outdated",i,sessionStorage)}if(i)for(let s of ae("outdated"))s.hidden=!1})}function ls(e,{worker$:t}){let{searchParams:r}=ye();r.has("q")&&(Je("search",!0),e.value=r.get("q"),e.focus(),ze("search").pipe(Ae(i=>!i)).subscribe(()=>{let i=ye();i.searchParams.delete("q"),history.replaceState({},"",`${i}`)}));let o=et(e),n=O(t.pipe(Ae(jt)),h(e,"keyup"),o).pipe(m(()=>e.value),K());return z([n,o]).pipe(m(([i,a])=>({value:i,focus:a})),G(1))}function pi(e,{worker$:t}){let r=new g,o=r.pipe(Z(),ie(!0));z([t.pipe(Ae(jt)),r],(i,a)=>a).pipe(ee("value")).subscribe(({value:i})=>t.next({type:2,data:i})),r.pipe(ee("focus")).subscribe(({focus:i})=>{i&&Je("search",i)}),h(e.form,"reset").pipe(W(o)).subscribe(()=>e.focus());let n=R("header [for=__search]");return h(n,"click").subscribe(()=>e.focus()),ls(e,{worker$:t}).pipe(w(i=>r.next(i)),_(()=>r.complete()),m(i=>$({ref:e},i)),G(1))}function li(e,{worker$:t,query$:r}){let o=new g,n=on(e.parentElement).pipe(b(Boolean)),i=e.parentElement,a=R(":scope > :first-child",e),s=R(":scope > :last-child",e);ze("search").subscribe(l=>s.setAttribute("role",l?"list":"presentation")),o.pipe(re(r),Wr(t.pipe(Ae(jt)))).subscribe(([{items:l},{value:f}])=>{switch(l.length){case 0:a.textContent=f.length?Ee("search.result.none"):Ee("search.result.placeholder");break;case 1:a.textContent=Ee("search.result.one");break;default:let u=sr(l.length);a.textContent=Ee("search.result.other",u)}});let p=o.pipe(w(()=>s.innerHTML=""),v(({items:l})=>O(I(...l.slice(0,10)),I(...l.slice(10)).pipe(Be(4),Vr(n),v(([f])=>f)))),m(Mn),pe());return p.subscribe(l=>s.appendChild(l)),p.pipe(ne(l=>{let f=fe("details",l);return typeof f=="undefined"?S:h(f,"toggle").pipe(W(o),m(()=>f))})).subscribe(l=>{l.open===!1&&l.offsetTop<=i.scrollTop&&i.scrollTo({top:l.offsetTop})}),t.pipe(b(dr),m(({data:l})=>l)).pipe(w(l=>o.next(l)),_(()=>o.complete()),m(l=>$({ref:e},l)))}function ms(e,{query$:t}){return t.pipe(m(({value:r})=>{let o=ye();return o.hash="",r=r.replace(/\s+/g,"+").replace(/&/g,"%26").replace(/=/g,"%3D"),o.search=`q=${r}`,{url:o}}))}function mi(e,t){let r=new g,o=r.pipe(Z(),ie(!0));return r.subscribe(({url:n})=>{e.setAttribute("data-clipboard-text",e.href),e.href=`${n}`}),h(e,"click").pipe(W(o)).subscribe(n=>n.preventDefault()),ms(e,t).pipe(w(n=>r.next(n)),_(()=>r.complete()),m(n=>$({ref:e},n)))}function fi(e,{worker$:t,keyboard$:r}){let o=new g,n=Se("search-query"),i=O(h(n,"keydown"),h(n,"focus")).pipe(ve(se),m(()=>n.value),K());return o.pipe(He(i),m(([{suggest:s},p])=>{let c=p.split(/([\s-]+)/);if(s!=null&&s.length&&c[c.length-1]){let l=s[s.length-1];l.startsWith(c[c.length-1])&&(c[c.length-1]=l)}else c.length=0;return c})).subscribe(s=>e.innerHTML=s.join("").replace(/\s/g," ")),r.pipe(b(({mode:s})=>s==="search")).subscribe(s=>{switch(s.type){case"ArrowRight":e.innerText.length&&n.selectionStart===n.value.length&&(n.value=e.innerText);break}}),t.pipe(b(dr),m(({data:s})=>s)).pipe(w(s=>o.next(s)),_(()=>o.complete()),m(()=>({ref:e})))}function ui(e,{index$:t,keyboard$:r}){let o=xe();try{let n=ai(o.search,t),i=Se("search-query",e),a=Se("search-result",e);h(e,"click").pipe(b(({target:p})=>p instanceof Element&&!!p.closest("a"))).subscribe(()=>Je("search",!1)),r.pipe(b(({mode:p})=>p==="search")).subscribe(p=>{let c=Ie();switch(p.type){case"Enter":if(c===i){let l=new Map;for(let f of P(":first-child [href]",a)){let u=f.firstElementChild;l.set(f,parseFloat(u.getAttribute("data-md-score")))}if(l.size){let[[f]]=[...l].sort(([,u],[,d])=>d-u);f.click()}p.claim()}break;case"Escape":case"Tab":Je("search",!1),i.blur();break;case"ArrowUp":case"ArrowDown":if(typeof c=="undefined")i.focus();else{let l=[i,...P(":not(details) > [href], summary, details[open] [href]",a)],f=Math.max(0,(Math.max(0,l.indexOf(c))+l.length+(p.type==="ArrowUp"?-1:1))%l.length);l[f].focus()}p.claim();break;default:i!==Ie()&&i.focus()}}),r.pipe(b(({mode:p})=>p==="global")).subscribe(p=>{switch(p.type){case"f":case"s":case"/":i.focus(),i.select(),p.claim();break}});let s=pi(i,{worker$:n});return O(s,li(a,{worker$:n,query$:s})).pipe(Re(...ae("search-share",e).map(p=>mi(p,{query$:s})),...ae("search-suggest",e).map(p=>fi(p,{worker$:n,keyboard$:r}))))}catch(n){return e.hidden=!0,Ye}}function di(e,{index$:t,location$:r}){return z([t,r.pipe(Q(ye()),b(o=>!!o.searchParams.get("h")))]).pipe(m(([o,n])=>ii(o.config)(n.searchParams.get("h"))),m(o=>{var a;let n=new Map,i=document.createNodeIterator(e,NodeFilter.SHOW_TEXT);for(let s=i.nextNode();s;s=i.nextNode())if((a=s.parentElement)!=null&&a.offsetHeight){let p=s.textContent,c=o(p);c.length>p.length&&n.set(s,c)}for(let[s,p]of n){let{childNodes:c}=x("span",null,p);s.replaceWith(...Array.from(c))}return{ref:e,nodes:n}}))}function fs(e,{viewport$:t,main$:r}){let o=e.closest(".md-grid"),n=o.offsetTop-o.parentElement.offsetTop;return z([r,t]).pipe(m(([{offset:i,height:a},{offset:{y:s}}])=>(a=a+Math.min(n,Math.max(0,s-i))-n,{height:a,locked:s>=i+n})),K((i,a)=>i.height===a.height&&i.locked===a.locked))}function Zr(e,o){var n=o,{header$:t}=n,r=so(n,["header$"]);let i=R(".md-sidebar__scrollwrap",e),{y:a}=Ve(i);return C(()=>{let s=new g,p=s.pipe(Z(),ie(!0)),c=s.pipe(Me(0,me));return c.pipe(re(t)).subscribe({next([{height:l},{height:f}]){i.style.height=`${l-2*a}px`,e.style.top=`${f}px`},complete(){i.style.height="",e.style.top=""}}),c.pipe(Ae()).subscribe(()=>{for(let l of P(".md-nav__link--active[href]",e)){if(!l.clientHeight)continue;let f=l.closest(".md-sidebar__scrollwrap");if(typeof f!="undefined"){let u=l.offsetTop-f.offsetTop,{height:d}=ce(f);f.scrollTo({top:u-d/2})}}}),ue(P("label[tabindex]",e)).pipe(ne(l=>h(l,"click").pipe(ve(se),m(()=>l),W(p)))).subscribe(l=>{let f=R(`[id="${l.htmlFor}"]`);R(`[aria-labelledby="${l.id}"]`).setAttribute("aria-expanded",`${f.checked}`)}),fs(e,r).pipe(w(l=>s.next(l)),_(()=>s.complete()),m(l=>$({ref:e},l)))})}function hi(e,t){if(typeof t!="undefined"){let r=`https://api.github.com/repos/${e}/${t}`;return st(je(`${r}/releases/latest`).pipe(de(()=>S),m(o=>({version:o.tag_name})),De({})),je(r).pipe(de(()=>S),m(o=>({stars:o.stargazers_count,forks:o.forks_count})),De({}))).pipe(m(([o,n])=>$($({},o),n)))}else{let r=`https://api.github.com/users/${e}`;return je(r).pipe(m(o=>({repositories:o.public_repos})),De({}))}}function bi(e,t){let r=`https://${e}/api/v4/projects/${encodeURIComponent(t)}`;return st(je(`${r}/releases/permalink/latest`).pipe(de(()=>S),m(({tag_name:o})=>({version:o})),De({})),je(r).pipe(de(()=>S),m(({star_count:o,forks_count:n})=>({stars:o,forks:n})),De({}))).pipe(m(([o,n])=>$($({},o),n)))}function vi(e){let t=e.match(/^.+github\.com\/([^/]+)\/?([^/]+)?/i);if(t){let[,r,o]=t;return hi(r,o)}if(t=e.match(/^.+?([^/]*gitlab[^/]+)\/(.+?)\/?$/i),t){let[,r,o]=t;return bi(r,o)}return S}var us;function ds(e){return us||(us=C(()=>{let t=__md_get("__source",sessionStorage);if(t)return I(t);if(ae("consent").length){let o=__md_get("__consent");if(!(o&&o.github))return S}return vi(e.href).pipe(w(o=>__md_set("__source",o,sessionStorage)))}).pipe(de(()=>S),b(t=>Object.keys(t).length>0),m(t=>({facts:t})),G(1)))}function gi(e){let t=R(":scope > :last-child",e);return C(()=>{let r=new g;return r.subscribe(({facts:o})=>{t.appendChild(_n(o)),t.classList.add("md-source__repository--active")}),ds(e).pipe(w(o=>r.next(o)),_(()=>r.complete()),m(o=>$({ref:e},o)))})}function hs(e,{viewport$:t,header$:r}){return ge(document.body).pipe(v(()=>mr(e,{header$:r,viewport$:t})),m(({offset:{y:o}})=>({hidden:o>=10})),ee("hidden"))}function yi(e,t){return C(()=>{let r=new g;return r.subscribe({next({hidden:o}){e.hidden=o},complete(){e.hidden=!1}}),(B("navigation.tabs.sticky")?I({hidden:!1}):hs(e,t)).pipe(w(o=>r.next(o)),_(()=>r.complete()),m(o=>$({ref:e},o)))})}function bs(e,{viewport$:t,header$:r}){let o=new Map,n=P(".md-nav__link",e);for(let s of n){let p=decodeURIComponent(s.hash.substring(1)),c=fe(`[id="${p}"]`);typeof c!="undefined"&&o.set(s,c)}let i=r.pipe(ee("height"),m(({height:s})=>{let p=Se("main"),c=R(":scope > :first-child",p);return s+.8*(c.offsetTop-p.offsetTop)}),pe());return ge(document.body).pipe(ee("height"),v(s=>C(()=>{let p=[];return I([...o].reduce((c,[l,f])=>{for(;p.length&&o.get(p[p.length-1]).tagName>=f.tagName;)p.pop();let u=f.offsetTop;for(;!u&&f.parentElement;)f=f.parentElement,u=f.offsetTop;let d=f.offsetParent;for(;d;d=d.offsetParent)u+=d.offsetTop;return c.set([...p=[...p,l]].reverse(),u)},new Map))}).pipe(m(p=>new Map([...p].sort(([,c],[,l])=>c-l))),He(i),v(([p,c])=>t.pipe(Fr(([l,f],{offset:{y:u},size:d})=>{let y=u+d.height>=Math.floor(s.height);for(;f.length;){let[,L]=f[0];if(L-c=u&&!y)f=[l.pop(),...f];else break}return[l,f]},[[],[...p]]),K((l,f)=>l[0]===f[0]&&l[1]===f[1])))))).pipe(m(([s,p])=>({prev:s.map(([c])=>c),next:p.map(([c])=>c)})),Q({prev:[],next:[]}),Be(2,1),m(([s,p])=>s.prev.length{let i=new g,a=i.pipe(Z(),ie(!0));if(i.subscribe(({prev:s,next:p})=>{for(let[c]of p)c.classList.remove("md-nav__link--passed"),c.classList.remove("md-nav__link--active");for(let[c,[l]]of s.entries())l.classList.add("md-nav__link--passed"),l.classList.toggle("md-nav__link--active",c===s.length-1)}),B("toc.follow")){let s=O(t.pipe(_e(1),m(()=>{})),t.pipe(_e(250),m(()=>"smooth")));i.pipe(b(({prev:p})=>p.length>0),He(o.pipe(ve(se))),re(s)).subscribe(([[{prev:p}],c])=>{let[l]=p[p.length-1];if(l.offsetHeight){let f=cr(l);if(typeof f!="undefined"){let u=l.offsetTop-f.offsetTop,{height:d}=ce(f);f.scrollTo({top:u-d/2,behavior:c})}}})}return B("navigation.tracking")&&t.pipe(W(a),ee("offset"),_e(250),Ce(1),W(n.pipe(Ce(1))),ct({delay:250}),re(i)).subscribe(([,{prev:s}])=>{let p=ye(),c=s[s.length-1];if(c&&c.length){let[l]=c,{hash:f}=new URL(l.href);p.hash!==f&&(p.hash=f,history.replaceState({},"",`${p}`))}else p.hash="",history.replaceState({},"",`${p}`)}),bs(e,{viewport$:t,header$:r}).pipe(w(s=>i.next(s)),_(()=>i.complete()),m(s=>$({ref:e},s)))})}function vs(e,{viewport$:t,main$:r,target$:o}){let n=t.pipe(m(({offset:{y:a}})=>a),Be(2,1),m(([a,s])=>a>s&&s>0),K()),i=r.pipe(m(({active:a})=>a));return z([i,n]).pipe(m(([a,s])=>!(a&&s)),K(),W(o.pipe(Ce(1))),ie(!0),ct({delay:250}),m(a=>({hidden:a})))}function Ei(e,{viewport$:t,header$:r,main$:o,target$:n}){let i=new g,a=i.pipe(Z(),ie(!0));return i.subscribe({next({hidden:s}){e.hidden=s,s?(e.setAttribute("tabindex","-1"),e.blur()):e.removeAttribute("tabindex")},complete(){e.style.top="",e.hidden=!0,e.removeAttribute("tabindex")}}),r.pipe(W(a),ee("height")).subscribe(({height:s})=>{e.style.top=`${s+16}px`}),h(e,"click").subscribe(s=>{s.preventDefault(),window.scrollTo({top:0})}),vs(e,{viewport$:t,main$:o,target$:n}).pipe(w(s=>i.next(s)),_(()=>i.complete()),m(s=>$({ref:e},s)))}function wi({document$:e,viewport$:t}){e.pipe(v(()=>P(".md-ellipsis")),ne(r=>tt(r).pipe(W(e.pipe(Ce(1))),b(o=>o),m(()=>r),Te(1))),b(r=>r.offsetWidth{let o=r.innerText,n=r.closest("a")||r;return n.title=o,B("content.tooltips")?mt(n,{viewport$:t}).pipe(W(e.pipe(Ce(1))),_(()=>n.removeAttribute("title"))):S})).subscribe(),B("content.tooltips")&&e.pipe(v(()=>P(".md-status")),ne(r=>mt(r,{viewport$:t}))).subscribe()}function Ti({document$:e,tablet$:t}){e.pipe(v(()=>P(".md-toggle--indeterminate")),w(r=>{r.indeterminate=!0,r.checked=!1}),ne(r=>h(r,"change").pipe(Dr(()=>r.classList.contains("md-toggle--indeterminate")),m(()=>r))),re(t)).subscribe(([r,o])=>{r.classList.remove("md-toggle--indeterminate"),o&&(r.checked=!1)})}function gs(){return/(iPad|iPhone|iPod)/.test(navigator.userAgent)}function Si({document$:e}){e.pipe(v(()=>P("[data-md-scrollfix]")),w(t=>t.removeAttribute("data-md-scrollfix")),b(gs),ne(t=>h(t,"touchstart").pipe(m(()=>t)))).subscribe(t=>{let r=t.scrollTop;r===0?t.scrollTop=1:r+t.offsetHeight===t.scrollHeight&&(t.scrollTop=r-1)})}function Oi({viewport$:e,tablet$:t}){z([ze("search"),t]).pipe(m(([r,o])=>r&&!o),v(r=>I(r).pipe(Ge(r?400:100))),re(e)).subscribe(([r,{offset:{y:o}}])=>{if(r)document.body.setAttribute("data-md-scrolllock",""),document.body.style.top=`-${o}px`;else{let n=-1*parseInt(document.body.style.top,10);document.body.removeAttribute("data-md-scrolllock"),document.body.style.top="",n&&window.scrollTo(0,n)}})}Object.entries||(Object.entries=function(e){let t=[];for(let r of Object.keys(e))t.push([r,e[r]]);return t});Object.values||(Object.values=function(e){let t=[];for(let r of Object.keys(e))t.push(e[r]);return t});typeof Element!="undefined"&&(Element.prototype.scrollTo||(Element.prototype.scrollTo=function(e,t){typeof e=="object"?(this.scrollLeft=e.left,this.scrollTop=e.top):(this.scrollLeft=e,this.scrollTop=t)}),Element.prototype.replaceWith||(Element.prototype.replaceWith=function(...e){let t=this.parentNode;if(t){e.length===0&&t.removeChild(this);for(let r=e.length-1;r>=0;r--){let o=e[r];typeof o=="string"?o=document.createTextNode(o):o.parentNode&&o.parentNode.removeChild(o),r?t.insertBefore(this.previousSibling,o):t.replaceChild(o,this)}}}));function ys(){return location.protocol==="file:"?Tt(`${new URL("search/search_index.js",eo.base)}`).pipe(m(()=>__index),G(1)):je(new URL("search/search_index.json",eo.base))}document.documentElement.classList.remove("no-js");document.documentElement.classList.add("js");var ot=Go(),Ut=sn(),Lt=ln(Ut),to=an(),Oe=gn(),hr=Pt("(min-width: 960px)"),Mi=Pt("(min-width: 1220px)"),_i=mn(),eo=xe(),Ai=document.forms.namedItem("search")?ys():Ye,ro=new g;Zn({alert$:ro});var oo=new g;B("navigation.instant")&&oi({location$:Ut,viewport$:Oe,progress$:oo}).subscribe(ot);var Li;((Li=eo.version)==null?void 0:Li.provider)==="mike"&&ci({document$:ot});O(Ut,Lt).pipe(Ge(125)).subscribe(()=>{Je("drawer",!1),Je("search",!1)});to.pipe(b(({mode:e})=>e==="global")).subscribe(e=>{switch(e.type){case"p":case",":let t=fe("link[rel=prev]");typeof t!="undefined"&<(t);break;case"n":case".":let r=fe("link[rel=next]");typeof r!="undefined"&<(r);break;case"Enter":let o=Ie();o instanceof HTMLLabelElement&&o.click()}});wi({viewport$:Oe,document$:ot});Ti({document$:ot,tablet$:hr});Si({document$:ot});Oi({viewport$:Oe,tablet$:hr});var rt=Kn(Se("header"),{viewport$:Oe}),Ft=ot.pipe(m(()=>Se("main")),v(e=>Gn(e,{viewport$:Oe,header$:rt})),G(1)),xs=O(...ae("consent").map(e=>En(e,{target$:Lt})),...ae("dialog").map(e=>qn(e,{alert$:ro})),...ae("palette").map(e=>Jn(e)),...ae("progress").map(e=>Xn(e,{progress$:oo})),...ae("search").map(e=>ui(e,{index$:Ai,keyboard$:to})),...ae("source").map(e=>gi(e))),Es=C(()=>O(...ae("announce").map(e=>xn(e)),...ae("content").map(e=>zn(e,{viewport$:Oe,target$:Lt,print$:_i})),...ae("content").map(e=>B("search.highlight")?di(e,{index$:Ai,location$:Ut}):S),...ae("header").map(e=>Yn(e,{viewport$:Oe,header$:rt,main$:Ft})),...ae("header-title").map(e=>Bn(e,{viewport$:Oe,header$:rt})),...ae("sidebar").map(e=>e.getAttribute("data-md-type")==="navigation"?Nr(Mi,()=>Zr(e,{viewport$:Oe,header$:rt,main$:Ft})):Nr(hr,()=>Zr(e,{viewport$:Oe,header$:rt,main$:Ft}))),...ae("tabs").map(e=>yi(e,{viewport$:Oe,header$:rt})),...ae("toc").map(e=>xi(e,{viewport$:Oe,header$:rt,main$:Ft,target$:Lt})),...ae("top").map(e=>Ei(e,{viewport$:Oe,header$:rt,main$:Ft,target$:Lt})))),Ci=ot.pipe(v(()=>Es),Re(xs),G(1));Ci.subscribe();window.document$=ot;window.location$=Ut;window.target$=Lt;window.keyboard$=to;window.viewport$=Oe;window.tablet$=hr;window.screen$=Mi;window.print$=_i;window.alert$=ro;window.progress$=oo;window.component$=Ci;})(); +//# sourceMappingURL=bundle.83f73b43.min.js.map + diff --git a/assets/javascripts/bundle.83f73b43.min.js.map b/assets/javascripts/bundle.83f73b43.min.js.map new file mode 100644 index 00000000..fe920b7d --- /dev/null +++ b/assets/javascripts/bundle.83f73b43.min.js.map @@ -0,0 +1,7 @@ +{ + "version": 3, + "sources": ["node_modules/focus-visible/dist/focus-visible.js", "node_modules/escape-html/index.js", "node_modules/clipboard/dist/clipboard.js", "src/templates/assets/javascripts/bundle.ts", "node_modules/tslib/tslib.es6.mjs", "node_modules/rxjs/src/internal/util/isFunction.ts", "node_modules/rxjs/src/internal/util/createErrorClass.ts", "node_modules/rxjs/src/internal/util/UnsubscriptionError.ts", "node_modules/rxjs/src/internal/util/arrRemove.ts", "node_modules/rxjs/src/internal/Subscription.ts", "node_modules/rxjs/src/internal/config.ts", "node_modules/rxjs/src/internal/scheduler/timeoutProvider.ts", "node_modules/rxjs/src/internal/util/reportUnhandledError.ts", "node_modules/rxjs/src/internal/util/noop.ts", "node_modules/rxjs/src/internal/NotificationFactories.ts", "node_modules/rxjs/src/internal/util/errorContext.ts", "node_modules/rxjs/src/internal/Subscriber.ts", "node_modules/rxjs/src/internal/symbol/observable.ts", "node_modules/rxjs/src/internal/util/identity.ts", "node_modules/rxjs/src/internal/util/pipe.ts", "node_modules/rxjs/src/internal/Observable.ts", "node_modules/rxjs/src/internal/util/lift.ts", "node_modules/rxjs/src/internal/operators/OperatorSubscriber.ts", "node_modules/rxjs/src/internal/scheduler/animationFrameProvider.ts", "node_modules/rxjs/src/internal/util/ObjectUnsubscribedError.ts", "node_modules/rxjs/src/internal/Subject.ts", "node_modules/rxjs/src/internal/BehaviorSubject.ts", "node_modules/rxjs/src/internal/scheduler/dateTimestampProvider.ts", "node_modules/rxjs/src/internal/ReplaySubject.ts", "node_modules/rxjs/src/internal/scheduler/Action.ts", "node_modules/rxjs/src/internal/scheduler/intervalProvider.ts", "node_modules/rxjs/src/internal/scheduler/AsyncAction.ts", "node_modules/rxjs/src/internal/Scheduler.ts", "node_modules/rxjs/src/internal/scheduler/AsyncScheduler.ts", "node_modules/rxjs/src/internal/scheduler/async.ts", "node_modules/rxjs/src/internal/scheduler/QueueAction.ts", "node_modules/rxjs/src/internal/scheduler/QueueScheduler.ts", "node_modules/rxjs/src/internal/scheduler/queue.ts", "node_modules/rxjs/src/internal/scheduler/AnimationFrameAction.ts", "node_modules/rxjs/src/internal/scheduler/AnimationFrameScheduler.ts", "node_modules/rxjs/src/internal/scheduler/animationFrame.ts", "node_modules/rxjs/src/internal/observable/empty.ts", "node_modules/rxjs/src/internal/util/isScheduler.ts", "node_modules/rxjs/src/internal/util/args.ts", "node_modules/rxjs/src/internal/util/isArrayLike.ts", "node_modules/rxjs/src/internal/util/isPromise.ts", "node_modules/rxjs/src/internal/util/isInteropObservable.ts", "node_modules/rxjs/src/internal/util/isAsyncIterable.ts", "node_modules/rxjs/src/internal/util/throwUnobservableError.ts", "node_modules/rxjs/src/internal/symbol/iterator.ts", "node_modules/rxjs/src/internal/util/isIterable.ts", "node_modules/rxjs/src/internal/util/isReadableStreamLike.ts", "node_modules/rxjs/src/internal/observable/innerFrom.ts", "node_modules/rxjs/src/internal/util/executeSchedule.ts", "node_modules/rxjs/src/internal/operators/observeOn.ts", "node_modules/rxjs/src/internal/operators/subscribeOn.ts", "node_modules/rxjs/src/internal/scheduled/scheduleObservable.ts", "node_modules/rxjs/src/internal/scheduled/schedulePromise.ts", "node_modules/rxjs/src/internal/scheduled/scheduleArray.ts", "node_modules/rxjs/src/internal/scheduled/scheduleIterable.ts", "node_modules/rxjs/src/internal/scheduled/scheduleAsyncIterable.ts", "node_modules/rxjs/src/internal/scheduled/scheduleReadableStreamLike.ts", "node_modules/rxjs/src/internal/scheduled/scheduled.ts", "node_modules/rxjs/src/internal/observable/from.ts", "node_modules/rxjs/src/internal/observable/of.ts", "node_modules/rxjs/src/internal/observable/throwError.ts", "node_modules/rxjs/src/internal/util/EmptyError.ts", "node_modules/rxjs/src/internal/util/isDate.ts", "node_modules/rxjs/src/internal/operators/map.ts", "node_modules/rxjs/src/internal/util/mapOneOrManyArgs.ts", "node_modules/rxjs/src/internal/util/argsArgArrayOrObject.ts", "node_modules/rxjs/src/internal/util/createObject.ts", "node_modules/rxjs/src/internal/observable/combineLatest.ts", "node_modules/rxjs/src/internal/operators/mergeInternals.ts", "node_modules/rxjs/src/internal/operators/mergeMap.ts", "node_modules/rxjs/src/internal/operators/mergeAll.ts", "node_modules/rxjs/src/internal/operators/concatAll.ts", "node_modules/rxjs/src/internal/observable/concat.ts", "node_modules/rxjs/src/internal/observable/defer.ts", "node_modules/rxjs/src/internal/observable/fromEvent.ts", "node_modules/rxjs/src/internal/observable/fromEventPattern.ts", "node_modules/rxjs/src/internal/observable/timer.ts", "node_modules/rxjs/src/internal/observable/merge.ts", "node_modules/rxjs/src/internal/observable/never.ts", "node_modules/rxjs/src/internal/util/argsOrArgArray.ts", "node_modules/rxjs/src/internal/operators/filter.ts", "node_modules/rxjs/src/internal/observable/zip.ts", "node_modules/rxjs/src/internal/operators/audit.ts", "node_modules/rxjs/src/internal/operators/auditTime.ts", "node_modules/rxjs/src/internal/operators/bufferCount.ts", "node_modules/rxjs/src/internal/operators/catchError.ts", "node_modules/rxjs/src/internal/operators/scanInternals.ts", "node_modules/rxjs/src/internal/operators/combineLatest.ts", "node_modules/rxjs/src/internal/operators/combineLatestWith.ts", "node_modules/rxjs/src/internal/operators/debounce.ts", "node_modules/rxjs/src/internal/operators/debounceTime.ts", "node_modules/rxjs/src/internal/operators/defaultIfEmpty.ts", "node_modules/rxjs/src/internal/operators/take.ts", "node_modules/rxjs/src/internal/operators/ignoreElements.ts", "node_modules/rxjs/src/internal/operators/mapTo.ts", "node_modules/rxjs/src/internal/operators/delayWhen.ts", "node_modules/rxjs/src/internal/operators/delay.ts", "node_modules/rxjs/src/internal/operators/distinctUntilChanged.ts", "node_modules/rxjs/src/internal/operators/distinctUntilKeyChanged.ts", "node_modules/rxjs/src/internal/operators/throwIfEmpty.ts", "node_modules/rxjs/src/internal/operators/endWith.ts", "node_modules/rxjs/src/internal/operators/finalize.ts", "node_modules/rxjs/src/internal/operators/first.ts", "node_modules/rxjs/src/internal/operators/takeLast.ts", "node_modules/rxjs/src/internal/operators/merge.ts", "node_modules/rxjs/src/internal/operators/mergeWith.ts", "node_modules/rxjs/src/internal/operators/repeat.ts", "node_modules/rxjs/src/internal/operators/scan.ts", "node_modules/rxjs/src/internal/operators/share.ts", "node_modules/rxjs/src/internal/operators/shareReplay.ts", "node_modules/rxjs/src/internal/operators/skip.ts", "node_modules/rxjs/src/internal/operators/skipUntil.ts", "node_modules/rxjs/src/internal/operators/startWith.ts", "node_modules/rxjs/src/internal/operators/switchMap.ts", "node_modules/rxjs/src/internal/operators/takeUntil.ts", "node_modules/rxjs/src/internal/operators/takeWhile.ts", "node_modules/rxjs/src/internal/operators/tap.ts", "node_modules/rxjs/src/internal/operators/throttle.ts", "node_modules/rxjs/src/internal/operators/throttleTime.ts", "node_modules/rxjs/src/internal/operators/withLatestFrom.ts", "node_modules/rxjs/src/internal/operators/zip.ts", "node_modules/rxjs/src/internal/operators/zipWith.ts", "src/templates/assets/javascripts/browser/document/index.ts", "src/templates/assets/javascripts/browser/element/_/index.ts", "src/templates/assets/javascripts/browser/element/focus/index.ts", "src/templates/assets/javascripts/browser/element/hover/index.ts", "src/templates/assets/javascripts/utilities/h/index.ts", "src/templates/assets/javascripts/utilities/round/index.ts", "src/templates/assets/javascripts/browser/script/index.ts", "src/templates/assets/javascripts/browser/element/size/_/index.ts", "src/templates/assets/javascripts/browser/element/size/content/index.ts", "src/templates/assets/javascripts/browser/element/offset/_/index.ts", "src/templates/assets/javascripts/browser/element/offset/content/index.ts", "src/templates/assets/javascripts/browser/element/visibility/index.ts", "src/templates/assets/javascripts/browser/toggle/index.ts", "src/templates/assets/javascripts/browser/keyboard/index.ts", "src/templates/assets/javascripts/browser/location/_/index.ts", "src/templates/assets/javascripts/browser/location/hash/index.ts", "src/templates/assets/javascripts/browser/media/index.ts", "src/templates/assets/javascripts/browser/request/index.ts", "src/templates/assets/javascripts/browser/viewport/offset/index.ts", "src/templates/assets/javascripts/browser/viewport/size/index.ts", "src/templates/assets/javascripts/browser/viewport/_/index.ts", "src/templates/assets/javascripts/browser/viewport/at/index.ts", "src/templates/assets/javascripts/browser/worker/index.ts", "src/templates/assets/javascripts/_/index.ts", "src/templates/assets/javascripts/components/_/index.ts", "src/templates/assets/javascripts/components/announce/index.ts", "src/templates/assets/javascripts/components/consent/index.ts", "src/templates/assets/javascripts/templates/tooltip/index.tsx", "src/templates/assets/javascripts/templates/annotation/index.tsx", "src/templates/assets/javascripts/templates/clipboard/index.tsx", "src/templates/assets/javascripts/templates/search/index.tsx", "src/templates/assets/javascripts/templates/source/index.tsx", "src/templates/assets/javascripts/templates/tabbed/index.tsx", "src/templates/assets/javascripts/templates/table/index.tsx", "src/templates/assets/javascripts/templates/version/index.tsx", "src/templates/assets/javascripts/components/tooltip2/index.ts", "src/templates/assets/javascripts/components/content/annotation/_/index.ts", "src/templates/assets/javascripts/components/content/annotation/list/index.ts", "src/templates/assets/javascripts/components/content/annotation/block/index.ts", "src/templates/assets/javascripts/components/content/code/_/index.ts", "src/templates/assets/javascripts/components/content/details/index.ts", "src/templates/assets/javascripts/components/content/mermaid/index.css", "src/templates/assets/javascripts/components/content/mermaid/index.ts", "src/templates/assets/javascripts/components/content/table/index.ts", "src/templates/assets/javascripts/components/content/tabs/index.ts", "src/templates/assets/javascripts/components/content/_/index.ts", "src/templates/assets/javascripts/components/dialog/index.ts", "src/templates/assets/javascripts/components/tooltip/index.ts", "src/templates/assets/javascripts/components/header/_/index.ts", "src/templates/assets/javascripts/components/header/title/index.ts", "src/templates/assets/javascripts/components/main/index.ts", "src/templates/assets/javascripts/components/palette/index.ts", "src/templates/assets/javascripts/components/progress/index.ts", "src/templates/assets/javascripts/integrations/clipboard/index.ts", "src/templates/assets/javascripts/integrations/sitemap/index.ts", "src/templates/assets/javascripts/integrations/instant/index.ts", "src/templates/assets/javascripts/integrations/search/highlighter/index.ts", "src/templates/assets/javascripts/integrations/search/worker/message/index.ts", "src/templates/assets/javascripts/integrations/search/worker/_/index.ts", "src/templates/assets/javascripts/integrations/version/findurl/index.ts", "src/templates/assets/javascripts/integrations/version/index.ts", "src/templates/assets/javascripts/components/search/query/index.ts", "src/templates/assets/javascripts/components/search/result/index.ts", "src/templates/assets/javascripts/components/search/share/index.ts", "src/templates/assets/javascripts/components/search/suggest/index.ts", "src/templates/assets/javascripts/components/search/_/index.ts", "src/templates/assets/javascripts/components/search/highlight/index.ts", "src/templates/assets/javascripts/components/sidebar/index.ts", "src/templates/assets/javascripts/components/source/facts/github/index.ts", "src/templates/assets/javascripts/components/source/facts/gitlab/index.ts", "src/templates/assets/javascripts/components/source/facts/_/index.ts", "src/templates/assets/javascripts/components/source/_/index.ts", "src/templates/assets/javascripts/components/tabs/index.ts", "src/templates/assets/javascripts/components/toc/index.ts", "src/templates/assets/javascripts/components/top/index.ts", "src/templates/assets/javascripts/patches/ellipsis/index.ts", "src/templates/assets/javascripts/patches/indeterminate/index.ts", "src/templates/assets/javascripts/patches/scrollfix/index.ts", "src/templates/assets/javascripts/patches/scrolllock/index.ts", "src/templates/assets/javascripts/polyfills/index.ts"], + "sourcesContent": ["(function (global, factory) {\n typeof exports === 'object' && typeof module !== 'undefined' ? factory() :\n typeof define === 'function' && define.amd ? define(factory) :\n (factory());\n}(this, (function () { 'use strict';\n\n /**\n * Applies the :focus-visible polyfill at the given scope.\n * A scope in this case is either the top-level Document or a Shadow Root.\n *\n * @param {(Document|ShadowRoot)} scope\n * @see https://github.com/WICG/focus-visible\n */\n function applyFocusVisiblePolyfill(scope) {\n var hadKeyboardEvent = true;\n var hadFocusVisibleRecently = false;\n var hadFocusVisibleRecentlyTimeout = null;\n\n var inputTypesAllowlist = {\n text: true,\n search: true,\n url: true,\n tel: true,\n email: true,\n password: true,\n number: true,\n date: true,\n month: true,\n week: true,\n time: true,\n datetime: true,\n 'datetime-local': true\n };\n\n /**\n * Helper function for legacy browsers and iframes which sometimes focus\n * elements like document, body, and non-interactive SVG.\n * @param {Element} el\n */\n function isValidFocusTarget(el) {\n if (\n el &&\n el !== document &&\n el.nodeName !== 'HTML' &&\n el.nodeName !== 'BODY' &&\n 'classList' in el &&\n 'contains' in el.classList\n ) {\n return true;\n }\n return false;\n }\n\n /**\n * Computes whether the given element should automatically trigger the\n * `focus-visible` class being added, i.e. whether it should always match\n * `:focus-visible` when focused.\n * @param {Element} el\n * @return {boolean}\n */\n function focusTriggersKeyboardModality(el) {\n var type = el.type;\n var tagName = el.tagName;\n\n if (tagName === 'INPUT' && inputTypesAllowlist[type] && !el.readOnly) {\n return true;\n }\n\n if (tagName === 'TEXTAREA' && !el.readOnly) {\n return true;\n }\n\n if (el.isContentEditable) {\n return true;\n }\n\n return false;\n }\n\n /**\n * Add the `focus-visible` class to the given element if it was not added by\n * the author.\n * @param {Element} el\n */\n function addFocusVisibleClass(el) {\n if (el.classList.contains('focus-visible')) {\n return;\n }\n el.classList.add('focus-visible');\n el.setAttribute('data-focus-visible-added', '');\n }\n\n /**\n * Remove the `focus-visible` class from the given element if it was not\n * originally added by the author.\n * @param {Element} el\n */\n function removeFocusVisibleClass(el) {\n if (!el.hasAttribute('data-focus-visible-added')) {\n return;\n }\n el.classList.remove('focus-visible');\n el.removeAttribute('data-focus-visible-added');\n }\n\n /**\n * If the most recent user interaction was via the keyboard;\n * and the key press did not include a meta, alt/option, or control key;\n * then the modality is keyboard. Otherwise, the modality is not keyboard.\n * Apply `focus-visible` to any current active element and keep track\n * of our keyboard modality state with `hadKeyboardEvent`.\n * @param {KeyboardEvent} e\n */\n function onKeyDown(e) {\n if (e.metaKey || e.altKey || e.ctrlKey) {\n return;\n }\n\n if (isValidFocusTarget(scope.activeElement)) {\n addFocusVisibleClass(scope.activeElement);\n }\n\n hadKeyboardEvent = true;\n }\n\n /**\n * If at any point a user clicks with a pointing device, ensure that we change\n * the modality away from keyboard.\n * This avoids the situation where a user presses a key on an already focused\n * element, and then clicks on a different element, focusing it with a\n * pointing device, while we still think we're in keyboard modality.\n * @param {Event} e\n */\n function onPointerDown(e) {\n hadKeyboardEvent = false;\n }\n\n /**\n * On `focus`, add the `focus-visible` class to the target if:\n * - the target received focus as a result of keyboard navigation, or\n * - the event target is an element that will likely require interaction\n * via the keyboard (e.g. a text box)\n * @param {Event} e\n */\n function onFocus(e) {\n // Prevent IE from focusing the document or HTML element.\n if (!isValidFocusTarget(e.target)) {\n return;\n }\n\n if (hadKeyboardEvent || focusTriggersKeyboardModality(e.target)) {\n addFocusVisibleClass(e.target);\n }\n }\n\n /**\n * On `blur`, remove the `focus-visible` class from the target.\n * @param {Event} e\n */\n function onBlur(e) {\n if (!isValidFocusTarget(e.target)) {\n return;\n }\n\n if (\n e.target.classList.contains('focus-visible') ||\n e.target.hasAttribute('data-focus-visible-added')\n ) {\n // To detect a tab/window switch, we look for a blur event followed\n // rapidly by a visibility change.\n // If we don't see a visibility change within 100ms, it's probably a\n // regular focus change.\n hadFocusVisibleRecently = true;\n window.clearTimeout(hadFocusVisibleRecentlyTimeout);\n hadFocusVisibleRecentlyTimeout = window.setTimeout(function() {\n hadFocusVisibleRecently = false;\n }, 100);\n removeFocusVisibleClass(e.target);\n }\n }\n\n /**\n * If the user changes tabs, keep track of whether or not the previously\n * focused element had .focus-visible.\n * @param {Event} e\n */\n function onVisibilityChange(e) {\n if (document.visibilityState === 'hidden') {\n // If the tab becomes active again, the browser will handle calling focus\n // on the element (Safari actually calls it twice).\n // If this tab change caused a blur on an element with focus-visible,\n // re-apply the class when the user switches back to the tab.\n if (hadFocusVisibleRecently) {\n hadKeyboardEvent = true;\n }\n addInitialPointerMoveListeners();\n }\n }\n\n /**\n * Add a group of listeners to detect usage of any pointing devices.\n * These listeners will be added when the polyfill first loads, and anytime\n * the window is blurred, so that they are active when the window regains\n * focus.\n */\n function addInitialPointerMoveListeners() {\n document.addEventListener('mousemove', onInitialPointerMove);\n document.addEventListener('mousedown', onInitialPointerMove);\n document.addEventListener('mouseup', onInitialPointerMove);\n document.addEventListener('pointermove', onInitialPointerMove);\n document.addEventListener('pointerdown', onInitialPointerMove);\n document.addEventListener('pointerup', onInitialPointerMove);\n document.addEventListener('touchmove', onInitialPointerMove);\n document.addEventListener('touchstart', onInitialPointerMove);\n document.addEventListener('touchend', onInitialPointerMove);\n }\n\n function removeInitialPointerMoveListeners() {\n document.removeEventListener('mousemove', onInitialPointerMove);\n document.removeEventListener('mousedown', onInitialPointerMove);\n document.removeEventListener('mouseup', onInitialPointerMove);\n document.removeEventListener('pointermove', onInitialPointerMove);\n document.removeEventListener('pointerdown', onInitialPointerMove);\n document.removeEventListener('pointerup', onInitialPointerMove);\n document.removeEventListener('touchmove', onInitialPointerMove);\n document.removeEventListener('touchstart', onInitialPointerMove);\n document.removeEventListener('touchend', onInitialPointerMove);\n }\n\n /**\n * When the polfyill first loads, assume the user is in keyboard modality.\n * If any event is received from a pointing device (e.g. mouse, pointer,\n * touch), turn off keyboard modality.\n * This accounts for situations where focus enters the page from the URL bar.\n * @param {Event} e\n */\n function onInitialPointerMove(e) {\n // Work around a Safari quirk that fires a mousemove on whenever the\n // window blurs, even if you're tabbing out of the page. \u00AF\\_(\u30C4)_/\u00AF\n if (e.target.nodeName && e.target.nodeName.toLowerCase() === 'html') {\n return;\n }\n\n hadKeyboardEvent = false;\n removeInitialPointerMoveListeners();\n }\n\n // For some kinds of state, we are interested in changes at the global scope\n // only. For example, global pointer input, global key presses and global\n // visibility change should affect the state at every scope:\n document.addEventListener('keydown', onKeyDown, true);\n document.addEventListener('mousedown', onPointerDown, true);\n document.addEventListener('pointerdown', onPointerDown, true);\n document.addEventListener('touchstart', onPointerDown, true);\n document.addEventListener('visibilitychange', onVisibilityChange, true);\n\n addInitialPointerMoveListeners();\n\n // For focus and blur, we specifically care about state changes in the local\n // scope. This is because focus / blur events that originate from within a\n // shadow root are not re-dispatched from the host element if it was already\n // the active element in its own scope:\n scope.addEventListener('focus', onFocus, true);\n scope.addEventListener('blur', onBlur, true);\n\n // We detect that a node is a ShadowRoot by ensuring that it is a\n // DocumentFragment and also has a host property. This check covers native\n // implementation and polyfill implementation transparently. If we only cared\n // about the native implementation, we could just check if the scope was\n // an instance of a ShadowRoot.\n if (scope.nodeType === Node.DOCUMENT_FRAGMENT_NODE && scope.host) {\n // Since a ShadowRoot is a special kind of DocumentFragment, it does not\n // have a root element to add a class to. So, we add this attribute to the\n // host element instead:\n scope.host.setAttribute('data-js-focus-visible', '');\n } else if (scope.nodeType === Node.DOCUMENT_NODE) {\n document.documentElement.classList.add('js-focus-visible');\n document.documentElement.setAttribute('data-js-focus-visible', '');\n }\n }\n\n // It is important to wrap all references to global window and document in\n // these checks to support server-side rendering use cases\n // @see https://github.com/WICG/focus-visible/issues/199\n if (typeof window !== 'undefined' && typeof document !== 'undefined') {\n // Make the polyfill helper globally available. This can be used as a signal\n // to interested libraries that wish to coordinate with the polyfill for e.g.,\n // applying the polyfill to a shadow root:\n window.applyFocusVisiblePolyfill = applyFocusVisiblePolyfill;\n\n // Notify interested libraries of the polyfill's presence, in case the\n // polyfill was loaded lazily:\n var event;\n\n try {\n event = new CustomEvent('focus-visible-polyfill-ready');\n } catch (error) {\n // IE11 does not support using CustomEvent as a constructor directly:\n event = document.createEvent('CustomEvent');\n event.initCustomEvent('focus-visible-polyfill-ready', false, false, {});\n }\n\n window.dispatchEvent(event);\n }\n\n if (typeof document !== 'undefined') {\n // Apply the polyfill to the global document, so that no JavaScript\n // coordination is required to use the polyfill in the top-level document:\n applyFocusVisiblePolyfill(document);\n }\n\n})));\n", "/*!\n * escape-html\n * Copyright(c) 2012-2013 TJ Holowaychuk\n * Copyright(c) 2015 Andreas Lubbe\n * Copyright(c) 2015 Tiancheng \"Timothy\" Gu\n * MIT Licensed\n */\n\n'use strict';\n\n/**\n * Module variables.\n * @private\n */\n\nvar matchHtmlRegExp = /[\"'&<>]/;\n\n/**\n * Module exports.\n * @public\n */\n\nmodule.exports = escapeHtml;\n\n/**\n * Escape special characters in the given string of html.\n *\n * @param {string} string The string to escape for inserting into HTML\n * @return {string}\n * @public\n */\n\nfunction escapeHtml(string) {\n var str = '' + string;\n var match = matchHtmlRegExp.exec(str);\n\n if (!match) {\n return str;\n }\n\n var escape;\n var html = '';\n var index = 0;\n var lastIndex = 0;\n\n for (index = match.index; index < str.length; index++) {\n switch (str.charCodeAt(index)) {\n case 34: // \"\n escape = '"';\n break;\n case 38: // &\n escape = '&';\n break;\n case 39: // '\n escape = ''';\n break;\n case 60: // <\n escape = '<';\n break;\n case 62: // >\n escape = '>';\n break;\n default:\n continue;\n }\n\n if (lastIndex !== index) {\n html += str.substring(lastIndex, index);\n }\n\n lastIndex = index + 1;\n html += escape;\n }\n\n return lastIndex !== index\n ? html + str.substring(lastIndex, index)\n : html;\n}\n", "/*!\n * clipboard.js v2.0.11\n * https://clipboardjs.com/\n *\n * Licensed MIT \u00A9 Zeno Rocha\n */\n(function webpackUniversalModuleDefinition(root, factory) {\n\tif(typeof exports === 'object' && typeof module === 'object')\n\t\tmodule.exports = factory();\n\telse if(typeof define === 'function' && define.amd)\n\t\tdefine([], factory);\n\telse if(typeof exports === 'object')\n\t\texports[\"ClipboardJS\"] = factory();\n\telse\n\t\troot[\"ClipboardJS\"] = factory();\n})(this, function() {\nreturn /******/ (function() { // webpackBootstrap\n/******/ \tvar __webpack_modules__ = ({\n\n/***/ 686:\n/***/ (function(__unused_webpack_module, __webpack_exports__, __webpack_require__) {\n\n\"use strict\";\n\n// EXPORTS\n__webpack_require__.d(__webpack_exports__, {\n \"default\": function() { return /* binding */ clipboard; }\n});\n\n// EXTERNAL MODULE: ./node_modules/tiny-emitter/index.js\nvar tiny_emitter = __webpack_require__(279);\nvar tiny_emitter_default = /*#__PURE__*/__webpack_require__.n(tiny_emitter);\n// EXTERNAL MODULE: ./node_modules/good-listener/src/listen.js\nvar listen = __webpack_require__(370);\nvar listen_default = /*#__PURE__*/__webpack_require__.n(listen);\n// EXTERNAL MODULE: ./node_modules/select/src/select.js\nvar src_select = __webpack_require__(817);\nvar select_default = /*#__PURE__*/__webpack_require__.n(src_select);\n;// CONCATENATED MODULE: ./src/common/command.js\n/**\n * Executes a given operation type.\n * @param {String} type\n * @return {Boolean}\n */\nfunction command(type) {\n try {\n return document.execCommand(type);\n } catch (err) {\n return false;\n }\n}\n;// CONCATENATED MODULE: ./src/actions/cut.js\n\n\n/**\n * Cut action wrapper.\n * @param {String|HTMLElement} target\n * @return {String}\n */\n\nvar ClipboardActionCut = function ClipboardActionCut(target) {\n var selectedText = select_default()(target);\n command('cut');\n return selectedText;\n};\n\n/* harmony default export */ var actions_cut = (ClipboardActionCut);\n;// CONCATENATED MODULE: ./src/common/create-fake-element.js\n/**\n * Creates a fake textarea element with a value.\n * @param {String} value\n * @return {HTMLElement}\n */\nfunction createFakeElement(value) {\n var isRTL = document.documentElement.getAttribute('dir') === 'rtl';\n var fakeElement = document.createElement('textarea'); // Prevent zooming on iOS\n\n fakeElement.style.fontSize = '12pt'; // Reset box model\n\n fakeElement.style.border = '0';\n fakeElement.style.padding = '0';\n fakeElement.style.margin = '0'; // Move element out of screen horizontally\n\n fakeElement.style.position = 'absolute';\n fakeElement.style[isRTL ? 'right' : 'left'] = '-9999px'; // Move element to the same position vertically\n\n var yPosition = window.pageYOffset || document.documentElement.scrollTop;\n fakeElement.style.top = \"\".concat(yPosition, \"px\");\n fakeElement.setAttribute('readonly', '');\n fakeElement.value = value;\n return fakeElement;\n}\n;// CONCATENATED MODULE: ./src/actions/copy.js\n\n\n\n/**\n * Create fake copy action wrapper using a fake element.\n * @param {String} target\n * @param {Object} options\n * @return {String}\n */\n\nvar fakeCopyAction = function fakeCopyAction(value, options) {\n var fakeElement = createFakeElement(value);\n options.container.appendChild(fakeElement);\n var selectedText = select_default()(fakeElement);\n command('copy');\n fakeElement.remove();\n return selectedText;\n};\n/**\n * Copy action wrapper.\n * @param {String|HTMLElement} target\n * @param {Object} options\n * @return {String}\n */\n\n\nvar ClipboardActionCopy = function ClipboardActionCopy(target) {\n var options = arguments.length > 1 && arguments[1] !== undefined ? arguments[1] : {\n container: document.body\n };\n var selectedText = '';\n\n if (typeof target === 'string') {\n selectedText = fakeCopyAction(target, options);\n } else if (target instanceof HTMLInputElement && !['text', 'search', 'url', 'tel', 'password'].includes(target === null || target === void 0 ? void 0 : target.type)) {\n // If input type doesn't support `setSelectionRange`. Simulate it. https://developer.mozilla.org/en-US/docs/Web/API/HTMLInputElement/setSelectionRange\n selectedText = fakeCopyAction(target.value, options);\n } else {\n selectedText = select_default()(target);\n command('copy');\n }\n\n return selectedText;\n};\n\n/* harmony default export */ var actions_copy = (ClipboardActionCopy);\n;// CONCATENATED MODULE: ./src/actions/default.js\nfunction _typeof(obj) { \"@babel/helpers - typeof\"; if (typeof Symbol === \"function\" && typeof Symbol.iterator === \"symbol\") { _typeof = function _typeof(obj) { return typeof obj; }; } else { _typeof = function _typeof(obj) { return obj && typeof Symbol === \"function\" && obj.constructor === Symbol && obj !== Symbol.prototype ? \"symbol\" : typeof obj; }; } return _typeof(obj); }\n\n\n\n/**\n * Inner function which performs selection from either `text` or `target`\n * properties and then executes copy or cut operations.\n * @param {Object} options\n */\n\nvar ClipboardActionDefault = function ClipboardActionDefault() {\n var options = arguments.length > 0 && arguments[0] !== undefined ? arguments[0] : {};\n // Defines base properties passed from constructor.\n var _options$action = options.action,\n action = _options$action === void 0 ? 'copy' : _options$action,\n container = options.container,\n target = options.target,\n text = options.text; // Sets the `action` to be performed which can be either 'copy' or 'cut'.\n\n if (action !== 'copy' && action !== 'cut') {\n throw new Error('Invalid \"action\" value, use either \"copy\" or \"cut\"');\n } // Sets the `target` property using an element that will be have its content copied.\n\n\n if (target !== undefined) {\n if (target && _typeof(target) === 'object' && target.nodeType === 1) {\n if (action === 'copy' && target.hasAttribute('disabled')) {\n throw new Error('Invalid \"target\" attribute. Please use \"readonly\" instead of \"disabled\" attribute');\n }\n\n if (action === 'cut' && (target.hasAttribute('readonly') || target.hasAttribute('disabled'))) {\n throw new Error('Invalid \"target\" attribute. You can\\'t cut text from elements with \"readonly\" or \"disabled\" attributes');\n }\n } else {\n throw new Error('Invalid \"target\" value, use a valid Element');\n }\n } // Define selection strategy based on `text` property.\n\n\n if (text) {\n return actions_copy(text, {\n container: container\n });\n } // Defines which selection strategy based on `target` property.\n\n\n if (target) {\n return action === 'cut' ? actions_cut(target) : actions_copy(target, {\n container: container\n });\n }\n};\n\n/* harmony default export */ var actions_default = (ClipboardActionDefault);\n;// CONCATENATED MODULE: ./src/clipboard.js\nfunction clipboard_typeof(obj) { \"@babel/helpers - typeof\"; if (typeof Symbol === \"function\" && typeof Symbol.iterator === \"symbol\") { clipboard_typeof = function _typeof(obj) { return typeof obj; }; } else { clipboard_typeof = function _typeof(obj) { return obj && typeof Symbol === \"function\" && obj.constructor === Symbol && obj !== Symbol.prototype ? \"symbol\" : typeof obj; }; } return clipboard_typeof(obj); }\n\nfunction _classCallCheck(instance, Constructor) { if (!(instance instanceof Constructor)) { throw new TypeError(\"Cannot call a class as a function\"); } }\n\nfunction _defineProperties(target, props) { for (var i = 0; i < props.length; i++) { var descriptor = props[i]; descriptor.enumerable = descriptor.enumerable || false; descriptor.configurable = true; if (\"value\" in descriptor) descriptor.writable = true; Object.defineProperty(target, descriptor.key, descriptor); } }\n\nfunction _createClass(Constructor, protoProps, staticProps) { if (protoProps) _defineProperties(Constructor.prototype, protoProps); if (staticProps) _defineProperties(Constructor, staticProps); return Constructor; }\n\nfunction _inherits(subClass, superClass) { if (typeof superClass !== \"function\" && superClass !== null) { throw new TypeError(\"Super expression must either be null or a function\"); } subClass.prototype = Object.create(superClass && superClass.prototype, { constructor: { value: subClass, writable: true, configurable: true } }); if (superClass) _setPrototypeOf(subClass, superClass); }\n\nfunction _setPrototypeOf(o, p) { _setPrototypeOf = Object.setPrototypeOf || function _setPrototypeOf(o, p) { o.__proto__ = p; return o; }; return _setPrototypeOf(o, p); }\n\nfunction _createSuper(Derived) { var hasNativeReflectConstruct = _isNativeReflectConstruct(); return function _createSuperInternal() { var Super = _getPrototypeOf(Derived), result; if (hasNativeReflectConstruct) { var NewTarget = _getPrototypeOf(this).constructor; result = Reflect.construct(Super, arguments, NewTarget); } else { result = Super.apply(this, arguments); } return _possibleConstructorReturn(this, result); }; }\n\nfunction _possibleConstructorReturn(self, call) { if (call && (clipboard_typeof(call) === \"object\" || typeof call === \"function\")) { return call; } return _assertThisInitialized(self); }\n\nfunction _assertThisInitialized(self) { if (self === void 0) { throw new ReferenceError(\"this hasn't been initialised - super() hasn't been called\"); } return self; }\n\nfunction _isNativeReflectConstruct() { if (typeof Reflect === \"undefined\" || !Reflect.construct) return false; if (Reflect.construct.sham) return false; if (typeof Proxy === \"function\") return true; try { Date.prototype.toString.call(Reflect.construct(Date, [], function () {})); return true; } catch (e) { return false; } }\n\nfunction _getPrototypeOf(o) { _getPrototypeOf = Object.setPrototypeOf ? Object.getPrototypeOf : function _getPrototypeOf(o) { return o.__proto__ || Object.getPrototypeOf(o); }; return _getPrototypeOf(o); }\n\n\n\n\n\n\n/**\n * Helper function to retrieve attribute value.\n * @param {String} suffix\n * @param {Element} element\n */\n\nfunction getAttributeValue(suffix, element) {\n var attribute = \"data-clipboard-\".concat(suffix);\n\n if (!element.hasAttribute(attribute)) {\n return;\n }\n\n return element.getAttribute(attribute);\n}\n/**\n * Base class which takes one or more elements, adds event listeners to them,\n * and instantiates a new `ClipboardAction` on each click.\n */\n\n\nvar Clipboard = /*#__PURE__*/function (_Emitter) {\n _inherits(Clipboard, _Emitter);\n\n var _super = _createSuper(Clipboard);\n\n /**\n * @param {String|HTMLElement|HTMLCollection|NodeList} trigger\n * @param {Object} options\n */\n function Clipboard(trigger, options) {\n var _this;\n\n _classCallCheck(this, Clipboard);\n\n _this = _super.call(this);\n\n _this.resolveOptions(options);\n\n _this.listenClick(trigger);\n\n return _this;\n }\n /**\n * Defines if attributes would be resolved using internal setter functions\n * or custom functions that were passed in the constructor.\n * @param {Object} options\n */\n\n\n _createClass(Clipboard, [{\n key: \"resolveOptions\",\n value: function resolveOptions() {\n var options = arguments.length > 0 && arguments[0] !== undefined ? arguments[0] : {};\n this.action = typeof options.action === 'function' ? options.action : this.defaultAction;\n this.target = typeof options.target === 'function' ? options.target : this.defaultTarget;\n this.text = typeof options.text === 'function' ? options.text : this.defaultText;\n this.container = clipboard_typeof(options.container) === 'object' ? options.container : document.body;\n }\n /**\n * Adds a click event listener to the passed trigger.\n * @param {String|HTMLElement|HTMLCollection|NodeList} trigger\n */\n\n }, {\n key: \"listenClick\",\n value: function listenClick(trigger) {\n var _this2 = this;\n\n this.listener = listen_default()(trigger, 'click', function (e) {\n return _this2.onClick(e);\n });\n }\n /**\n * Defines a new `ClipboardAction` on each click event.\n * @param {Event} e\n */\n\n }, {\n key: \"onClick\",\n value: function onClick(e) {\n var trigger = e.delegateTarget || e.currentTarget;\n var action = this.action(trigger) || 'copy';\n var text = actions_default({\n action: action,\n container: this.container,\n target: this.target(trigger),\n text: this.text(trigger)\n }); // Fires an event based on the copy operation result.\n\n this.emit(text ? 'success' : 'error', {\n action: action,\n text: text,\n trigger: trigger,\n clearSelection: function clearSelection() {\n if (trigger) {\n trigger.focus();\n }\n\n window.getSelection().removeAllRanges();\n }\n });\n }\n /**\n * Default `action` lookup function.\n * @param {Element} trigger\n */\n\n }, {\n key: \"defaultAction\",\n value: function defaultAction(trigger) {\n return getAttributeValue('action', trigger);\n }\n /**\n * Default `target` lookup function.\n * @param {Element} trigger\n */\n\n }, {\n key: \"defaultTarget\",\n value: function defaultTarget(trigger) {\n var selector = getAttributeValue('target', trigger);\n\n if (selector) {\n return document.querySelector(selector);\n }\n }\n /**\n * Allow fire programmatically a copy action\n * @param {String|HTMLElement} target\n * @param {Object} options\n * @returns Text copied.\n */\n\n }, {\n key: \"defaultText\",\n\n /**\n * Default `text` lookup function.\n * @param {Element} trigger\n */\n value: function defaultText(trigger) {\n return getAttributeValue('text', trigger);\n }\n /**\n * Destroy lifecycle.\n */\n\n }, {\n key: \"destroy\",\n value: function destroy() {\n this.listener.destroy();\n }\n }], [{\n key: \"copy\",\n value: function copy(target) {\n var options = arguments.length > 1 && arguments[1] !== undefined ? arguments[1] : {\n container: document.body\n };\n return actions_copy(target, options);\n }\n /**\n * Allow fire programmatically a cut action\n * @param {String|HTMLElement} target\n * @returns Text cutted.\n */\n\n }, {\n key: \"cut\",\n value: function cut(target) {\n return actions_cut(target);\n }\n /**\n * Returns the support of the given action, or all actions if no action is\n * given.\n * @param {String} [action]\n */\n\n }, {\n key: \"isSupported\",\n value: function isSupported() {\n var action = arguments.length > 0 && arguments[0] !== undefined ? arguments[0] : ['copy', 'cut'];\n var actions = typeof action === 'string' ? [action] : action;\n var support = !!document.queryCommandSupported;\n actions.forEach(function (action) {\n support = support && !!document.queryCommandSupported(action);\n });\n return support;\n }\n }]);\n\n return Clipboard;\n}((tiny_emitter_default()));\n\n/* harmony default export */ var clipboard = (Clipboard);\n\n/***/ }),\n\n/***/ 828:\n/***/ (function(module) {\n\nvar DOCUMENT_NODE_TYPE = 9;\n\n/**\n * A polyfill for Element.matches()\n */\nif (typeof Element !== 'undefined' && !Element.prototype.matches) {\n var proto = Element.prototype;\n\n proto.matches = proto.matchesSelector ||\n proto.mozMatchesSelector ||\n proto.msMatchesSelector ||\n proto.oMatchesSelector ||\n proto.webkitMatchesSelector;\n}\n\n/**\n * Finds the closest parent that matches a selector.\n *\n * @param {Element} element\n * @param {String} selector\n * @return {Function}\n */\nfunction closest (element, selector) {\n while (element && element.nodeType !== DOCUMENT_NODE_TYPE) {\n if (typeof element.matches === 'function' &&\n element.matches(selector)) {\n return element;\n }\n element = element.parentNode;\n }\n}\n\nmodule.exports = closest;\n\n\n/***/ }),\n\n/***/ 438:\n/***/ (function(module, __unused_webpack_exports, __webpack_require__) {\n\nvar closest = __webpack_require__(828);\n\n/**\n * Delegates event to a selector.\n *\n * @param {Element} element\n * @param {String} selector\n * @param {String} type\n * @param {Function} callback\n * @param {Boolean} useCapture\n * @return {Object}\n */\nfunction _delegate(element, selector, type, callback, useCapture) {\n var listenerFn = listener.apply(this, arguments);\n\n element.addEventListener(type, listenerFn, useCapture);\n\n return {\n destroy: function() {\n element.removeEventListener(type, listenerFn, useCapture);\n }\n }\n}\n\n/**\n * Delegates event to a selector.\n *\n * @param {Element|String|Array} [elements]\n * @param {String} selector\n * @param {String} type\n * @param {Function} callback\n * @param {Boolean} useCapture\n * @return {Object}\n */\nfunction delegate(elements, selector, type, callback, useCapture) {\n // Handle the regular Element usage\n if (typeof elements.addEventListener === 'function') {\n return _delegate.apply(null, arguments);\n }\n\n // Handle Element-less usage, it defaults to global delegation\n if (typeof type === 'function') {\n // Use `document` as the first parameter, then apply arguments\n // This is a short way to .unshift `arguments` without running into deoptimizations\n return _delegate.bind(null, document).apply(null, arguments);\n }\n\n // Handle Selector-based usage\n if (typeof elements === 'string') {\n elements = document.querySelectorAll(elements);\n }\n\n // Handle Array-like based usage\n return Array.prototype.map.call(elements, function (element) {\n return _delegate(element, selector, type, callback, useCapture);\n });\n}\n\n/**\n * Finds closest match and invokes callback.\n *\n * @param {Element} element\n * @param {String} selector\n * @param {String} type\n * @param {Function} callback\n * @return {Function}\n */\nfunction listener(element, selector, type, callback) {\n return function(e) {\n e.delegateTarget = closest(e.target, selector);\n\n if (e.delegateTarget) {\n callback.call(element, e);\n }\n }\n}\n\nmodule.exports = delegate;\n\n\n/***/ }),\n\n/***/ 879:\n/***/ (function(__unused_webpack_module, exports) {\n\n/**\n * Check if argument is a HTML element.\n *\n * @param {Object} value\n * @return {Boolean}\n */\nexports.node = function(value) {\n return value !== undefined\n && value instanceof HTMLElement\n && value.nodeType === 1;\n};\n\n/**\n * Check if argument is a list of HTML elements.\n *\n * @param {Object} value\n * @return {Boolean}\n */\nexports.nodeList = function(value) {\n var type = Object.prototype.toString.call(value);\n\n return value !== undefined\n && (type === '[object NodeList]' || type === '[object HTMLCollection]')\n && ('length' in value)\n && (value.length === 0 || exports.node(value[0]));\n};\n\n/**\n * Check if argument is a string.\n *\n * @param {Object} value\n * @return {Boolean}\n */\nexports.string = function(value) {\n return typeof value === 'string'\n || value instanceof String;\n};\n\n/**\n * Check if argument is a function.\n *\n * @param {Object} value\n * @return {Boolean}\n */\nexports.fn = function(value) {\n var type = Object.prototype.toString.call(value);\n\n return type === '[object Function]';\n};\n\n\n/***/ }),\n\n/***/ 370:\n/***/ (function(module, __unused_webpack_exports, __webpack_require__) {\n\nvar is = __webpack_require__(879);\nvar delegate = __webpack_require__(438);\n\n/**\n * Validates all params and calls the right\n * listener function based on its target type.\n *\n * @param {String|HTMLElement|HTMLCollection|NodeList} target\n * @param {String} type\n * @param {Function} callback\n * @return {Object}\n */\nfunction listen(target, type, callback) {\n if (!target && !type && !callback) {\n throw new Error('Missing required arguments');\n }\n\n if (!is.string(type)) {\n throw new TypeError('Second argument must be a String');\n }\n\n if (!is.fn(callback)) {\n throw new TypeError('Third argument must be a Function');\n }\n\n if (is.node(target)) {\n return listenNode(target, type, callback);\n }\n else if (is.nodeList(target)) {\n return listenNodeList(target, type, callback);\n }\n else if (is.string(target)) {\n return listenSelector(target, type, callback);\n }\n else {\n throw new TypeError('First argument must be a String, HTMLElement, HTMLCollection, or NodeList');\n }\n}\n\n/**\n * Adds an event listener to a HTML element\n * and returns a remove listener function.\n *\n * @param {HTMLElement} node\n * @param {String} type\n * @param {Function} callback\n * @return {Object}\n */\nfunction listenNode(node, type, callback) {\n node.addEventListener(type, callback);\n\n return {\n destroy: function() {\n node.removeEventListener(type, callback);\n }\n }\n}\n\n/**\n * Add an event listener to a list of HTML elements\n * and returns a remove listener function.\n *\n * @param {NodeList|HTMLCollection} nodeList\n * @param {String} type\n * @param {Function} callback\n * @return {Object}\n */\nfunction listenNodeList(nodeList, type, callback) {\n Array.prototype.forEach.call(nodeList, function(node) {\n node.addEventListener(type, callback);\n });\n\n return {\n destroy: function() {\n Array.prototype.forEach.call(nodeList, function(node) {\n node.removeEventListener(type, callback);\n });\n }\n }\n}\n\n/**\n * Add an event listener to a selector\n * and returns a remove listener function.\n *\n * @param {String} selector\n * @param {String} type\n * @param {Function} callback\n * @return {Object}\n */\nfunction listenSelector(selector, type, callback) {\n return delegate(document.body, selector, type, callback);\n}\n\nmodule.exports = listen;\n\n\n/***/ }),\n\n/***/ 817:\n/***/ (function(module) {\n\nfunction select(element) {\n var selectedText;\n\n if (element.nodeName === 'SELECT') {\n element.focus();\n\n selectedText = element.value;\n }\n else if (element.nodeName === 'INPUT' || element.nodeName === 'TEXTAREA') {\n var isReadOnly = element.hasAttribute('readonly');\n\n if (!isReadOnly) {\n element.setAttribute('readonly', '');\n }\n\n element.select();\n element.setSelectionRange(0, element.value.length);\n\n if (!isReadOnly) {\n element.removeAttribute('readonly');\n }\n\n selectedText = element.value;\n }\n else {\n if (element.hasAttribute('contenteditable')) {\n element.focus();\n }\n\n var selection = window.getSelection();\n var range = document.createRange();\n\n range.selectNodeContents(element);\n selection.removeAllRanges();\n selection.addRange(range);\n\n selectedText = selection.toString();\n }\n\n return selectedText;\n}\n\nmodule.exports = select;\n\n\n/***/ }),\n\n/***/ 279:\n/***/ (function(module) {\n\nfunction E () {\n // Keep this empty so it's easier to inherit from\n // (via https://github.com/lipsmack from https://github.com/scottcorgan/tiny-emitter/issues/3)\n}\n\nE.prototype = {\n on: function (name, callback, ctx) {\n var e = this.e || (this.e = {});\n\n (e[name] || (e[name] = [])).push({\n fn: callback,\n ctx: ctx\n });\n\n return this;\n },\n\n once: function (name, callback, ctx) {\n var self = this;\n function listener () {\n self.off(name, listener);\n callback.apply(ctx, arguments);\n };\n\n listener._ = callback\n return this.on(name, listener, ctx);\n },\n\n emit: function (name) {\n var data = [].slice.call(arguments, 1);\n var evtArr = ((this.e || (this.e = {}))[name] || []).slice();\n var i = 0;\n var len = evtArr.length;\n\n for (i; i < len; i++) {\n evtArr[i].fn.apply(evtArr[i].ctx, data);\n }\n\n return this;\n },\n\n off: function (name, callback) {\n var e = this.e || (this.e = {});\n var evts = e[name];\n var liveEvents = [];\n\n if (evts && callback) {\n for (var i = 0, len = evts.length; i < len; i++) {\n if (evts[i].fn !== callback && evts[i].fn._ !== callback)\n liveEvents.push(evts[i]);\n }\n }\n\n // Remove event from queue to prevent memory leak\n // Suggested by https://github.com/lazd\n // Ref: https://github.com/scottcorgan/tiny-emitter/commit/c6ebfaa9bc973b33d110a84a307742b7cf94c953#commitcomment-5024910\n\n (liveEvents.length)\n ? e[name] = liveEvents\n : delete e[name];\n\n return this;\n }\n};\n\nmodule.exports = E;\nmodule.exports.TinyEmitter = E;\n\n\n/***/ })\n\n/******/ \t});\n/************************************************************************/\n/******/ \t// The module cache\n/******/ \tvar __webpack_module_cache__ = {};\n/******/ \t\n/******/ \t// The require function\n/******/ \tfunction __webpack_require__(moduleId) {\n/******/ \t\t// Check if module is in cache\n/******/ \t\tif(__webpack_module_cache__[moduleId]) {\n/******/ \t\t\treturn __webpack_module_cache__[moduleId].exports;\n/******/ \t\t}\n/******/ \t\t// Create a new module (and put it into the cache)\n/******/ \t\tvar module = __webpack_module_cache__[moduleId] = {\n/******/ \t\t\t// no module.id needed\n/******/ \t\t\t// no module.loaded needed\n/******/ \t\t\texports: {}\n/******/ \t\t};\n/******/ \t\n/******/ \t\t// Execute the module function\n/******/ \t\t__webpack_modules__[moduleId](module, module.exports, __webpack_require__);\n/******/ \t\n/******/ \t\t// Return the exports of the module\n/******/ \t\treturn module.exports;\n/******/ \t}\n/******/ \t\n/************************************************************************/\n/******/ \t/* webpack/runtime/compat get default export */\n/******/ \t!function() {\n/******/ \t\t// getDefaultExport function for compatibility with non-harmony modules\n/******/ \t\t__webpack_require__.n = function(module) {\n/******/ \t\t\tvar getter = module && module.__esModule ?\n/******/ \t\t\t\tfunction() { return module['default']; } :\n/******/ \t\t\t\tfunction() { return module; };\n/******/ \t\t\t__webpack_require__.d(getter, { a: getter });\n/******/ \t\t\treturn getter;\n/******/ \t\t};\n/******/ \t}();\n/******/ \t\n/******/ \t/* webpack/runtime/define property getters */\n/******/ \t!function() {\n/******/ \t\t// define getter functions for harmony exports\n/******/ \t\t__webpack_require__.d = function(exports, definition) {\n/******/ \t\t\tfor(var key in definition) {\n/******/ \t\t\t\tif(__webpack_require__.o(definition, key) && !__webpack_require__.o(exports, key)) {\n/******/ \t\t\t\t\tObject.defineProperty(exports, key, { enumerable: true, get: definition[key] });\n/******/ \t\t\t\t}\n/******/ \t\t\t}\n/******/ \t\t};\n/******/ \t}();\n/******/ \t\n/******/ \t/* webpack/runtime/hasOwnProperty shorthand */\n/******/ \t!function() {\n/******/ \t\t__webpack_require__.o = function(obj, prop) { return Object.prototype.hasOwnProperty.call(obj, prop); }\n/******/ \t}();\n/******/ \t\n/************************************************************************/\n/******/ \t// module exports must be returned from runtime so entry inlining is disabled\n/******/ \t// startup\n/******/ \t// Load entry module and return exports\n/******/ \treturn __webpack_require__(686);\n/******/ })()\n.default;\n});", "/*\n * Copyright (c) 2016-2024 Martin Donath \n *\n * Permission is hereby granted, free of charge, to any person obtaining a copy\n * of this software and associated documentation files (the \"Software\"), to\n * deal in the Software without restriction, including without limitation the\n * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or\n * sell copies of the Software, and to permit persons to whom the Software is\n * furnished to do so, subject to the following conditions:\n *\n * The above copyright notice and this permission notice shall be included in\n * all copies or substantial portions of the Software.\n *\n * THE SOFTWARE IS PROVIDED \"AS IS\", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR\n * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,\n * FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL THE\n * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER\n * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING\n * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS\n * IN THE SOFTWARE.\n */\n\nimport \"focus-visible\"\n\nimport {\n EMPTY,\n NEVER,\n Observable,\n Subject,\n defer,\n delay,\n filter,\n map,\n merge,\n mergeWith,\n shareReplay,\n switchMap\n} from \"rxjs\"\n\nimport { configuration, feature } from \"./_\"\nimport {\n at,\n getActiveElement,\n getOptionalElement,\n requestJSON,\n setLocation,\n setToggle,\n watchDocument,\n watchKeyboard,\n watchLocation,\n watchLocationTarget,\n watchMedia,\n watchPrint,\n watchScript,\n watchViewport\n} from \"./browser\"\nimport {\n getComponentElement,\n getComponentElements,\n mountAnnounce,\n mountBackToTop,\n mountConsent,\n mountContent,\n mountDialog,\n mountHeader,\n mountHeaderTitle,\n mountPalette,\n mountProgress,\n mountSearch,\n mountSearchHiglight,\n mountSidebar,\n mountSource,\n mountTableOfContents,\n mountTabs,\n watchHeader,\n watchMain\n} from \"./components\"\nimport {\n SearchIndex,\n setupClipboardJS,\n setupInstantNavigation,\n setupVersionSelector\n} from \"./integrations\"\nimport {\n patchEllipsis,\n patchIndeterminate,\n patchScrollfix,\n patchScrolllock\n} from \"./patches\"\nimport \"./polyfills\"\n\n/* ----------------------------------------------------------------------------\n * Functions - @todo refactor\n * ------------------------------------------------------------------------- */\n\n/**\n * Fetch search index\n *\n * @returns Search index observable\n */\nfunction fetchSearchIndex(): Observable {\n if (location.protocol === \"file:\") {\n return watchScript(\n `${new URL(\"search/search_index.js\", config.base)}`\n )\n .pipe(\n // @ts-ignore - @todo fix typings\n map(() => __index),\n shareReplay(1)\n )\n } else {\n return requestJSON(\n new URL(\"search/search_index.json\", config.base)\n )\n }\n}\n\n/* ----------------------------------------------------------------------------\n * Application\n * ------------------------------------------------------------------------- */\n\n/* Yay, JavaScript is available */\ndocument.documentElement.classList.remove(\"no-js\")\ndocument.documentElement.classList.add(\"js\")\n\n/* Set up navigation observables and subjects */\nconst document$ = watchDocument()\nconst location$ = watchLocation()\nconst target$ = watchLocationTarget(location$)\nconst keyboard$ = watchKeyboard()\n\n/* Set up media observables */\nconst viewport$ = watchViewport()\nconst tablet$ = watchMedia(\"(min-width: 960px)\")\nconst screen$ = watchMedia(\"(min-width: 1220px)\")\nconst print$ = watchPrint()\n\n/* Retrieve search index, if search is enabled */\nconst config = configuration()\nconst index$ = document.forms.namedItem(\"search\")\n ? fetchSearchIndex()\n : NEVER\n\n/* Set up Clipboard.js integration */\nconst alert$ = new Subject()\nsetupClipboardJS({ alert$ })\n\n/* Set up progress indicator */\nconst progress$ = new Subject()\n\n/* Set up instant navigation, if enabled */\nif (feature(\"navigation.instant\"))\n setupInstantNavigation({ location$, viewport$, progress$ })\n .subscribe(document$)\n\n/* Set up version selector */\nif (config.version?.provider === \"mike\")\n setupVersionSelector({ document$ })\n\n/* Always close drawer and search on navigation */\nmerge(location$, target$)\n .pipe(\n delay(125)\n )\n .subscribe(() => {\n setToggle(\"drawer\", false)\n setToggle(\"search\", false)\n })\n\n/* Set up global keyboard handlers */\nkeyboard$\n .pipe(\n filter(({ mode }) => mode === \"global\")\n )\n .subscribe(key => {\n switch (key.type) {\n\n /* Go to previous page */\n case \"p\":\n case \",\":\n const prev = getOptionalElement(\"link[rel=prev]\")\n if (typeof prev !== \"undefined\")\n setLocation(prev)\n break\n\n /* Go to next page */\n case \"n\":\n case \".\":\n const next = getOptionalElement(\"link[rel=next]\")\n if (typeof next !== \"undefined\")\n setLocation(next)\n break\n\n /* Expand navigation, see https://bit.ly/3ZjG5io */\n case \"Enter\":\n const active = getActiveElement()\n if (active instanceof HTMLLabelElement)\n active.click()\n }\n })\n\n/* Set up patches */\npatchEllipsis({ viewport$, document$ })\npatchIndeterminate({ document$, tablet$ })\npatchScrollfix({ document$ })\npatchScrolllock({ viewport$, tablet$ })\n\n/* Set up header and main area observable */\nconst header$ = watchHeader(getComponentElement(\"header\"), { viewport$ })\nconst main$ = document$\n .pipe(\n map(() => getComponentElement(\"main\")),\n switchMap(el => watchMain(el, { viewport$, header$ })),\n shareReplay(1)\n )\n\n/* Set up control component observables */\nconst control$ = merge(\n\n /* Consent */\n ...getComponentElements(\"consent\")\n .map(el => mountConsent(el, { target$ })),\n\n /* Dialog */\n ...getComponentElements(\"dialog\")\n .map(el => mountDialog(el, { alert$ })),\n\n /* Color palette */\n ...getComponentElements(\"palette\")\n .map(el => mountPalette(el)),\n\n /* Progress bar */\n ...getComponentElements(\"progress\")\n .map(el => mountProgress(el, { progress$ })),\n\n /* Search */\n ...getComponentElements(\"search\")\n .map(el => mountSearch(el, { index$, keyboard$ })),\n\n /* Repository information */\n ...getComponentElements(\"source\")\n .map(el => mountSource(el))\n)\n\n/* Set up content component observables */\nconst content$ = defer(() => merge(\n\n /* Announcement bar */\n ...getComponentElements(\"announce\")\n .map(el => mountAnnounce(el)),\n\n /* Content */\n ...getComponentElements(\"content\")\n .map(el => mountContent(el, { viewport$, target$, print$ })),\n\n /* Search highlighting */\n ...getComponentElements(\"content\")\n .map(el => feature(\"search.highlight\")\n ? mountSearchHiglight(el, { index$, location$ })\n : EMPTY\n ),\n\n /* Header */\n ...getComponentElements(\"header\")\n .map(el => mountHeader(el, { viewport$, header$, main$ })),\n\n /* Header title */\n ...getComponentElements(\"header-title\")\n .map(el => mountHeaderTitle(el, { viewport$, header$ })),\n\n /* Sidebar */\n ...getComponentElements(\"sidebar\")\n .map(el => el.getAttribute(\"data-md-type\") === \"navigation\"\n ? at(screen$, () => mountSidebar(el, { viewport$, header$, main$ }))\n : at(tablet$, () => mountSidebar(el, { viewport$, header$, main$ }))\n ),\n\n /* Navigation tabs */\n ...getComponentElements(\"tabs\")\n .map(el => mountTabs(el, { viewport$, header$ })),\n\n /* Table of contents */\n ...getComponentElements(\"toc\")\n .map(el => mountTableOfContents(el, {\n viewport$, header$, main$, target$\n })),\n\n /* Back-to-top button */\n ...getComponentElements(\"top\")\n .map(el => mountBackToTop(el, { viewport$, header$, main$, target$ }))\n))\n\n/* Set up component observables */\nconst component$ = document$\n .pipe(\n switchMap(() => content$),\n mergeWith(control$),\n shareReplay(1)\n )\n\n/* Subscribe to all components */\ncomponent$.subscribe()\n\n/* ----------------------------------------------------------------------------\n * Exports\n * ------------------------------------------------------------------------- */\n\nwindow.document$ = document$ /* Document observable */\nwindow.location$ = location$ /* Location subject */\nwindow.target$ = target$ /* Location target observable */\nwindow.keyboard$ = keyboard$ /* Keyboard observable */\nwindow.viewport$ = viewport$ /* Viewport observable */\nwindow.tablet$ = tablet$ /* Media tablet observable */\nwindow.screen$ = screen$ /* Media screen observable */\nwindow.print$ = print$ /* Media print observable */\nwindow.alert$ = alert$ /* Alert subject */\nwindow.progress$ = progress$ /* Progress indicator subject */\nwindow.component$ = component$ /* Component observable */\n", "/******************************************************************************\nCopyright (c) Microsoft Corporation.\n\nPermission to use, copy, modify, and/or distribute this software for any\npurpose with or without fee is hereby granted.\n\nTHE SOFTWARE IS PROVIDED \"AS IS\" AND THE AUTHOR DISCLAIMS ALL WARRANTIES WITH\nREGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY\nAND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT,\nINDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM\nLOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR\nOTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR\nPERFORMANCE OF THIS SOFTWARE.\n***************************************************************************** */\n/* global Reflect, Promise, SuppressedError, Symbol, Iterator */\n\nvar extendStatics = function(d, b) {\n extendStatics = Object.setPrototypeOf ||\n ({ __proto__: [] } instanceof Array && function (d, b) { d.__proto__ = b; }) ||\n function (d, b) { for (var p in b) if (Object.prototype.hasOwnProperty.call(b, p)) d[p] = b[p]; };\n return extendStatics(d, b);\n};\n\nexport function __extends(d, b) {\n if (typeof b !== \"function\" && b !== null)\n throw new TypeError(\"Class extends value \" + String(b) + \" is not a constructor or null\");\n extendStatics(d, b);\n function __() { this.constructor = d; }\n d.prototype = b === null ? Object.create(b) : (__.prototype = b.prototype, new __());\n}\n\nexport var __assign = function() {\n __assign = Object.assign || function __assign(t) {\n for (var s, i = 1, n = arguments.length; i < n; i++) {\n s = arguments[i];\n for (var p in s) if (Object.prototype.hasOwnProperty.call(s, p)) t[p] = s[p];\n }\n return t;\n }\n return __assign.apply(this, arguments);\n}\n\nexport function __rest(s, e) {\n var t = {};\n for (var p in s) if (Object.prototype.hasOwnProperty.call(s, p) && e.indexOf(p) < 0)\n t[p] = s[p];\n if (s != null && typeof Object.getOwnPropertySymbols === \"function\")\n for (var i = 0, p = Object.getOwnPropertySymbols(s); i < p.length; i++) {\n if (e.indexOf(p[i]) < 0 && Object.prototype.propertyIsEnumerable.call(s, p[i]))\n t[p[i]] = s[p[i]];\n }\n return t;\n}\n\nexport function __decorate(decorators, target, key, desc) {\n var c = arguments.length, r = c < 3 ? target : desc === null ? desc = Object.getOwnPropertyDescriptor(target, key) : desc, d;\n if (typeof Reflect === \"object\" && typeof Reflect.decorate === \"function\") r = Reflect.decorate(decorators, target, key, desc);\n else for (var i = decorators.length - 1; i >= 0; i--) if (d = decorators[i]) r = (c < 3 ? d(r) : c > 3 ? d(target, key, r) : d(target, key)) || r;\n return c > 3 && r && Object.defineProperty(target, key, r), r;\n}\n\nexport function __param(paramIndex, decorator) {\n return function (target, key) { decorator(target, key, paramIndex); }\n}\n\nexport function __esDecorate(ctor, descriptorIn, decorators, contextIn, initializers, extraInitializers) {\n function accept(f) { if (f !== void 0 && typeof f !== \"function\") throw new TypeError(\"Function expected\"); return f; }\n var kind = contextIn.kind, key = kind === \"getter\" ? \"get\" : kind === \"setter\" ? \"set\" : \"value\";\n var target = !descriptorIn && ctor ? contextIn[\"static\"] ? ctor : ctor.prototype : null;\n var descriptor = descriptorIn || (target ? Object.getOwnPropertyDescriptor(target, contextIn.name) : {});\n var _, done = false;\n for (var i = decorators.length - 1; i >= 0; i--) {\n var context = {};\n for (var p in contextIn) context[p] = p === \"access\" ? {} : contextIn[p];\n for (var p in contextIn.access) context.access[p] = contextIn.access[p];\n context.addInitializer = function (f) { if (done) throw new TypeError(\"Cannot add initializers after decoration has completed\"); extraInitializers.push(accept(f || null)); };\n var result = (0, decorators[i])(kind === \"accessor\" ? { get: descriptor.get, set: descriptor.set } : descriptor[key], context);\n if (kind === \"accessor\") {\n if (result === void 0) continue;\n if (result === null || typeof result !== \"object\") throw new TypeError(\"Object expected\");\n if (_ = accept(result.get)) descriptor.get = _;\n if (_ = accept(result.set)) descriptor.set = _;\n if (_ = accept(result.init)) initializers.unshift(_);\n }\n else if (_ = accept(result)) {\n if (kind === \"field\") initializers.unshift(_);\n else descriptor[key] = _;\n }\n }\n if (target) Object.defineProperty(target, contextIn.name, descriptor);\n done = true;\n};\n\nexport function __runInitializers(thisArg, initializers, value) {\n var useValue = arguments.length > 2;\n for (var i = 0; i < initializers.length; i++) {\n value = useValue ? initializers[i].call(thisArg, value) : initializers[i].call(thisArg);\n }\n return useValue ? value : void 0;\n};\n\nexport function __propKey(x) {\n return typeof x === \"symbol\" ? x : \"\".concat(x);\n};\n\nexport function __setFunctionName(f, name, prefix) {\n if (typeof name === \"symbol\") name = name.description ? \"[\".concat(name.description, \"]\") : \"\";\n return Object.defineProperty(f, \"name\", { configurable: true, value: prefix ? \"\".concat(prefix, \" \", name) : name });\n};\n\nexport function __metadata(metadataKey, metadataValue) {\n if (typeof Reflect === \"object\" && typeof Reflect.metadata === \"function\") return Reflect.metadata(metadataKey, metadataValue);\n}\n\nexport function __awaiter(thisArg, _arguments, P, generator) {\n function adopt(value) { return value instanceof P ? value : new P(function (resolve) { resolve(value); }); }\n return new (P || (P = Promise))(function (resolve, reject) {\n function fulfilled(value) { try { step(generator.next(value)); } catch (e) { reject(e); } }\n function rejected(value) { try { step(generator[\"throw\"](value)); } catch (e) { reject(e); } }\n function step(result) { result.done ? resolve(result.value) : adopt(result.value).then(fulfilled, rejected); }\n step((generator = generator.apply(thisArg, _arguments || [])).next());\n });\n}\n\nexport function __generator(thisArg, body) {\n var _ = { label: 0, sent: function() { if (t[0] & 1) throw t[1]; return t[1]; }, trys: [], ops: [] }, f, y, t, g = Object.create((typeof Iterator === \"function\" ? Iterator : Object).prototype);\n return g.next = verb(0), g[\"throw\"] = verb(1), g[\"return\"] = verb(2), typeof Symbol === \"function\" && (g[Symbol.iterator] = function() { return this; }), g;\n function verb(n) { return function (v) { return step([n, v]); }; }\n function step(op) {\n if (f) throw new TypeError(\"Generator is already executing.\");\n while (g && (g = 0, op[0] && (_ = 0)), _) try {\n if (f = 1, y && (t = op[0] & 2 ? y[\"return\"] : op[0] ? y[\"throw\"] || ((t = y[\"return\"]) && t.call(y), 0) : y.next) && !(t = t.call(y, op[1])).done) return t;\n if (y = 0, t) op = [op[0] & 2, t.value];\n switch (op[0]) {\n case 0: case 1: t = op; break;\n case 4: _.label++; return { value: op[1], done: false };\n case 5: _.label++; y = op[1]; op = [0]; continue;\n case 7: op = _.ops.pop(); _.trys.pop(); continue;\n default:\n if (!(t = _.trys, t = t.length > 0 && t[t.length - 1]) && (op[0] === 6 || op[0] === 2)) { _ = 0; continue; }\n if (op[0] === 3 && (!t || (op[1] > t[0] && op[1] < t[3]))) { _.label = op[1]; break; }\n if (op[0] === 6 && _.label < t[1]) { _.label = t[1]; t = op; break; }\n if (t && _.label < t[2]) { _.label = t[2]; _.ops.push(op); break; }\n if (t[2]) _.ops.pop();\n _.trys.pop(); continue;\n }\n op = body.call(thisArg, _);\n } catch (e) { op = [6, e]; y = 0; } finally { f = t = 0; }\n if (op[0] & 5) throw op[1]; return { value: op[0] ? op[1] : void 0, done: true };\n }\n}\n\nexport var __createBinding = Object.create ? (function(o, m, k, k2) {\n if (k2 === undefined) k2 = k;\n var desc = Object.getOwnPropertyDescriptor(m, k);\n if (!desc || (\"get\" in desc ? !m.__esModule : desc.writable || desc.configurable)) {\n desc = { enumerable: true, get: function() { return m[k]; } };\n }\n Object.defineProperty(o, k2, desc);\n}) : (function(o, m, k, k2) {\n if (k2 === undefined) k2 = k;\n o[k2] = m[k];\n});\n\nexport function __exportStar(m, o) {\n for (var p in m) if (p !== \"default\" && !Object.prototype.hasOwnProperty.call(o, p)) __createBinding(o, m, p);\n}\n\nexport function __values(o) {\n var s = typeof Symbol === \"function\" && Symbol.iterator, m = s && o[s], i = 0;\n if (m) return m.call(o);\n if (o && typeof o.length === \"number\") return {\n next: function () {\n if (o && i >= o.length) o = void 0;\n return { value: o && o[i++], done: !o };\n }\n };\n throw new TypeError(s ? \"Object is not iterable.\" : \"Symbol.iterator is not defined.\");\n}\n\nexport function __read(o, n) {\n var m = typeof Symbol === \"function\" && o[Symbol.iterator];\n if (!m) return o;\n var i = m.call(o), r, ar = [], e;\n try {\n while ((n === void 0 || n-- > 0) && !(r = i.next()).done) ar.push(r.value);\n }\n catch (error) { e = { error: error }; }\n finally {\n try {\n if (r && !r.done && (m = i[\"return\"])) m.call(i);\n }\n finally { if (e) throw e.error; }\n }\n return ar;\n}\n\n/** @deprecated */\nexport function __spread() {\n for (var ar = [], i = 0; i < arguments.length; i++)\n ar = ar.concat(__read(arguments[i]));\n return ar;\n}\n\n/** @deprecated */\nexport function __spreadArrays() {\n for (var s = 0, i = 0, il = arguments.length; i < il; i++) s += arguments[i].length;\n for (var r = Array(s), k = 0, i = 0; i < il; i++)\n for (var a = arguments[i], j = 0, jl = a.length; j < jl; j++, k++)\n r[k] = a[j];\n return r;\n}\n\nexport function __spreadArray(to, from, pack) {\n if (pack || arguments.length === 2) for (var i = 0, l = from.length, ar; i < l; i++) {\n if (ar || !(i in from)) {\n if (!ar) ar = Array.prototype.slice.call(from, 0, i);\n ar[i] = from[i];\n }\n }\n return to.concat(ar || Array.prototype.slice.call(from));\n}\n\nexport function __await(v) {\n return this instanceof __await ? (this.v = v, this) : new __await(v);\n}\n\nexport function __asyncGenerator(thisArg, _arguments, generator) {\n if (!Symbol.asyncIterator) throw new TypeError(\"Symbol.asyncIterator is not defined.\");\n var g = generator.apply(thisArg, _arguments || []), i, q = [];\n return i = Object.create((typeof AsyncIterator === \"function\" ? AsyncIterator : Object).prototype), verb(\"next\"), verb(\"throw\"), verb(\"return\", awaitReturn), i[Symbol.asyncIterator] = function () { return this; }, i;\n function awaitReturn(f) { return function (v) { return Promise.resolve(v).then(f, reject); }; }\n function verb(n, f) { if (g[n]) { i[n] = function (v) { return new Promise(function (a, b) { q.push([n, v, a, b]) > 1 || resume(n, v); }); }; if (f) i[n] = f(i[n]); } }\n function resume(n, v) { try { step(g[n](v)); } catch (e) { settle(q[0][3], e); } }\n function step(r) { r.value instanceof __await ? Promise.resolve(r.value.v).then(fulfill, reject) : settle(q[0][2], r); }\n function fulfill(value) { resume(\"next\", value); }\n function reject(value) { resume(\"throw\", value); }\n function settle(f, v) { if (f(v), q.shift(), q.length) resume(q[0][0], q[0][1]); }\n}\n\nexport function __asyncDelegator(o) {\n var i, p;\n return i = {}, verb(\"next\"), verb(\"throw\", function (e) { throw e; }), verb(\"return\"), i[Symbol.iterator] = function () { return this; }, i;\n function verb(n, f) { i[n] = o[n] ? function (v) { return (p = !p) ? { value: __await(o[n](v)), done: false } : f ? f(v) : v; } : f; }\n}\n\nexport function __asyncValues(o) {\n if (!Symbol.asyncIterator) throw new TypeError(\"Symbol.asyncIterator is not defined.\");\n var m = o[Symbol.asyncIterator], i;\n return m ? m.call(o) : (o = typeof __values === \"function\" ? __values(o) : o[Symbol.iterator](), i = {}, verb(\"next\"), verb(\"throw\"), verb(\"return\"), i[Symbol.asyncIterator] = function () { return this; }, i);\n function verb(n) { i[n] = o[n] && function (v) { return new Promise(function (resolve, reject) { v = o[n](v), settle(resolve, reject, v.done, v.value); }); }; }\n function settle(resolve, reject, d, v) { Promise.resolve(v).then(function(v) { resolve({ value: v, done: d }); }, reject); }\n}\n\nexport function __makeTemplateObject(cooked, raw) {\n if (Object.defineProperty) { Object.defineProperty(cooked, \"raw\", { value: raw }); } else { cooked.raw = raw; }\n return cooked;\n};\n\nvar __setModuleDefault = Object.create ? (function(o, v) {\n Object.defineProperty(o, \"default\", { enumerable: true, value: v });\n}) : function(o, v) {\n o[\"default\"] = v;\n};\n\nexport function __importStar(mod) {\n if (mod && mod.__esModule) return mod;\n var result = {};\n if (mod != null) for (var k in mod) if (k !== \"default\" && Object.prototype.hasOwnProperty.call(mod, k)) __createBinding(result, mod, k);\n __setModuleDefault(result, mod);\n return result;\n}\n\nexport function __importDefault(mod) {\n return (mod && mod.__esModule) ? mod : { default: mod };\n}\n\nexport function __classPrivateFieldGet(receiver, state, kind, f) {\n if (kind === \"a\" && !f) throw new TypeError(\"Private accessor was defined without a getter\");\n if (typeof state === \"function\" ? receiver !== state || !f : !state.has(receiver)) throw new TypeError(\"Cannot read private member from an object whose class did not declare it\");\n return kind === \"m\" ? f : kind === \"a\" ? f.call(receiver) : f ? f.value : state.get(receiver);\n}\n\nexport function __classPrivateFieldSet(receiver, state, value, kind, f) {\n if (kind === \"m\") throw new TypeError(\"Private method is not writable\");\n if (kind === \"a\" && !f) throw new TypeError(\"Private accessor was defined without a setter\");\n if (typeof state === \"function\" ? receiver !== state || !f : !state.has(receiver)) throw new TypeError(\"Cannot write private member to an object whose class did not declare it\");\n return (kind === \"a\" ? f.call(receiver, value) : f ? f.value = value : state.set(receiver, value)), value;\n}\n\nexport function __classPrivateFieldIn(state, receiver) {\n if (receiver === null || (typeof receiver !== \"object\" && typeof receiver !== \"function\")) throw new TypeError(\"Cannot use 'in' operator on non-object\");\n return typeof state === \"function\" ? receiver === state : state.has(receiver);\n}\n\nexport function __addDisposableResource(env, value, async) {\n if (value !== null && value !== void 0) {\n if (typeof value !== \"object\" && typeof value !== \"function\") throw new TypeError(\"Object expected.\");\n var dispose, inner;\n if (async) {\n if (!Symbol.asyncDispose) throw new TypeError(\"Symbol.asyncDispose is not defined.\");\n dispose = value[Symbol.asyncDispose];\n }\n if (dispose === void 0) {\n if (!Symbol.dispose) throw new TypeError(\"Symbol.dispose is not defined.\");\n dispose = value[Symbol.dispose];\n if (async) inner = dispose;\n }\n if (typeof dispose !== \"function\") throw new TypeError(\"Object not disposable.\");\n if (inner) dispose = function() { try { inner.call(this); } catch (e) { return Promise.reject(e); } };\n env.stack.push({ value: value, dispose: dispose, async: async });\n }\n else if (async) {\n env.stack.push({ async: true });\n }\n return value;\n}\n\nvar _SuppressedError = typeof SuppressedError === \"function\" ? SuppressedError : function (error, suppressed, message) {\n var e = new Error(message);\n return e.name = \"SuppressedError\", e.error = error, e.suppressed = suppressed, e;\n};\n\nexport function __disposeResources(env) {\n function fail(e) {\n env.error = env.hasError ? new _SuppressedError(e, env.error, \"An error was suppressed during disposal.\") : e;\n env.hasError = true;\n }\n var r, s = 0;\n function next() {\n while (r = env.stack.pop()) {\n try {\n if (!r.async && s === 1) return s = 0, env.stack.push(r), Promise.resolve().then(next);\n if (r.dispose) {\n var result = r.dispose.call(r.value);\n if (r.async) return s |= 2, Promise.resolve(result).then(next, function(e) { fail(e); return next(); });\n }\n else s |= 1;\n }\n catch (e) {\n fail(e);\n }\n }\n if (s === 1) return env.hasError ? Promise.reject(env.error) : Promise.resolve();\n if (env.hasError) throw env.error;\n }\n return next();\n}\n\nexport default {\n __extends,\n __assign,\n __rest,\n __decorate,\n __param,\n __metadata,\n __awaiter,\n __generator,\n __createBinding,\n __exportStar,\n __values,\n __read,\n __spread,\n __spreadArrays,\n __spreadArray,\n __await,\n __asyncGenerator,\n __asyncDelegator,\n __asyncValues,\n __makeTemplateObject,\n __importStar,\n __importDefault,\n __classPrivateFieldGet,\n __classPrivateFieldSet,\n __classPrivateFieldIn,\n __addDisposableResource,\n __disposeResources,\n};\n", "/**\n * Returns true if the object is a function.\n * @param value The value to check\n */\nexport function isFunction(value: any): value is (...args: any[]) => any {\n return typeof value === 'function';\n}\n", "/**\n * Used to create Error subclasses until the community moves away from ES5.\n *\n * This is because compiling from TypeScript down to ES5 has issues with subclassing Errors\n * as well as other built-in types: https://github.com/Microsoft/TypeScript/issues/12123\n *\n * @param createImpl A factory function to create the actual constructor implementation. The returned\n * function should be a named function that calls `_super` internally.\n */\nexport function createErrorClass(createImpl: (_super: any) => any): T {\n const _super = (instance: any) => {\n Error.call(instance);\n instance.stack = new Error().stack;\n };\n\n const ctorFunc = createImpl(_super);\n ctorFunc.prototype = Object.create(Error.prototype);\n ctorFunc.prototype.constructor = ctorFunc;\n return ctorFunc;\n}\n", "import { createErrorClass } from './createErrorClass';\n\nexport interface UnsubscriptionError extends Error {\n readonly errors: any[];\n}\n\nexport interface UnsubscriptionErrorCtor {\n /**\n * @deprecated Internal implementation detail. Do not construct error instances.\n * Cannot be tagged as internal: https://github.com/ReactiveX/rxjs/issues/6269\n */\n new (errors: any[]): UnsubscriptionError;\n}\n\n/**\n * An error thrown when one or more errors have occurred during the\n * `unsubscribe` of a {@link Subscription}.\n */\nexport const UnsubscriptionError: UnsubscriptionErrorCtor = createErrorClass(\n (_super) =>\n function UnsubscriptionErrorImpl(this: any, errors: (Error | string)[]) {\n _super(this);\n this.message = errors\n ? `${errors.length} errors occurred during unsubscription:\n${errors.map((err, i) => `${i + 1}) ${err.toString()}`).join('\\n ')}`\n : '';\n this.name = 'UnsubscriptionError';\n this.errors = errors;\n }\n);\n", "/**\n * Removes an item from an array, mutating it.\n * @param arr The array to remove the item from\n * @param item The item to remove\n */\nexport function arrRemove(arr: T[] | undefined | null, item: T) {\n if (arr) {\n const index = arr.indexOf(item);\n 0 <= index && arr.splice(index, 1);\n }\n}\n", "import { isFunction } from './util/isFunction';\nimport { UnsubscriptionError } from './util/UnsubscriptionError';\nimport { SubscriptionLike, TeardownLogic, Unsubscribable } from './types';\nimport { arrRemove } from './util/arrRemove';\n\n/**\n * Represents a disposable resource, such as the execution of an Observable. A\n * Subscription has one important method, `unsubscribe`, that takes no argument\n * and just disposes the resource held by the subscription.\n *\n * Additionally, subscriptions may be grouped together through the `add()`\n * method, which will attach a child Subscription to the current Subscription.\n * When a Subscription is unsubscribed, all its children (and its grandchildren)\n * will be unsubscribed as well.\n *\n * @class Subscription\n */\nexport class Subscription implements SubscriptionLike {\n /** @nocollapse */\n public static EMPTY = (() => {\n const empty = new Subscription();\n empty.closed = true;\n return empty;\n })();\n\n /**\n * A flag to indicate whether this Subscription has already been unsubscribed.\n */\n public closed = false;\n\n private _parentage: Subscription[] | Subscription | null = null;\n\n /**\n * The list of registered finalizers to execute upon unsubscription. Adding and removing from this\n * list occurs in the {@link #add} and {@link #remove} methods.\n */\n private _finalizers: Exclude[] | null = null;\n\n /**\n * @param initialTeardown A function executed first as part of the finalization\n * process that is kicked off when {@link #unsubscribe} is called.\n */\n constructor(private initialTeardown?: () => void) {}\n\n /**\n * Disposes the resources held by the subscription. May, for instance, cancel\n * an ongoing Observable execution or cancel any other type of work that\n * started when the Subscription was created.\n * @return {void}\n */\n unsubscribe(): void {\n let errors: any[] | undefined;\n\n if (!this.closed) {\n this.closed = true;\n\n // Remove this from it's parents.\n const { _parentage } = this;\n if (_parentage) {\n this._parentage = null;\n if (Array.isArray(_parentage)) {\n for (const parent of _parentage) {\n parent.remove(this);\n }\n } else {\n _parentage.remove(this);\n }\n }\n\n const { initialTeardown: initialFinalizer } = this;\n if (isFunction(initialFinalizer)) {\n try {\n initialFinalizer();\n } catch (e) {\n errors = e instanceof UnsubscriptionError ? e.errors : [e];\n }\n }\n\n const { _finalizers } = this;\n if (_finalizers) {\n this._finalizers = null;\n for (const finalizer of _finalizers) {\n try {\n execFinalizer(finalizer);\n } catch (err) {\n errors = errors ?? [];\n if (err instanceof UnsubscriptionError) {\n errors = [...errors, ...err.errors];\n } else {\n errors.push(err);\n }\n }\n }\n }\n\n if (errors) {\n throw new UnsubscriptionError(errors);\n }\n }\n }\n\n /**\n * Adds a finalizer to this subscription, so that finalization will be unsubscribed/called\n * when this subscription is unsubscribed. If this subscription is already {@link #closed},\n * because it has already been unsubscribed, then whatever finalizer is passed to it\n * will automatically be executed (unless the finalizer itself is also a closed subscription).\n *\n * Closed Subscriptions cannot be added as finalizers to any subscription. Adding a closed\n * subscription to a any subscription will result in no operation. (A noop).\n *\n * Adding a subscription to itself, or adding `null` or `undefined` will not perform any\n * operation at all. (A noop).\n *\n * `Subscription` instances that are added to this instance will automatically remove themselves\n * if they are unsubscribed. Functions and {@link Unsubscribable} objects that you wish to remove\n * will need to be removed manually with {@link #remove}\n *\n * @param teardown The finalization logic to add to this subscription.\n */\n add(teardown: TeardownLogic): void {\n // Only add the finalizer if it's not undefined\n // and don't add a subscription to itself.\n if (teardown && teardown !== this) {\n if (this.closed) {\n // If this subscription is already closed,\n // execute whatever finalizer is handed to it automatically.\n execFinalizer(teardown);\n } else {\n if (teardown instanceof Subscription) {\n // We don't add closed subscriptions, and we don't add the same subscription\n // twice. Subscription unsubscribe is idempotent.\n if (teardown.closed || teardown._hasParent(this)) {\n return;\n }\n teardown._addParent(this);\n }\n (this._finalizers = this._finalizers ?? []).push(teardown);\n }\n }\n }\n\n /**\n * Checks to see if a this subscription already has a particular parent.\n * This will signal that this subscription has already been added to the parent in question.\n * @param parent the parent to check for\n */\n private _hasParent(parent: Subscription) {\n const { _parentage } = this;\n return _parentage === parent || (Array.isArray(_parentage) && _parentage.includes(parent));\n }\n\n /**\n * Adds a parent to this subscription so it can be removed from the parent if it\n * unsubscribes on it's own.\n *\n * NOTE: THIS ASSUMES THAT {@link _hasParent} HAS ALREADY BEEN CHECKED.\n * @param parent The parent subscription to add\n */\n private _addParent(parent: Subscription) {\n const { _parentage } = this;\n this._parentage = Array.isArray(_parentage) ? (_parentage.push(parent), _parentage) : _parentage ? [_parentage, parent] : parent;\n }\n\n /**\n * Called on a child when it is removed via {@link #remove}.\n * @param parent The parent to remove\n */\n private _removeParent(parent: Subscription) {\n const { _parentage } = this;\n if (_parentage === parent) {\n this._parentage = null;\n } else if (Array.isArray(_parentage)) {\n arrRemove(_parentage, parent);\n }\n }\n\n /**\n * Removes a finalizer from this subscription that was previously added with the {@link #add} method.\n *\n * Note that `Subscription` instances, when unsubscribed, will automatically remove themselves\n * from every other `Subscription` they have been added to. This means that using the `remove` method\n * is not a common thing and should be used thoughtfully.\n *\n * If you add the same finalizer instance of a function or an unsubscribable object to a `Subscription` instance\n * more than once, you will need to call `remove` the same number of times to remove all instances.\n *\n * All finalizer instances are removed to free up memory upon unsubscription.\n *\n * @param teardown The finalizer to remove from this subscription\n */\n remove(teardown: Exclude): void {\n const { _finalizers } = this;\n _finalizers && arrRemove(_finalizers, teardown);\n\n if (teardown instanceof Subscription) {\n teardown._removeParent(this);\n }\n }\n}\n\nexport const EMPTY_SUBSCRIPTION = Subscription.EMPTY;\n\nexport function isSubscription(value: any): value is Subscription {\n return (\n value instanceof Subscription ||\n (value && 'closed' in value && isFunction(value.remove) && isFunction(value.add) && isFunction(value.unsubscribe))\n );\n}\n\nfunction execFinalizer(finalizer: Unsubscribable | (() => void)) {\n if (isFunction(finalizer)) {\n finalizer();\n } else {\n finalizer.unsubscribe();\n }\n}\n", "import { Subscriber } from './Subscriber';\nimport { ObservableNotification } from './types';\n\n/**\n * The {@link GlobalConfig} object for RxJS. It is used to configure things\n * like how to react on unhandled errors.\n */\nexport const config: GlobalConfig = {\n onUnhandledError: null,\n onStoppedNotification: null,\n Promise: undefined,\n useDeprecatedSynchronousErrorHandling: false,\n useDeprecatedNextContext: false,\n};\n\n/**\n * The global configuration object for RxJS, used to configure things\n * like how to react on unhandled errors. Accessible via {@link config}\n * object.\n */\nexport interface GlobalConfig {\n /**\n * A registration point for unhandled errors from RxJS. These are errors that\n * cannot were not handled by consuming code in the usual subscription path. For\n * example, if you have this configured, and you subscribe to an observable without\n * providing an error handler, errors from that subscription will end up here. This\n * will _always_ be called asynchronously on another job in the runtime. This is because\n * we do not want errors thrown in this user-configured handler to interfere with the\n * behavior of the library.\n */\n onUnhandledError: ((err: any) => void) | null;\n\n /**\n * A registration point for notifications that cannot be sent to subscribers because they\n * have completed, errored or have been explicitly unsubscribed. By default, next, complete\n * and error notifications sent to stopped subscribers are noops. However, sometimes callers\n * might want a different behavior. For example, with sources that attempt to report errors\n * to stopped subscribers, a caller can configure RxJS to throw an unhandled error instead.\n * This will _always_ be called asynchronously on another job in the runtime. This is because\n * we do not want errors thrown in this user-configured handler to interfere with the\n * behavior of the library.\n */\n onStoppedNotification: ((notification: ObservableNotification, subscriber: Subscriber) => void) | null;\n\n /**\n * The promise constructor used by default for {@link Observable#toPromise toPromise} and {@link Observable#forEach forEach}\n * methods.\n *\n * @deprecated As of version 8, RxJS will no longer support this sort of injection of a\n * Promise constructor. If you need a Promise implementation other than native promises,\n * please polyfill/patch Promise as you see appropriate. Will be removed in v8.\n */\n Promise?: PromiseConstructorLike;\n\n /**\n * If true, turns on synchronous error rethrowing, which is a deprecated behavior\n * in v6 and higher. This behavior enables bad patterns like wrapping a subscribe\n * call in a try/catch block. It also enables producer interference, a nasty bug\n * where a multicast can be broken for all observers by a downstream consumer with\n * an unhandled error. DO NOT USE THIS FLAG UNLESS IT'S NEEDED TO BUY TIME\n * FOR MIGRATION REASONS.\n *\n * @deprecated As of version 8, RxJS will no longer support synchronous throwing\n * of unhandled errors. All errors will be thrown on a separate call stack to prevent bad\n * behaviors described above. Will be removed in v8.\n */\n useDeprecatedSynchronousErrorHandling: boolean;\n\n /**\n * If true, enables an as-of-yet undocumented feature from v5: The ability to access\n * `unsubscribe()` via `this` context in `next` functions created in observers passed\n * to `subscribe`.\n *\n * This is being removed because the performance was severely problematic, and it could also cause\n * issues when types other than POJOs are passed to subscribe as subscribers, as they will likely have\n * their `this` context overwritten.\n *\n * @deprecated As of version 8, RxJS will no longer support altering the\n * context of next functions provided as part of an observer to Subscribe. Instead,\n * you will have access to a subscription or a signal or token that will allow you to do things like\n * unsubscribe and test closed status. Will be removed in v8.\n */\n useDeprecatedNextContext: boolean;\n}\n", "import type { TimerHandle } from './timerHandle';\ntype SetTimeoutFunction = (handler: () => void, timeout?: number, ...args: any[]) => TimerHandle;\ntype ClearTimeoutFunction = (handle: TimerHandle) => void;\n\ninterface TimeoutProvider {\n setTimeout: SetTimeoutFunction;\n clearTimeout: ClearTimeoutFunction;\n delegate:\n | {\n setTimeout: SetTimeoutFunction;\n clearTimeout: ClearTimeoutFunction;\n }\n | undefined;\n}\n\nexport const timeoutProvider: TimeoutProvider = {\n // When accessing the delegate, use the variable rather than `this` so that\n // the functions can be called without being bound to the provider.\n setTimeout(handler: () => void, timeout?: number, ...args) {\n const { delegate } = timeoutProvider;\n if (delegate?.setTimeout) {\n return delegate.setTimeout(handler, timeout, ...args);\n }\n return setTimeout(handler, timeout, ...args);\n },\n clearTimeout(handle) {\n const { delegate } = timeoutProvider;\n return (delegate?.clearTimeout || clearTimeout)(handle as any);\n },\n delegate: undefined,\n};\n", "import { config } from '../config';\nimport { timeoutProvider } from '../scheduler/timeoutProvider';\n\n/**\n * Handles an error on another job either with the user-configured {@link onUnhandledError},\n * or by throwing it on that new job so it can be picked up by `window.onerror`, `process.on('error')`, etc.\n *\n * This should be called whenever there is an error that is out-of-band with the subscription\n * or when an error hits a terminal boundary of the subscription and no error handler was provided.\n *\n * @param err the error to report\n */\nexport function reportUnhandledError(err: any) {\n timeoutProvider.setTimeout(() => {\n const { onUnhandledError } = config;\n if (onUnhandledError) {\n // Execute the user-configured error handler.\n onUnhandledError(err);\n } else {\n // Throw so it is picked up by the runtime's uncaught error mechanism.\n throw err;\n }\n });\n}\n", "/* tslint:disable:no-empty */\nexport function noop() { }\n", "import { CompleteNotification, NextNotification, ErrorNotification } from './types';\n\n/**\n * A completion object optimized for memory use and created to be the\n * same \"shape\" as other notifications in v8.\n * @internal\n */\nexport const COMPLETE_NOTIFICATION = (() => createNotification('C', undefined, undefined) as CompleteNotification)();\n\n/**\n * Internal use only. Creates an optimized error notification that is the same \"shape\"\n * as other notifications.\n * @internal\n */\nexport function errorNotification(error: any): ErrorNotification {\n return createNotification('E', undefined, error) as any;\n}\n\n/**\n * Internal use only. Creates an optimized next notification that is the same \"shape\"\n * as other notifications.\n * @internal\n */\nexport function nextNotification(value: T) {\n return createNotification('N', value, undefined) as NextNotification;\n}\n\n/**\n * Ensures that all notifications created internally have the same \"shape\" in v8.\n *\n * TODO: This is only exported to support a crazy legacy test in `groupBy`.\n * @internal\n */\nexport function createNotification(kind: 'N' | 'E' | 'C', value: any, error: any) {\n return {\n kind,\n value,\n error,\n };\n}\n", "import { config } from '../config';\n\nlet context: { errorThrown: boolean; error: any } | null = null;\n\n/**\n * Handles dealing with errors for super-gross mode. Creates a context, in which\n * any synchronously thrown errors will be passed to {@link captureError}. Which\n * will record the error such that it will be rethrown after the call back is complete.\n * TODO: Remove in v8\n * @param cb An immediately executed function.\n */\nexport function errorContext(cb: () => void) {\n if (config.useDeprecatedSynchronousErrorHandling) {\n const isRoot = !context;\n if (isRoot) {\n context = { errorThrown: false, error: null };\n }\n cb();\n if (isRoot) {\n const { errorThrown, error } = context!;\n context = null;\n if (errorThrown) {\n throw error;\n }\n }\n } else {\n // This is the general non-deprecated path for everyone that\n // isn't crazy enough to use super-gross mode (useDeprecatedSynchronousErrorHandling)\n cb();\n }\n}\n\n/**\n * Captures errors only in super-gross mode.\n * @param err the error to capture\n */\nexport function captureError(err: any) {\n if (config.useDeprecatedSynchronousErrorHandling && context) {\n context.errorThrown = true;\n context.error = err;\n }\n}\n", "import { isFunction } from './util/isFunction';\nimport { Observer, ObservableNotification } from './types';\nimport { isSubscription, Subscription } from './Subscription';\nimport { config } from './config';\nimport { reportUnhandledError } from './util/reportUnhandledError';\nimport { noop } from './util/noop';\nimport { nextNotification, errorNotification, COMPLETE_NOTIFICATION } from './NotificationFactories';\nimport { timeoutProvider } from './scheduler/timeoutProvider';\nimport { captureError } from './util/errorContext';\n\n/**\n * Implements the {@link Observer} interface and extends the\n * {@link Subscription} class. While the {@link Observer} is the public API for\n * consuming the values of an {@link Observable}, all Observers get converted to\n * a Subscriber, in order to provide Subscription-like capabilities such as\n * `unsubscribe`. Subscriber is a common type in RxJS, and crucial for\n * implementing operators, but it is rarely used as a public API.\n *\n * @class Subscriber\n */\nexport class Subscriber extends Subscription implements Observer {\n /**\n * A static factory for a Subscriber, given a (potentially partial) definition\n * of an Observer.\n * @param next The `next` callback of an Observer.\n * @param error The `error` callback of an\n * Observer.\n * @param complete The `complete` callback of an\n * Observer.\n * @return A Subscriber wrapping the (partially defined)\n * Observer represented by the given arguments.\n * @nocollapse\n * @deprecated Do not use. Will be removed in v8. There is no replacement for this\n * method, and there is no reason to be creating instances of `Subscriber` directly.\n * If you have a specific use case, please file an issue.\n */\n static create(next?: (x?: T) => void, error?: (e?: any) => void, complete?: () => void): Subscriber {\n return new SafeSubscriber(next, error, complete);\n }\n\n /** @deprecated Internal implementation detail, do not use directly. Will be made internal in v8. */\n protected isStopped: boolean = false;\n /** @deprecated Internal implementation detail, do not use directly. Will be made internal in v8. */\n protected destination: Subscriber | Observer; // this `any` is the escape hatch to erase extra type param (e.g. R)\n\n /**\n * @deprecated Internal implementation detail, do not use directly. Will be made internal in v8.\n * There is no reason to directly create an instance of Subscriber. This type is exported for typings reasons.\n */\n constructor(destination?: Subscriber | Observer) {\n super();\n if (destination) {\n this.destination = destination;\n // Automatically chain subscriptions together here.\n // if destination is a Subscription, then it is a Subscriber.\n if (isSubscription(destination)) {\n destination.add(this);\n }\n } else {\n this.destination = EMPTY_OBSERVER;\n }\n }\n\n /**\n * The {@link Observer} callback to receive notifications of type `next` from\n * the Observable, with a value. The Observable may call this method 0 or more\n * times.\n * @param {T} [value] The `next` value.\n * @return {void}\n */\n next(value?: T): void {\n if (this.isStopped) {\n handleStoppedNotification(nextNotification(value), this);\n } else {\n this._next(value!);\n }\n }\n\n /**\n * The {@link Observer} callback to receive notifications of type `error` from\n * the Observable, with an attached `Error`. Notifies the Observer that\n * the Observable has experienced an error condition.\n * @param {any} [err] The `error` exception.\n * @return {void}\n */\n error(err?: any): void {\n if (this.isStopped) {\n handleStoppedNotification(errorNotification(err), this);\n } else {\n this.isStopped = true;\n this._error(err);\n }\n }\n\n /**\n * The {@link Observer} callback to receive a valueless notification of type\n * `complete` from the Observable. Notifies the Observer that the Observable\n * has finished sending push-based notifications.\n * @return {void}\n */\n complete(): void {\n if (this.isStopped) {\n handleStoppedNotification(COMPLETE_NOTIFICATION, this);\n } else {\n this.isStopped = true;\n this._complete();\n }\n }\n\n unsubscribe(): void {\n if (!this.closed) {\n this.isStopped = true;\n super.unsubscribe();\n this.destination = null!;\n }\n }\n\n protected _next(value: T): void {\n this.destination.next(value);\n }\n\n protected _error(err: any): void {\n try {\n this.destination.error(err);\n } finally {\n this.unsubscribe();\n }\n }\n\n protected _complete(): void {\n try {\n this.destination.complete();\n } finally {\n this.unsubscribe();\n }\n }\n}\n\n/**\n * This bind is captured here because we want to be able to have\n * compatibility with monoid libraries that tend to use a method named\n * `bind`. In particular, a library called Monio requires this.\n */\nconst _bind = Function.prototype.bind;\n\nfunction bind any>(fn: Fn, thisArg: any): Fn {\n return _bind.call(fn, thisArg);\n}\n\n/**\n * Internal optimization only, DO NOT EXPOSE.\n * @internal\n */\nclass ConsumerObserver implements Observer {\n constructor(private partialObserver: Partial>) {}\n\n next(value: T): void {\n const { partialObserver } = this;\n if (partialObserver.next) {\n try {\n partialObserver.next(value);\n } catch (error) {\n handleUnhandledError(error);\n }\n }\n }\n\n error(err: any): void {\n const { partialObserver } = this;\n if (partialObserver.error) {\n try {\n partialObserver.error(err);\n } catch (error) {\n handleUnhandledError(error);\n }\n } else {\n handleUnhandledError(err);\n }\n }\n\n complete(): void {\n const { partialObserver } = this;\n if (partialObserver.complete) {\n try {\n partialObserver.complete();\n } catch (error) {\n handleUnhandledError(error);\n }\n }\n }\n}\n\nexport class SafeSubscriber extends Subscriber {\n constructor(\n observerOrNext?: Partial> | ((value: T) => void) | null,\n error?: ((e?: any) => void) | null,\n complete?: (() => void) | null\n ) {\n super();\n\n let partialObserver: Partial>;\n if (isFunction(observerOrNext) || !observerOrNext) {\n // The first argument is a function, not an observer. The next\n // two arguments *could* be observers, or they could be empty.\n partialObserver = {\n next: (observerOrNext ?? undefined) as (((value: T) => void) | undefined),\n error: error ?? undefined,\n complete: complete ?? undefined,\n };\n } else {\n // The first argument is a partial observer.\n let context: any;\n if (this && config.useDeprecatedNextContext) {\n // This is a deprecated path that made `this.unsubscribe()` available in\n // next handler functions passed to subscribe. This only exists behind a flag\n // now, as it is *very* slow.\n context = Object.create(observerOrNext);\n context.unsubscribe = () => this.unsubscribe();\n partialObserver = {\n next: observerOrNext.next && bind(observerOrNext.next, context),\n error: observerOrNext.error && bind(observerOrNext.error, context),\n complete: observerOrNext.complete && bind(observerOrNext.complete, context),\n };\n } else {\n // The \"normal\" path. Just use the partial observer directly.\n partialObserver = observerOrNext;\n }\n }\n\n // Wrap the partial observer to ensure it's a full observer, and\n // make sure proper error handling is accounted for.\n this.destination = new ConsumerObserver(partialObserver);\n }\n}\n\nfunction handleUnhandledError(error: any) {\n if (config.useDeprecatedSynchronousErrorHandling) {\n captureError(error);\n } else {\n // Ideal path, we report this as an unhandled error,\n // which is thrown on a new call stack.\n reportUnhandledError(error);\n }\n}\n\n/**\n * An error handler used when no error handler was supplied\n * to the SafeSubscriber -- meaning no error handler was supplied\n * do the `subscribe` call on our observable.\n * @param err The error to handle\n */\nfunction defaultErrorHandler(err: any) {\n throw err;\n}\n\n/**\n * A handler for notifications that cannot be sent to a stopped subscriber.\n * @param notification The notification being sent\n * @param subscriber The stopped subscriber\n */\nfunction handleStoppedNotification(notification: ObservableNotification, subscriber: Subscriber) {\n const { onStoppedNotification } = config;\n onStoppedNotification && timeoutProvider.setTimeout(() => onStoppedNotification(notification, subscriber));\n}\n\n/**\n * The observer used as a stub for subscriptions where the user did not\n * pass any arguments to `subscribe`. Comes with the default error handling\n * behavior.\n */\nexport const EMPTY_OBSERVER: Readonly> & { closed: true } = {\n closed: true,\n next: noop,\n error: defaultErrorHandler,\n complete: noop,\n};\n", "/**\n * Symbol.observable or a string \"@@observable\". Used for interop\n *\n * @deprecated We will no longer be exporting this symbol in upcoming versions of RxJS.\n * Instead polyfill and use Symbol.observable directly *or* use https://www.npmjs.com/package/symbol-observable\n */\nexport const observable: string | symbol = (() => (typeof Symbol === 'function' && Symbol.observable) || '@@observable')();\n", "/**\n * This function takes one parameter and just returns it. Simply put,\n * this is like `(x: T): T => x`.\n *\n * ## Examples\n *\n * This is useful in some cases when using things like `mergeMap`\n *\n * ```ts\n * import { interval, take, map, range, mergeMap, identity } from 'rxjs';\n *\n * const source$ = interval(1000).pipe(take(5));\n *\n * const result$ = source$.pipe(\n * map(i => range(i)),\n * mergeMap(identity) // same as mergeMap(x => x)\n * );\n *\n * result$.subscribe({\n * next: console.log\n * });\n * ```\n *\n * Or when you want to selectively apply an operator\n *\n * ```ts\n * import { interval, take, identity } from 'rxjs';\n *\n * const shouldLimit = () => Math.random() < 0.5;\n *\n * const source$ = interval(1000);\n *\n * const result$ = source$.pipe(shouldLimit() ? take(5) : identity);\n *\n * result$.subscribe({\n * next: console.log\n * });\n * ```\n *\n * @param x Any value that is returned by this function\n * @returns The value passed as the first parameter to this function\n */\nexport function identity(x: T): T {\n return x;\n}\n", "import { identity } from './identity';\nimport { UnaryFunction } from '../types';\n\nexport function pipe(): typeof identity;\nexport function pipe(fn1: UnaryFunction): UnaryFunction;\nexport function pipe(fn1: UnaryFunction, fn2: UnaryFunction): UnaryFunction;\nexport function pipe(fn1: UnaryFunction, fn2: UnaryFunction, fn3: UnaryFunction): UnaryFunction;\nexport function pipe(\n fn1: UnaryFunction,\n fn2: UnaryFunction,\n fn3: UnaryFunction,\n fn4: UnaryFunction\n): UnaryFunction;\nexport function pipe(\n fn1: UnaryFunction,\n fn2: UnaryFunction,\n fn3: UnaryFunction,\n fn4: UnaryFunction,\n fn5: UnaryFunction\n): UnaryFunction;\nexport function pipe(\n fn1: UnaryFunction,\n fn2: UnaryFunction,\n fn3: UnaryFunction,\n fn4: UnaryFunction,\n fn5: UnaryFunction,\n fn6: UnaryFunction\n): UnaryFunction;\nexport function pipe(\n fn1: UnaryFunction,\n fn2: UnaryFunction,\n fn3: UnaryFunction,\n fn4: UnaryFunction,\n fn5: UnaryFunction,\n fn6: UnaryFunction,\n fn7: UnaryFunction\n): UnaryFunction;\nexport function pipe(\n fn1: UnaryFunction,\n fn2: UnaryFunction,\n fn3: UnaryFunction,\n fn4: UnaryFunction,\n fn5: UnaryFunction,\n fn6: UnaryFunction,\n fn7: UnaryFunction,\n fn8: UnaryFunction\n): UnaryFunction;\nexport function pipe(\n fn1: UnaryFunction,\n fn2: UnaryFunction,\n fn3: UnaryFunction,\n fn4: UnaryFunction,\n fn5: UnaryFunction,\n fn6: UnaryFunction,\n fn7: UnaryFunction,\n fn8: UnaryFunction,\n fn9: UnaryFunction\n): UnaryFunction;\nexport function pipe(\n fn1: UnaryFunction,\n fn2: UnaryFunction,\n fn3: UnaryFunction,\n fn4: UnaryFunction,\n fn5: UnaryFunction,\n fn6: UnaryFunction,\n fn7: UnaryFunction,\n fn8: UnaryFunction,\n fn9: UnaryFunction,\n ...fns: UnaryFunction[]\n): UnaryFunction;\n\n/**\n * pipe() can be called on one or more functions, each of which can take one argument (\"UnaryFunction\")\n * and uses it to return a value.\n * It returns a function that takes one argument, passes it to the first UnaryFunction, and then\n * passes the result to the next one, passes that result to the next one, and so on. \n */\nexport function pipe(...fns: Array>): UnaryFunction {\n return pipeFromArray(fns);\n}\n\n/** @internal */\nexport function pipeFromArray(fns: Array>): UnaryFunction {\n if (fns.length === 0) {\n return identity as UnaryFunction;\n }\n\n if (fns.length === 1) {\n return fns[0];\n }\n\n return function piped(input: T): R {\n return fns.reduce((prev: any, fn: UnaryFunction) => fn(prev), input as any);\n };\n}\n", "import { Operator } from './Operator';\nimport { SafeSubscriber, Subscriber } from './Subscriber';\nimport { isSubscription, Subscription } from './Subscription';\nimport { TeardownLogic, OperatorFunction, Subscribable, Observer } from './types';\nimport { observable as Symbol_observable } from './symbol/observable';\nimport { pipeFromArray } from './util/pipe';\nimport { config } from './config';\nimport { isFunction } from './util/isFunction';\nimport { errorContext } from './util/errorContext';\n\n/**\n * A representation of any set of values over any amount of time. This is the most basic building block\n * of RxJS.\n *\n * @class Observable\n */\nexport class Observable implements Subscribable {\n /**\n * @deprecated Internal implementation detail, do not use directly. Will be made internal in v8.\n */\n source: Observable | undefined;\n\n /**\n * @deprecated Internal implementation detail, do not use directly. Will be made internal in v8.\n */\n operator: Operator | undefined;\n\n /**\n * @constructor\n * @param {Function} subscribe the function that is called when the Observable is\n * initially subscribed to. This function is given a Subscriber, to which new values\n * can be `next`ed, or an `error` method can be called to raise an error, or\n * `complete` can be called to notify of a successful completion.\n */\n constructor(subscribe?: (this: Observable, subscriber: Subscriber) => TeardownLogic) {\n if (subscribe) {\n this._subscribe = subscribe;\n }\n }\n\n // HACK: Since TypeScript inherits static properties too, we have to\n // fight against TypeScript here so Subject can have a different static create signature\n /**\n * Creates a new Observable by calling the Observable constructor\n * @owner Observable\n * @method create\n * @param {Function} subscribe? the subscriber function to be passed to the Observable constructor\n * @return {Observable} a new observable\n * @nocollapse\n * @deprecated Use `new Observable()` instead. Will be removed in v8.\n */\n static create: (...args: any[]) => any = (subscribe?: (subscriber: Subscriber) => TeardownLogic) => {\n return new Observable(subscribe);\n };\n\n /**\n * Creates a new Observable, with this Observable instance as the source, and the passed\n * operator defined as the new observable's operator.\n * @method lift\n * @param operator the operator defining the operation to take on the observable\n * @return a new observable with the Operator applied\n * @deprecated Internal implementation detail, do not use directly. Will be made internal in v8.\n * If you have implemented an operator using `lift`, it is recommended that you create an\n * operator by simply returning `new Observable()` directly. See \"Creating new operators from\n * scratch\" section here: https://rxjs.dev/guide/operators\n */\n lift(operator?: Operator): Observable {\n const observable = new Observable();\n observable.source = this;\n observable.operator = operator;\n return observable;\n }\n\n subscribe(observerOrNext?: Partial> | ((value: T) => void)): Subscription;\n /** @deprecated Instead of passing separate callback arguments, use an observer argument. Signatures taking separate callback arguments will be removed in v8. Details: https://rxjs.dev/deprecations/subscribe-arguments */\n subscribe(next?: ((value: T) => void) | null, error?: ((error: any) => void) | null, complete?: (() => void) | null): Subscription;\n /**\n * Invokes an execution of an Observable and registers Observer handlers for notifications it will emit.\n *\n * Use it when you have all these Observables, but still nothing is happening.\n *\n * `subscribe` is not a regular operator, but a method that calls Observable's internal `subscribe` function. It\n * might be for example a function that you passed to Observable's constructor, but most of the time it is\n * a library implementation, which defines what will be emitted by an Observable, and when it be will emitted. This means\n * that calling `subscribe` is actually the moment when Observable starts its work, not when it is created, as it is often\n * the thought.\n *\n * Apart from starting the execution of an Observable, this method allows you to listen for values\n * that an Observable emits, as well as for when it completes or errors. You can achieve this in two\n * of the following ways.\n *\n * The first way is creating an object that implements {@link Observer} interface. It should have methods\n * defined by that interface, but note that it should be just a regular JavaScript object, which you can create\n * yourself in any way you want (ES6 class, classic function constructor, object literal etc.). In particular, do\n * not attempt to use any RxJS implementation details to create Observers - you don't need them. Remember also\n * that your object does not have to implement all methods. If you find yourself creating a method that doesn't\n * do anything, you can simply omit it. Note however, if the `error` method is not provided and an error happens,\n * it will be thrown asynchronously. Errors thrown asynchronously cannot be caught using `try`/`catch`. Instead,\n * use the {@link onUnhandledError} configuration option or use a runtime handler (like `window.onerror` or\n * `process.on('error)`) to be notified of unhandled errors. Because of this, it's recommended that you provide\n * an `error` method to avoid missing thrown errors.\n *\n * The second way is to give up on Observer object altogether and simply provide callback functions in place of its methods.\n * This means you can provide three functions as arguments to `subscribe`, where the first function is equivalent\n * of a `next` method, the second of an `error` method and the third of a `complete` method. Just as in case of an Observer,\n * if you do not need to listen for something, you can omit a function by passing `undefined` or `null`,\n * since `subscribe` recognizes these functions by where they were placed in function call. When it comes\n * to the `error` function, as with an Observer, if not provided, errors emitted by an Observable will be thrown asynchronously.\n *\n * You can, however, subscribe with no parameters at all. This may be the case where you're not interested in terminal events\n * and you also handled emissions internally by using operators (e.g. using `tap`).\n *\n * Whichever style of calling `subscribe` you use, in both cases it returns a Subscription object.\n * This object allows you to call `unsubscribe` on it, which in turn will stop the work that an Observable does and will clean\n * up all resources that an Observable used. Note that cancelling a subscription will not call `complete` callback\n * provided to `subscribe` function, which is reserved for a regular completion signal that comes from an Observable.\n *\n * Remember that callbacks provided to `subscribe` are not guaranteed to be called asynchronously.\n * It is an Observable itself that decides when these functions will be called. For example {@link of}\n * by default emits all its values synchronously. Always check documentation for how given Observable\n * will behave when subscribed and if its default behavior can be modified with a `scheduler`.\n *\n * #### Examples\n *\n * Subscribe with an {@link guide/observer Observer}\n *\n * ```ts\n * import { of } from 'rxjs';\n *\n * const sumObserver = {\n * sum: 0,\n * next(value) {\n * console.log('Adding: ' + value);\n * this.sum = this.sum + value;\n * },\n * error() {\n * // We actually could just remove this method,\n * // since we do not really care about errors right now.\n * },\n * complete() {\n * console.log('Sum equals: ' + this.sum);\n * }\n * };\n *\n * of(1, 2, 3) // Synchronously emits 1, 2, 3 and then completes.\n * .subscribe(sumObserver);\n *\n * // Logs:\n * // 'Adding: 1'\n * // 'Adding: 2'\n * // 'Adding: 3'\n * // 'Sum equals: 6'\n * ```\n *\n * Subscribe with functions ({@link deprecations/subscribe-arguments deprecated})\n *\n * ```ts\n * import { of } from 'rxjs'\n *\n * let sum = 0;\n *\n * of(1, 2, 3).subscribe(\n * value => {\n * console.log('Adding: ' + value);\n * sum = sum + value;\n * },\n * undefined,\n * () => console.log('Sum equals: ' + sum)\n * );\n *\n * // Logs:\n * // 'Adding: 1'\n * // 'Adding: 2'\n * // 'Adding: 3'\n * // 'Sum equals: 6'\n * ```\n *\n * Cancel a subscription\n *\n * ```ts\n * import { interval } from 'rxjs';\n *\n * const subscription = interval(1000).subscribe({\n * next(num) {\n * console.log(num)\n * },\n * complete() {\n * // Will not be called, even when cancelling subscription.\n * console.log('completed!');\n * }\n * });\n *\n * setTimeout(() => {\n * subscription.unsubscribe();\n * console.log('unsubscribed!');\n * }, 2500);\n *\n * // Logs:\n * // 0 after 1s\n * // 1 after 2s\n * // 'unsubscribed!' after 2.5s\n * ```\n *\n * @param {Observer|Function} observerOrNext (optional) Either an observer with methods to be called,\n * or the first of three possible handlers, which is the handler for each value emitted from the subscribed\n * Observable.\n * @param {Function} error (optional) A handler for a terminal event resulting from an error. If no error handler is provided,\n * the error will be thrown asynchronously as unhandled.\n * @param {Function} complete (optional) A handler for a terminal event resulting from successful completion.\n * @return {Subscription} a subscription reference to the registered handlers\n * @method subscribe\n */\n subscribe(\n observerOrNext?: Partial> | ((value: T) => void) | null,\n error?: ((error: any) => void) | null,\n complete?: (() => void) | null\n ): Subscription {\n const subscriber = isSubscriber(observerOrNext) ? observerOrNext : new SafeSubscriber(observerOrNext, error, complete);\n\n errorContext(() => {\n const { operator, source } = this;\n subscriber.add(\n operator\n ? // We're dealing with a subscription in the\n // operator chain to one of our lifted operators.\n operator.call(subscriber, source)\n : source\n ? // If `source` has a value, but `operator` does not, something that\n // had intimate knowledge of our API, like our `Subject`, must have\n // set it. We're going to just call `_subscribe` directly.\n this._subscribe(subscriber)\n : // In all other cases, we're likely wrapping a user-provided initializer\n // function, so we need to catch errors and handle them appropriately.\n this._trySubscribe(subscriber)\n );\n });\n\n return subscriber;\n }\n\n /** @internal */\n protected _trySubscribe(sink: Subscriber): TeardownLogic {\n try {\n return this._subscribe(sink);\n } catch (err) {\n // We don't need to return anything in this case,\n // because it's just going to try to `add()` to a subscription\n // above.\n sink.error(err);\n }\n }\n\n /**\n * Used as a NON-CANCELLABLE means of subscribing to an observable, for use with\n * APIs that expect promises, like `async/await`. You cannot unsubscribe from this.\n *\n * **WARNING**: Only use this with observables you *know* will complete. If the source\n * observable does not complete, you will end up with a promise that is hung up, and\n * potentially all of the state of an async function hanging out in memory. To avoid\n * this situation, look into adding something like {@link timeout}, {@link take},\n * {@link takeWhile}, or {@link takeUntil} amongst others.\n *\n * #### Example\n *\n * ```ts\n * import { interval, take } from 'rxjs';\n *\n * const source$ = interval(1000).pipe(take(4));\n *\n * async function getTotal() {\n * let total = 0;\n *\n * await source$.forEach(value => {\n * total += value;\n * console.log('observable -> ' + value);\n * });\n *\n * return total;\n * }\n *\n * getTotal().then(\n * total => console.log('Total: ' + total)\n * );\n *\n * // Expected:\n * // 'observable -> 0'\n * // 'observable -> 1'\n * // 'observable -> 2'\n * // 'observable -> 3'\n * // 'Total: 6'\n * ```\n *\n * @param next a handler for each value emitted by the observable\n * @return a promise that either resolves on observable completion or\n * rejects with the handled error\n */\n forEach(next: (value: T) => void): Promise;\n\n /**\n * @param next a handler for each value emitted by the observable\n * @param promiseCtor a constructor function used to instantiate the Promise\n * @return a promise that either resolves on observable completion or\n * rejects with the handled error\n * @deprecated Passing a Promise constructor will no longer be available\n * in upcoming versions of RxJS. This is because it adds weight to the library, for very\n * little benefit. If you need this functionality, it is recommended that you either\n * polyfill Promise, or you create an adapter to convert the returned native promise\n * to whatever promise implementation you wanted. Will be removed in v8.\n */\n forEach(next: (value: T) => void, promiseCtor: PromiseConstructorLike): Promise;\n\n forEach(next: (value: T) => void, promiseCtor?: PromiseConstructorLike): Promise {\n promiseCtor = getPromiseCtor(promiseCtor);\n\n return new promiseCtor((resolve, reject) => {\n const subscriber = new SafeSubscriber({\n next: (value) => {\n try {\n next(value);\n } catch (err) {\n reject(err);\n subscriber.unsubscribe();\n }\n },\n error: reject,\n complete: resolve,\n });\n this.subscribe(subscriber);\n }) as Promise;\n }\n\n /** @internal */\n protected _subscribe(subscriber: Subscriber): TeardownLogic {\n return this.source?.subscribe(subscriber);\n }\n\n /**\n * An interop point defined by the es7-observable spec https://github.com/zenparsing/es-observable\n * @method Symbol.observable\n * @return {Observable} this instance of the observable\n */\n [Symbol_observable]() {\n return this;\n }\n\n /* tslint:disable:max-line-length */\n pipe(): Observable;\n pipe(op1: OperatorFunction): Observable;\n pipe(op1: OperatorFunction, op2: OperatorFunction): Observable;\n pipe(op1: OperatorFunction, op2: OperatorFunction, op3: OperatorFunction): Observable;\n pipe(\n op1: OperatorFunction,\n op2: OperatorFunction,\n op3: OperatorFunction,\n op4: OperatorFunction\n ): Observable;\n pipe(\n op1: OperatorFunction,\n op2: OperatorFunction,\n op3: OperatorFunction,\n op4: OperatorFunction,\n op5: OperatorFunction\n ): Observable;\n pipe(\n op1: OperatorFunction,\n op2: OperatorFunction,\n op3: OperatorFunction,\n op4: OperatorFunction,\n op5: OperatorFunction,\n op6: OperatorFunction\n ): Observable;\n pipe(\n op1: OperatorFunction,\n op2: OperatorFunction,\n op3: OperatorFunction,\n op4: OperatorFunction,\n op5: OperatorFunction,\n op6: OperatorFunction,\n op7: OperatorFunction\n ): Observable;\n pipe(\n op1: OperatorFunction,\n op2: OperatorFunction,\n op3: OperatorFunction,\n op4: OperatorFunction,\n op5: OperatorFunction,\n op6: OperatorFunction,\n op7: OperatorFunction,\n op8: OperatorFunction\n ): Observable;\n pipe(\n op1: OperatorFunction,\n op2: OperatorFunction,\n op3: OperatorFunction,\n op4: OperatorFunction,\n op5: OperatorFunction,\n op6: OperatorFunction,\n op7: OperatorFunction,\n op8: OperatorFunction,\n op9: OperatorFunction\n ): Observable;\n pipe(\n op1: OperatorFunction,\n op2: OperatorFunction,\n op3: OperatorFunction,\n op4: OperatorFunction,\n op5: OperatorFunction,\n op6: OperatorFunction,\n op7: OperatorFunction,\n op8: OperatorFunction,\n op9: OperatorFunction,\n ...operations: OperatorFunction[]\n ): Observable;\n /* tslint:enable:max-line-length */\n\n /**\n * Used to stitch together functional operators into a chain.\n * @method pipe\n * @return {Observable} the Observable result of all of the operators having\n * been called in the order they were passed in.\n *\n * ## Example\n *\n * ```ts\n * import { interval, filter, map, scan } from 'rxjs';\n *\n * interval(1000)\n * .pipe(\n * filter(x => x % 2 === 0),\n * map(x => x + x),\n * scan((acc, x) => acc + x)\n * )\n * .subscribe(x => console.log(x));\n * ```\n */\n pipe(...operations: OperatorFunction[]): Observable {\n return pipeFromArray(operations)(this);\n }\n\n /* tslint:disable:max-line-length */\n /** @deprecated Replaced with {@link firstValueFrom} and {@link lastValueFrom}. Will be removed in v8. Details: https://rxjs.dev/deprecations/to-promise */\n toPromise(): Promise;\n /** @deprecated Replaced with {@link firstValueFrom} and {@link lastValueFrom}. Will be removed in v8. Details: https://rxjs.dev/deprecations/to-promise */\n toPromise(PromiseCtor: typeof Promise): Promise;\n /** @deprecated Replaced with {@link firstValueFrom} and {@link lastValueFrom}. Will be removed in v8. Details: https://rxjs.dev/deprecations/to-promise */\n toPromise(PromiseCtor: PromiseConstructorLike): Promise;\n /* tslint:enable:max-line-length */\n\n /**\n * Subscribe to this Observable and get a Promise resolving on\n * `complete` with the last emission (if any).\n *\n * **WARNING**: Only use this with observables you *know* will complete. If the source\n * observable does not complete, you will end up with a promise that is hung up, and\n * potentially all of the state of an async function hanging out in memory. To avoid\n * this situation, look into adding something like {@link timeout}, {@link take},\n * {@link takeWhile}, or {@link takeUntil} amongst others.\n *\n * @method toPromise\n * @param [promiseCtor] a constructor function used to instantiate\n * the Promise\n * @return A Promise that resolves with the last value emit, or\n * rejects on an error. If there were no emissions, Promise\n * resolves with undefined.\n * @deprecated Replaced with {@link firstValueFrom} and {@link lastValueFrom}. Will be removed in v8. Details: https://rxjs.dev/deprecations/to-promise\n */\n toPromise(promiseCtor?: PromiseConstructorLike): Promise {\n promiseCtor = getPromiseCtor(promiseCtor);\n\n return new promiseCtor((resolve, reject) => {\n let value: T | undefined;\n this.subscribe(\n (x: T) => (value = x),\n (err: any) => reject(err),\n () => resolve(value)\n );\n }) as Promise;\n }\n}\n\n/**\n * Decides between a passed promise constructor from consuming code,\n * A default configured promise constructor, and the native promise\n * constructor and returns it. If nothing can be found, it will throw\n * an error.\n * @param promiseCtor The optional promise constructor to passed by consuming code\n */\nfunction getPromiseCtor(promiseCtor: PromiseConstructorLike | undefined) {\n return promiseCtor ?? config.Promise ?? Promise;\n}\n\nfunction isObserver(value: any): value is Observer {\n return value && isFunction(value.next) && isFunction(value.error) && isFunction(value.complete);\n}\n\nfunction isSubscriber(value: any): value is Subscriber {\n return (value && value instanceof Subscriber) || (isObserver(value) && isSubscription(value));\n}\n", "import { Observable } from '../Observable';\nimport { Subscriber } from '../Subscriber';\nimport { OperatorFunction } from '../types';\nimport { isFunction } from './isFunction';\n\n/**\n * Used to determine if an object is an Observable with a lift function.\n */\nexport function hasLift(source: any): source is { lift: InstanceType['lift'] } {\n return isFunction(source?.lift);\n}\n\n/**\n * Creates an `OperatorFunction`. Used to define operators throughout the library in a concise way.\n * @param init The logic to connect the liftedSource to the subscriber at the moment of subscription.\n */\nexport function operate(\n init: (liftedSource: Observable, subscriber: Subscriber) => (() => void) | void\n): OperatorFunction {\n return (source: Observable) => {\n if (hasLift(source)) {\n return source.lift(function (this: Subscriber, liftedSource: Observable) {\n try {\n return init(liftedSource, this);\n } catch (err) {\n this.error(err);\n }\n });\n }\n throw new TypeError('Unable to lift unknown Observable type');\n };\n}\n", "import { Subscriber } from '../Subscriber';\n\n/**\n * Creates an instance of an `OperatorSubscriber`.\n * @param destination The downstream subscriber.\n * @param onNext Handles next values, only called if this subscriber is not stopped or closed. Any\n * error that occurs in this function is caught and sent to the `error` method of this subscriber.\n * @param onError Handles errors from the subscription, any errors that occur in this handler are caught\n * and send to the `destination` error handler.\n * @param onComplete Handles completion notification from the subscription. Any errors that occur in\n * this handler are sent to the `destination` error handler.\n * @param onFinalize Additional teardown logic here. This will only be called on teardown if the\n * subscriber itself is not already closed. This is called after all other teardown logic is executed.\n */\nexport function createOperatorSubscriber(\n destination: Subscriber,\n onNext?: (value: T) => void,\n onComplete?: () => void,\n onError?: (err: any) => void,\n onFinalize?: () => void\n): Subscriber {\n return new OperatorSubscriber(destination, onNext, onComplete, onError, onFinalize);\n}\n\n/**\n * A generic helper for allowing operators to be created with a Subscriber and\n * use closures to capture necessary state from the operator function itself.\n */\nexport class OperatorSubscriber extends Subscriber {\n /**\n * Creates an instance of an `OperatorSubscriber`.\n * @param destination The downstream subscriber.\n * @param onNext Handles next values, only called if this subscriber is not stopped or closed. Any\n * error that occurs in this function is caught and sent to the `error` method of this subscriber.\n * @param onError Handles errors from the subscription, any errors that occur in this handler are caught\n * and send to the `destination` error handler.\n * @param onComplete Handles completion notification from the subscription. Any errors that occur in\n * this handler are sent to the `destination` error handler.\n * @param onFinalize Additional finalization logic here. This will only be called on finalization if the\n * subscriber itself is not already closed. This is called after all other finalization logic is executed.\n * @param shouldUnsubscribe An optional check to see if an unsubscribe call should truly unsubscribe.\n * NOTE: This currently **ONLY** exists to support the strange behavior of {@link groupBy}, where unsubscription\n * to the resulting observable does not actually disconnect from the source if there are active subscriptions\n * to any grouped observable. (DO NOT EXPOSE OR USE EXTERNALLY!!!)\n */\n constructor(\n destination: Subscriber,\n onNext?: (value: T) => void,\n onComplete?: () => void,\n onError?: (err: any) => void,\n private onFinalize?: () => void,\n private shouldUnsubscribe?: () => boolean\n ) {\n // It's important - for performance reasons - that all of this class's\n // members are initialized and that they are always initialized in the same\n // order. This will ensure that all OperatorSubscriber instances have the\n // same hidden class in V8. This, in turn, will help keep the number of\n // hidden classes involved in property accesses within the base class as\n // low as possible. If the number of hidden classes involved exceeds four,\n // the property accesses will become megamorphic and performance penalties\n // will be incurred - i.e. inline caches won't be used.\n //\n // The reasons for ensuring all instances have the same hidden class are\n // further discussed in this blog post from Benedikt Meurer:\n // https://benediktmeurer.de/2018/03/23/impact-of-polymorphism-on-component-based-frameworks-like-react/\n super(destination);\n this._next = onNext\n ? function (this: OperatorSubscriber, value: T) {\n try {\n onNext(value);\n } catch (err) {\n destination.error(err);\n }\n }\n : super._next;\n this._error = onError\n ? function (this: OperatorSubscriber, err: any) {\n try {\n onError(err);\n } catch (err) {\n // Send any errors that occur down stream.\n destination.error(err);\n } finally {\n // Ensure finalization.\n this.unsubscribe();\n }\n }\n : super._error;\n this._complete = onComplete\n ? function (this: OperatorSubscriber) {\n try {\n onComplete();\n } catch (err) {\n // Send any errors that occur down stream.\n destination.error(err);\n } finally {\n // Ensure finalization.\n this.unsubscribe();\n }\n }\n : super._complete;\n }\n\n unsubscribe() {\n if (!this.shouldUnsubscribe || this.shouldUnsubscribe()) {\n const { closed } = this;\n super.unsubscribe();\n // Execute additional teardown if we have any and we didn't already do so.\n !closed && this.onFinalize?.();\n }\n }\n}\n", "import { Subscription } from '../Subscription';\n\ninterface AnimationFrameProvider {\n schedule(callback: FrameRequestCallback): Subscription;\n requestAnimationFrame: typeof requestAnimationFrame;\n cancelAnimationFrame: typeof cancelAnimationFrame;\n delegate:\n | {\n requestAnimationFrame: typeof requestAnimationFrame;\n cancelAnimationFrame: typeof cancelAnimationFrame;\n }\n | undefined;\n}\n\nexport const animationFrameProvider: AnimationFrameProvider = {\n // When accessing the delegate, use the variable rather than `this` so that\n // the functions can be called without being bound to the provider.\n schedule(callback) {\n let request = requestAnimationFrame;\n let cancel: typeof cancelAnimationFrame | undefined = cancelAnimationFrame;\n const { delegate } = animationFrameProvider;\n if (delegate) {\n request = delegate.requestAnimationFrame;\n cancel = delegate.cancelAnimationFrame;\n }\n const handle = request((timestamp) => {\n // Clear the cancel function. The request has been fulfilled, so\n // attempting to cancel the request upon unsubscription would be\n // pointless.\n cancel = undefined;\n callback(timestamp);\n });\n return new Subscription(() => cancel?.(handle));\n },\n requestAnimationFrame(...args) {\n const { delegate } = animationFrameProvider;\n return (delegate?.requestAnimationFrame || requestAnimationFrame)(...args);\n },\n cancelAnimationFrame(...args) {\n const { delegate } = animationFrameProvider;\n return (delegate?.cancelAnimationFrame || cancelAnimationFrame)(...args);\n },\n delegate: undefined,\n};\n", "import { createErrorClass } from './createErrorClass';\n\nexport interface ObjectUnsubscribedError extends Error {}\n\nexport interface ObjectUnsubscribedErrorCtor {\n /**\n * @deprecated Internal implementation detail. Do not construct error instances.\n * Cannot be tagged as internal: https://github.com/ReactiveX/rxjs/issues/6269\n */\n new (): ObjectUnsubscribedError;\n}\n\n/**\n * An error thrown when an action is invalid because the object has been\n * unsubscribed.\n *\n * @see {@link Subject}\n * @see {@link BehaviorSubject}\n *\n * @class ObjectUnsubscribedError\n */\nexport const ObjectUnsubscribedError: ObjectUnsubscribedErrorCtor = createErrorClass(\n (_super) =>\n function ObjectUnsubscribedErrorImpl(this: any) {\n _super(this);\n this.name = 'ObjectUnsubscribedError';\n this.message = 'object unsubscribed';\n }\n);\n", "import { Operator } from './Operator';\nimport { Observable } from './Observable';\nimport { Subscriber } from './Subscriber';\nimport { Subscription, EMPTY_SUBSCRIPTION } from './Subscription';\nimport { Observer, SubscriptionLike, TeardownLogic } from './types';\nimport { ObjectUnsubscribedError } from './util/ObjectUnsubscribedError';\nimport { arrRemove } from './util/arrRemove';\nimport { errorContext } from './util/errorContext';\n\n/**\n * A Subject is a special type of Observable that allows values to be\n * multicasted to many Observers. Subjects are like EventEmitters.\n *\n * Every Subject is an Observable and an Observer. You can subscribe to a\n * Subject, and you can call next to feed values as well as error and complete.\n */\nexport class Subject extends Observable implements SubscriptionLike {\n closed = false;\n\n private currentObservers: Observer[] | null = null;\n\n /** @deprecated Internal implementation detail, do not use directly. Will be made internal in v8. */\n observers: Observer[] = [];\n /** @deprecated Internal implementation detail, do not use directly. Will be made internal in v8. */\n isStopped = false;\n /** @deprecated Internal implementation detail, do not use directly. Will be made internal in v8. */\n hasError = false;\n /** @deprecated Internal implementation detail, do not use directly. Will be made internal in v8. */\n thrownError: any = null;\n\n /**\n * Creates a \"subject\" by basically gluing an observer to an observable.\n *\n * @nocollapse\n * @deprecated Recommended you do not use. Will be removed at some point in the future. Plans for replacement still under discussion.\n */\n static create: (...args: any[]) => any = (destination: Observer, source: Observable): AnonymousSubject => {\n return new AnonymousSubject(destination, source);\n };\n\n constructor() {\n // NOTE: This must be here to obscure Observable's constructor.\n super();\n }\n\n /** @deprecated Internal implementation detail, do not use directly. Will be made internal in v8. */\n lift(operator: Operator): Observable {\n const subject = new AnonymousSubject(this, this);\n subject.operator = operator as any;\n return subject as any;\n }\n\n /** @internal */\n protected _throwIfClosed() {\n if (this.closed) {\n throw new ObjectUnsubscribedError();\n }\n }\n\n next(value: T) {\n errorContext(() => {\n this._throwIfClosed();\n if (!this.isStopped) {\n if (!this.currentObservers) {\n this.currentObservers = Array.from(this.observers);\n }\n for (const observer of this.currentObservers) {\n observer.next(value);\n }\n }\n });\n }\n\n error(err: any) {\n errorContext(() => {\n this._throwIfClosed();\n if (!this.isStopped) {\n this.hasError = this.isStopped = true;\n this.thrownError = err;\n const { observers } = this;\n while (observers.length) {\n observers.shift()!.error(err);\n }\n }\n });\n }\n\n complete() {\n errorContext(() => {\n this._throwIfClosed();\n if (!this.isStopped) {\n this.isStopped = true;\n const { observers } = this;\n while (observers.length) {\n observers.shift()!.complete();\n }\n }\n });\n }\n\n unsubscribe() {\n this.isStopped = this.closed = true;\n this.observers = this.currentObservers = null!;\n }\n\n get observed() {\n return this.observers?.length > 0;\n }\n\n /** @internal */\n protected _trySubscribe(subscriber: Subscriber): TeardownLogic {\n this._throwIfClosed();\n return super._trySubscribe(subscriber);\n }\n\n /** @internal */\n protected _subscribe(subscriber: Subscriber): Subscription {\n this._throwIfClosed();\n this._checkFinalizedStatuses(subscriber);\n return this._innerSubscribe(subscriber);\n }\n\n /** @internal */\n protected _innerSubscribe(subscriber: Subscriber) {\n const { hasError, isStopped, observers } = this;\n if (hasError || isStopped) {\n return EMPTY_SUBSCRIPTION;\n }\n this.currentObservers = null;\n observers.push(subscriber);\n return new Subscription(() => {\n this.currentObservers = null;\n arrRemove(observers, subscriber);\n });\n }\n\n /** @internal */\n protected _checkFinalizedStatuses(subscriber: Subscriber) {\n const { hasError, thrownError, isStopped } = this;\n if (hasError) {\n subscriber.error(thrownError);\n } else if (isStopped) {\n subscriber.complete();\n }\n }\n\n /**\n * Creates a new Observable with this Subject as the source. You can do this\n * to create custom Observer-side logic of the Subject and conceal it from\n * code that uses the Observable.\n * @return {Observable} Observable that the Subject casts to\n */\n asObservable(): Observable {\n const observable: any = new Observable();\n observable.source = this;\n return observable;\n }\n}\n\n/**\n * @class AnonymousSubject\n */\nexport class AnonymousSubject extends Subject {\n constructor(\n /** @deprecated Internal implementation detail, do not use directly. Will be made internal in v8. */\n public destination?: Observer,\n source?: Observable\n ) {\n super();\n this.source = source;\n }\n\n next(value: T) {\n this.destination?.next?.(value);\n }\n\n error(err: any) {\n this.destination?.error?.(err);\n }\n\n complete() {\n this.destination?.complete?.();\n }\n\n /** @internal */\n protected _subscribe(subscriber: Subscriber): Subscription {\n return this.source?.subscribe(subscriber) ?? EMPTY_SUBSCRIPTION;\n }\n}\n", "import { Subject } from './Subject';\nimport { Subscriber } from './Subscriber';\nimport { Subscription } from './Subscription';\n\n/**\n * A variant of Subject that requires an initial value and emits its current\n * value whenever it is subscribed to.\n *\n * @class BehaviorSubject\n */\nexport class BehaviorSubject extends Subject {\n constructor(private _value: T) {\n super();\n }\n\n get value(): T {\n return this.getValue();\n }\n\n /** @internal */\n protected _subscribe(subscriber: Subscriber): Subscription {\n const subscription = super._subscribe(subscriber);\n !subscription.closed && subscriber.next(this._value);\n return subscription;\n }\n\n getValue(): T {\n const { hasError, thrownError, _value } = this;\n if (hasError) {\n throw thrownError;\n }\n this._throwIfClosed();\n return _value;\n }\n\n next(value: T): void {\n super.next((this._value = value));\n }\n}\n", "import { TimestampProvider } from '../types';\n\ninterface DateTimestampProvider extends TimestampProvider {\n delegate: TimestampProvider | undefined;\n}\n\nexport const dateTimestampProvider: DateTimestampProvider = {\n now() {\n // Use the variable rather than `this` so that the function can be called\n // without being bound to the provider.\n return (dateTimestampProvider.delegate || Date).now();\n },\n delegate: undefined,\n};\n", "import { Subject } from './Subject';\nimport { TimestampProvider } from './types';\nimport { Subscriber } from './Subscriber';\nimport { Subscription } from './Subscription';\nimport { dateTimestampProvider } from './scheduler/dateTimestampProvider';\n\n/**\n * A variant of {@link Subject} that \"replays\" old values to new subscribers by emitting them when they first subscribe.\n *\n * `ReplaySubject` has an internal buffer that will store a specified number of values that it has observed. Like `Subject`,\n * `ReplaySubject` \"observes\" values by having them passed to its `next` method. When it observes a value, it will store that\n * value for a time determined by the configuration of the `ReplaySubject`, as passed to its constructor.\n *\n * When a new subscriber subscribes to the `ReplaySubject` instance, it will synchronously emit all values in its buffer in\n * a First-In-First-Out (FIFO) manner. The `ReplaySubject` will also complete, if it has observed completion; and it will\n * error if it has observed an error.\n *\n * There are two main configuration items to be concerned with:\n *\n * 1. `bufferSize` - This will determine how many items are stored in the buffer, defaults to infinite.\n * 2. `windowTime` - The amount of time to hold a value in the buffer before removing it from the buffer.\n *\n * Both configurations may exist simultaneously. So if you would like to buffer a maximum of 3 values, as long as the values\n * are less than 2 seconds old, you could do so with a `new ReplaySubject(3, 2000)`.\n *\n * ### Differences with BehaviorSubject\n *\n * `BehaviorSubject` is similar to `new ReplaySubject(1)`, with a couple of exceptions:\n *\n * 1. `BehaviorSubject` comes \"primed\" with a single value upon construction.\n * 2. `ReplaySubject` will replay values, even after observing an error, where `BehaviorSubject` will not.\n *\n * @see {@link Subject}\n * @see {@link BehaviorSubject}\n * @see {@link shareReplay}\n */\nexport class ReplaySubject extends Subject {\n private _buffer: (T | number)[] = [];\n private _infiniteTimeWindow = true;\n\n /**\n * @param bufferSize The size of the buffer to replay on subscription\n * @param windowTime The amount of time the buffered items will stay buffered\n * @param timestampProvider An object with a `now()` method that provides the current timestamp. This is used to\n * calculate the amount of time something has been buffered.\n */\n constructor(\n private _bufferSize = Infinity,\n private _windowTime = Infinity,\n private _timestampProvider: TimestampProvider = dateTimestampProvider\n ) {\n super();\n this._infiniteTimeWindow = _windowTime === Infinity;\n this._bufferSize = Math.max(1, _bufferSize);\n this._windowTime = Math.max(1, _windowTime);\n }\n\n next(value: T): void {\n const { isStopped, _buffer, _infiniteTimeWindow, _timestampProvider, _windowTime } = this;\n if (!isStopped) {\n _buffer.push(value);\n !_infiniteTimeWindow && _buffer.push(_timestampProvider.now() + _windowTime);\n }\n this._trimBuffer();\n super.next(value);\n }\n\n /** @internal */\n protected _subscribe(subscriber: Subscriber): Subscription {\n this._throwIfClosed();\n this._trimBuffer();\n\n const subscription = this._innerSubscribe(subscriber);\n\n const { _infiniteTimeWindow, _buffer } = this;\n // We use a copy here, so reentrant code does not mutate our array while we're\n // emitting it to a new subscriber.\n const copy = _buffer.slice();\n for (let i = 0; i < copy.length && !subscriber.closed; i += _infiniteTimeWindow ? 1 : 2) {\n subscriber.next(copy[i] as T);\n }\n\n this._checkFinalizedStatuses(subscriber);\n\n return subscription;\n }\n\n private _trimBuffer() {\n const { _bufferSize, _timestampProvider, _buffer, _infiniteTimeWindow } = this;\n // If we don't have an infinite buffer size, and we're over the length,\n // use splice to truncate the old buffer values off. Note that we have to\n // double the size for instances where we're not using an infinite time window\n // because we're storing the values and the timestamps in the same array.\n const adjustedBufferSize = (_infiniteTimeWindow ? 1 : 2) * _bufferSize;\n _bufferSize < Infinity && adjustedBufferSize < _buffer.length && _buffer.splice(0, _buffer.length - adjustedBufferSize);\n\n // Now, if we're not in an infinite time window, remove all values where the time is\n // older than what is allowed.\n if (!_infiniteTimeWindow) {\n const now = _timestampProvider.now();\n let last = 0;\n // Search the array for the first timestamp that isn't expired and\n // truncate the buffer up to that point.\n for (let i = 1; i < _buffer.length && (_buffer[i] as number) <= now; i += 2) {\n last = i;\n }\n last && _buffer.splice(0, last + 1);\n }\n }\n}\n", "import { Scheduler } from '../Scheduler';\nimport { Subscription } from '../Subscription';\nimport { SchedulerAction } from '../types';\n\n/**\n * A unit of work to be executed in a `scheduler`. An action is typically\n * created from within a {@link SchedulerLike} and an RxJS user does not need to concern\n * themselves about creating and manipulating an Action.\n *\n * ```ts\n * class Action extends Subscription {\n * new (scheduler: Scheduler, work: (state?: T) => void);\n * schedule(state?: T, delay: number = 0): Subscription;\n * }\n * ```\n *\n * @class Action\n */\nexport class Action extends Subscription {\n constructor(scheduler: Scheduler, work: (this: SchedulerAction, state?: T) => void) {\n super();\n }\n /**\n * Schedules this action on its parent {@link SchedulerLike} for execution. May be passed\n * some context object, `state`. May happen at some point in the future,\n * according to the `delay` parameter, if specified.\n * @param {T} [state] Some contextual data that the `work` function uses when\n * called by the Scheduler.\n * @param {number} [delay] Time to wait before executing the work, where the\n * time unit is implicit and defined by the Scheduler.\n * @return {void}\n */\n public schedule(state?: T, delay: number = 0): Subscription {\n return this;\n }\n}\n", "import type { TimerHandle } from './timerHandle';\ntype SetIntervalFunction = (handler: () => void, timeout?: number, ...args: any[]) => TimerHandle;\ntype ClearIntervalFunction = (handle: TimerHandle) => void;\n\ninterface IntervalProvider {\n setInterval: SetIntervalFunction;\n clearInterval: ClearIntervalFunction;\n delegate:\n | {\n setInterval: SetIntervalFunction;\n clearInterval: ClearIntervalFunction;\n }\n | undefined;\n}\n\nexport const intervalProvider: IntervalProvider = {\n // When accessing the delegate, use the variable rather than `this` so that\n // the functions can be called without being bound to the provider.\n setInterval(handler: () => void, timeout?: number, ...args) {\n const { delegate } = intervalProvider;\n if (delegate?.setInterval) {\n return delegate.setInterval(handler, timeout, ...args);\n }\n return setInterval(handler, timeout, ...args);\n },\n clearInterval(handle) {\n const { delegate } = intervalProvider;\n return (delegate?.clearInterval || clearInterval)(handle as any);\n },\n delegate: undefined,\n};\n", "import { Action } from './Action';\nimport { SchedulerAction } from '../types';\nimport { Subscription } from '../Subscription';\nimport { AsyncScheduler } from './AsyncScheduler';\nimport { intervalProvider } from './intervalProvider';\nimport { arrRemove } from '../util/arrRemove';\nimport { TimerHandle } from './timerHandle';\n\nexport class AsyncAction extends Action {\n public id: TimerHandle | undefined;\n public state?: T;\n // @ts-ignore: Property has no initializer and is not definitely assigned\n public delay: number;\n protected pending: boolean = false;\n\n constructor(protected scheduler: AsyncScheduler, protected work: (this: SchedulerAction, state?: T) => void) {\n super(scheduler, work);\n }\n\n public schedule(state?: T, delay: number = 0): Subscription {\n if (this.closed) {\n return this;\n }\n\n // Always replace the current state with the new state.\n this.state = state;\n\n const id = this.id;\n const scheduler = this.scheduler;\n\n //\n // Important implementation note:\n //\n // Actions only execute once by default, unless rescheduled from within the\n // scheduled callback. This allows us to implement single and repeat\n // actions via the same code path, without adding API surface area, as well\n // as mimic traditional recursion but across asynchronous boundaries.\n //\n // However, JS runtimes and timers distinguish between intervals achieved by\n // serial `setTimeout` calls vs. a single `setInterval` call. An interval of\n // serial `setTimeout` calls can be individually delayed, which delays\n // scheduling the next `setTimeout`, and so on. `setInterval` attempts to\n // guarantee the interval callback will be invoked more precisely to the\n // interval period, regardless of load.\n //\n // Therefore, we use `setInterval` to schedule single and repeat actions.\n // If the action reschedules itself with the same delay, the interval is not\n // canceled. If the action doesn't reschedule, or reschedules with a\n // different delay, the interval will be canceled after scheduled callback\n // execution.\n //\n if (id != null) {\n this.id = this.recycleAsyncId(scheduler, id, delay);\n }\n\n // Set the pending flag indicating that this action has been scheduled, or\n // has recursively rescheduled itself.\n this.pending = true;\n\n this.delay = delay;\n // If this action has already an async Id, don't request a new one.\n this.id = this.id ?? this.requestAsyncId(scheduler, this.id, delay);\n\n return this;\n }\n\n protected requestAsyncId(scheduler: AsyncScheduler, _id?: TimerHandle, delay: number = 0): TimerHandle {\n return intervalProvider.setInterval(scheduler.flush.bind(scheduler, this), delay);\n }\n\n protected recycleAsyncId(_scheduler: AsyncScheduler, id?: TimerHandle, delay: number | null = 0): TimerHandle | undefined {\n // If this action is rescheduled with the same delay time, don't clear the interval id.\n if (delay != null && this.delay === delay && this.pending === false) {\n return id;\n }\n // Otherwise, if the action's delay time is different from the current delay,\n // or the action has been rescheduled before it's executed, clear the interval id\n if (id != null) {\n intervalProvider.clearInterval(id);\n }\n\n return undefined;\n }\n\n /**\n * Immediately executes this action and the `work` it contains.\n * @return {any}\n */\n public execute(state: T, delay: number): any {\n if (this.closed) {\n return new Error('executing a cancelled action');\n }\n\n this.pending = false;\n const error = this._execute(state, delay);\n if (error) {\n return error;\n } else if (this.pending === false && this.id != null) {\n // Dequeue if the action didn't reschedule itself. Don't call\n // unsubscribe(), because the action could reschedule later.\n // For example:\n // ```\n // scheduler.schedule(function doWork(counter) {\n // /* ... I'm a busy worker bee ... */\n // var originalAction = this;\n // /* wait 100ms before rescheduling the action */\n // setTimeout(function () {\n // originalAction.schedule(counter + 1);\n // }, 100);\n // }, 1000);\n // ```\n this.id = this.recycleAsyncId(this.scheduler, this.id, null);\n }\n }\n\n protected _execute(state: T, _delay: number): any {\n let errored: boolean = false;\n let errorValue: any;\n try {\n this.work(state);\n } catch (e) {\n errored = true;\n // HACK: Since code elsewhere is relying on the \"truthiness\" of the\n // return here, we can't have it return \"\" or 0 or false.\n // TODO: Clean this up when we refactor schedulers mid-version-8 or so.\n errorValue = e ? e : new Error('Scheduled action threw falsy error');\n }\n if (errored) {\n this.unsubscribe();\n return errorValue;\n }\n }\n\n unsubscribe() {\n if (!this.closed) {\n const { id, scheduler } = this;\n const { actions } = scheduler;\n\n this.work = this.state = this.scheduler = null!;\n this.pending = false;\n\n arrRemove(actions, this);\n if (id != null) {\n this.id = this.recycleAsyncId(scheduler, id, null);\n }\n\n this.delay = null!;\n super.unsubscribe();\n }\n }\n}\n", "import { Action } from './scheduler/Action';\nimport { Subscription } from './Subscription';\nimport { SchedulerLike, SchedulerAction } from './types';\nimport { dateTimestampProvider } from './scheduler/dateTimestampProvider';\n\n/**\n * An execution context and a data structure to order tasks and schedule their\n * execution. Provides a notion of (potentially virtual) time, through the\n * `now()` getter method.\n *\n * Each unit of work in a Scheduler is called an `Action`.\n *\n * ```ts\n * class Scheduler {\n * now(): number;\n * schedule(work, delay?, state?): Subscription;\n * }\n * ```\n *\n * @class Scheduler\n * @deprecated Scheduler is an internal implementation detail of RxJS, and\n * should not be used directly. Rather, create your own class and implement\n * {@link SchedulerLike}. Will be made internal in v8.\n */\nexport class Scheduler implements SchedulerLike {\n public static now: () => number = dateTimestampProvider.now;\n\n constructor(private schedulerActionCtor: typeof Action, now: () => number = Scheduler.now) {\n this.now = now;\n }\n\n /**\n * A getter method that returns a number representing the current time\n * (at the time this function was called) according to the scheduler's own\n * internal clock.\n * @return {number} A number that represents the current time. May or may not\n * have a relation to wall-clock time. May or may not refer to a time unit\n * (e.g. milliseconds).\n */\n public now: () => number;\n\n /**\n * Schedules a function, `work`, for execution. May happen at some point in\n * the future, according to the `delay` parameter, if specified. May be passed\n * some context object, `state`, which will be passed to the `work` function.\n *\n * The given arguments will be processed an stored as an Action object in a\n * queue of actions.\n *\n * @param {function(state: ?T): ?Subscription} work A function representing a\n * task, or some unit of work to be executed by the Scheduler.\n * @param {number} [delay] Time to wait before executing the work, where the\n * time unit is implicit and defined by the Scheduler itself.\n * @param {T} [state] Some contextual data that the `work` function uses when\n * called by the Scheduler.\n * @return {Subscription} A subscription in order to be able to unsubscribe\n * the scheduled work.\n */\n public schedule(work: (this: SchedulerAction, state?: T) => void, delay: number = 0, state?: T): Subscription {\n return new this.schedulerActionCtor(this, work).schedule(state, delay);\n }\n}\n", "import { Scheduler } from '../Scheduler';\nimport { Action } from './Action';\nimport { AsyncAction } from './AsyncAction';\nimport { TimerHandle } from './timerHandle';\n\nexport class AsyncScheduler extends Scheduler {\n public actions: Array> = [];\n /**\n * A flag to indicate whether the Scheduler is currently executing a batch of\n * queued actions.\n * @type {boolean}\n * @internal\n */\n public _active: boolean = false;\n /**\n * An internal ID used to track the latest asynchronous task such as those\n * coming from `setTimeout`, `setInterval`, `requestAnimationFrame`, and\n * others.\n * @type {any}\n * @internal\n */\n public _scheduled: TimerHandle | undefined;\n\n constructor(SchedulerAction: typeof Action, now: () => number = Scheduler.now) {\n super(SchedulerAction, now);\n }\n\n public flush(action: AsyncAction): void {\n const { actions } = this;\n\n if (this._active) {\n actions.push(action);\n return;\n }\n\n let error: any;\n this._active = true;\n\n do {\n if ((error = action.execute(action.state, action.delay))) {\n break;\n }\n } while ((action = actions.shift()!)); // exhaust the scheduler queue\n\n this._active = false;\n\n if (error) {\n while ((action = actions.shift()!)) {\n action.unsubscribe();\n }\n throw error;\n }\n }\n}\n", "import { AsyncAction } from './AsyncAction';\nimport { AsyncScheduler } from './AsyncScheduler';\n\n/**\n *\n * Async Scheduler\n *\n * Schedule task as if you used setTimeout(task, duration)\n *\n * `async` scheduler schedules tasks asynchronously, by putting them on the JavaScript\n * event loop queue. It is best used to delay tasks in time or to schedule tasks repeating\n * in intervals.\n *\n * If you just want to \"defer\" task, that is to perform it right after currently\n * executing synchronous code ends (commonly achieved by `setTimeout(deferredTask, 0)`),\n * better choice will be the {@link asapScheduler} scheduler.\n *\n * ## Examples\n * Use async scheduler to delay task\n * ```ts\n * import { asyncScheduler } from 'rxjs';\n *\n * const task = () => console.log('it works!');\n *\n * asyncScheduler.schedule(task, 2000);\n *\n * // After 2 seconds logs:\n * // \"it works!\"\n * ```\n *\n * Use async scheduler to repeat task in intervals\n * ```ts\n * import { asyncScheduler } from 'rxjs';\n *\n * function task(state) {\n * console.log(state);\n * this.schedule(state + 1, 1000); // `this` references currently executing Action,\n * // which we reschedule with new state and delay\n * }\n *\n * asyncScheduler.schedule(task, 3000, 0);\n *\n * // Logs:\n * // 0 after 3s\n * // 1 after 4s\n * // 2 after 5s\n * // 3 after 6s\n * ```\n */\n\nexport const asyncScheduler = new AsyncScheduler(AsyncAction);\n\n/**\n * @deprecated Renamed to {@link asyncScheduler}. Will be removed in v8.\n */\nexport const async = asyncScheduler;\n", "import { AsyncAction } from './AsyncAction';\nimport { Subscription } from '../Subscription';\nimport { QueueScheduler } from './QueueScheduler';\nimport { SchedulerAction } from '../types';\nimport { TimerHandle } from './timerHandle';\n\nexport class QueueAction extends AsyncAction {\n constructor(protected scheduler: QueueScheduler, protected work: (this: SchedulerAction, state?: T) => void) {\n super(scheduler, work);\n }\n\n public schedule(state?: T, delay: number = 0): Subscription {\n if (delay > 0) {\n return super.schedule(state, delay);\n }\n this.delay = delay;\n this.state = state;\n this.scheduler.flush(this);\n return this;\n }\n\n public execute(state: T, delay: number): any {\n return delay > 0 || this.closed ? super.execute(state, delay) : this._execute(state, delay);\n }\n\n protected requestAsyncId(scheduler: QueueScheduler, id?: TimerHandle, delay: number = 0): TimerHandle {\n // If delay exists and is greater than 0, or if the delay is null (the\n // action wasn't rescheduled) but was originally scheduled as an async\n // action, then recycle as an async action.\n\n if ((delay != null && delay > 0) || (delay == null && this.delay > 0)) {\n return super.requestAsyncId(scheduler, id, delay);\n }\n\n // Otherwise flush the scheduler starting with this action.\n scheduler.flush(this);\n\n // HACK: In the past, this was returning `void`. However, `void` isn't a valid\n // `TimerHandle`, and generally the return value here isn't really used. So the\n // compromise is to return `0` which is both \"falsy\" and a valid `TimerHandle`,\n // as opposed to refactoring every other instanceo of `requestAsyncId`.\n return 0;\n }\n}\n", "import { AsyncScheduler } from './AsyncScheduler';\n\nexport class QueueScheduler extends AsyncScheduler {\n}\n", "import { QueueAction } from './QueueAction';\nimport { QueueScheduler } from './QueueScheduler';\n\n/**\n *\n * Queue Scheduler\n *\n * Put every next task on a queue, instead of executing it immediately\n *\n * `queue` scheduler, when used with delay, behaves the same as {@link asyncScheduler} scheduler.\n *\n * When used without delay, it schedules given task synchronously - executes it right when\n * it is scheduled. However when called recursively, that is when inside the scheduled task,\n * another task is scheduled with queue scheduler, instead of executing immediately as well,\n * that task will be put on a queue and wait for current one to finish.\n *\n * This means that when you execute task with `queue` scheduler, you are sure it will end\n * before any other task scheduled with that scheduler will start.\n *\n * ## Examples\n * Schedule recursively first, then do something\n * ```ts\n * import { queueScheduler } from 'rxjs';\n *\n * queueScheduler.schedule(() => {\n * queueScheduler.schedule(() => console.log('second')); // will not happen now, but will be put on a queue\n *\n * console.log('first');\n * });\n *\n * // Logs:\n * // \"first\"\n * // \"second\"\n * ```\n *\n * Reschedule itself recursively\n * ```ts\n * import { queueScheduler } from 'rxjs';\n *\n * queueScheduler.schedule(function(state) {\n * if (state !== 0) {\n * console.log('before', state);\n * this.schedule(state - 1); // `this` references currently executing Action,\n * // which we reschedule with new state\n * console.log('after', state);\n * }\n * }, 0, 3);\n *\n * // In scheduler that runs recursively, you would expect:\n * // \"before\", 3\n * // \"before\", 2\n * // \"before\", 1\n * // \"after\", 1\n * // \"after\", 2\n * // \"after\", 3\n *\n * // But with queue it logs:\n * // \"before\", 3\n * // \"after\", 3\n * // \"before\", 2\n * // \"after\", 2\n * // \"before\", 1\n * // \"after\", 1\n * ```\n */\n\nexport const queueScheduler = new QueueScheduler(QueueAction);\n\n/**\n * @deprecated Renamed to {@link queueScheduler}. Will be removed in v8.\n */\nexport const queue = queueScheduler;\n", "import { AsyncAction } from './AsyncAction';\nimport { AnimationFrameScheduler } from './AnimationFrameScheduler';\nimport { SchedulerAction } from '../types';\nimport { animationFrameProvider } from './animationFrameProvider';\nimport { TimerHandle } from './timerHandle';\n\nexport class AnimationFrameAction extends AsyncAction {\n constructor(protected scheduler: AnimationFrameScheduler, protected work: (this: SchedulerAction, state?: T) => void) {\n super(scheduler, work);\n }\n\n protected requestAsyncId(scheduler: AnimationFrameScheduler, id?: TimerHandle, delay: number = 0): TimerHandle {\n // If delay is greater than 0, request as an async action.\n if (delay !== null && delay > 0) {\n return super.requestAsyncId(scheduler, id, delay);\n }\n // Push the action to the end of the scheduler queue.\n scheduler.actions.push(this);\n // If an animation frame has already been requested, don't request another\n // one. If an animation frame hasn't been requested yet, request one. Return\n // the current animation frame request id.\n return scheduler._scheduled || (scheduler._scheduled = animationFrameProvider.requestAnimationFrame(() => scheduler.flush(undefined)));\n }\n\n protected recycleAsyncId(scheduler: AnimationFrameScheduler, id?: TimerHandle, delay: number = 0): TimerHandle | undefined {\n // If delay exists and is greater than 0, or if the delay is null (the\n // action wasn't rescheduled) but was originally scheduled as an async\n // action, then recycle as an async action.\n if (delay != null ? delay > 0 : this.delay > 0) {\n return super.recycleAsyncId(scheduler, id, delay);\n }\n // If the scheduler queue has no remaining actions with the same async id,\n // cancel the requested animation frame and set the scheduled flag to\n // undefined so the next AnimationFrameAction will request its own.\n const { actions } = scheduler;\n if (id != null && actions[actions.length - 1]?.id !== id) {\n animationFrameProvider.cancelAnimationFrame(id as number);\n scheduler._scheduled = undefined;\n }\n // Return undefined so the action knows to request a new async id if it's rescheduled.\n return undefined;\n }\n}\n", "import { AsyncAction } from './AsyncAction';\nimport { AsyncScheduler } from './AsyncScheduler';\n\nexport class AnimationFrameScheduler extends AsyncScheduler {\n public flush(action?: AsyncAction): void {\n this._active = true;\n // The async id that effects a call to flush is stored in _scheduled.\n // Before executing an action, it's necessary to check the action's async\n // id to determine whether it's supposed to be executed in the current\n // flush.\n // Previous implementations of this method used a count to determine this,\n // but that was unsound, as actions that are unsubscribed - i.e. cancelled -\n // are removed from the actions array and that can shift actions that are\n // scheduled to be executed in a subsequent flush into positions at which\n // they are executed within the current flush.\n const flushId = this._scheduled;\n this._scheduled = undefined;\n\n const { actions } = this;\n let error: any;\n action = action || actions.shift()!;\n\n do {\n if ((error = action.execute(action.state, action.delay))) {\n break;\n }\n } while ((action = actions[0]) && action.id === flushId && actions.shift());\n\n this._active = false;\n\n if (error) {\n while ((action = actions[0]) && action.id === flushId && actions.shift()) {\n action.unsubscribe();\n }\n throw error;\n }\n }\n}\n", "import { AnimationFrameAction } from './AnimationFrameAction';\nimport { AnimationFrameScheduler } from './AnimationFrameScheduler';\n\n/**\n *\n * Animation Frame Scheduler\n *\n * Perform task when `window.requestAnimationFrame` would fire\n *\n * When `animationFrame` scheduler is used with delay, it will fall back to {@link asyncScheduler} scheduler\n * behaviour.\n *\n * Without delay, `animationFrame` scheduler can be used to create smooth browser animations.\n * It makes sure scheduled task will happen just before next browser content repaint,\n * thus performing animations as efficiently as possible.\n *\n * ## Example\n * Schedule div height animation\n * ```ts\n * // html:
\n * import { animationFrameScheduler } from 'rxjs';\n *\n * const div = document.querySelector('div');\n *\n * animationFrameScheduler.schedule(function(height) {\n * div.style.height = height + \"px\";\n *\n * this.schedule(height + 1); // `this` references currently executing Action,\n * // which we reschedule with new state\n * }, 0, 0);\n *\n * // You will see a div element growing in height\n * ```\n */\n\nexport const animationFrameScheduler = new AnimationFrameScheduler(AnimationFrameAction);\n\n/**\n * @deprecated Renamed to {@link animationFrameScheduler}. Will be removed in v8.\n */\nexport const animationFrame = animationFrameScheduler;\n", "import { Observable } from '../Observable';\nimport { SchedulerLike } from '../types';\n\n/**\n * A simple Observable that emits no items to the Observer and immediately\n * emits a complete notification.\n *\n * Just emits 'complete', and nothing else.\n *\n * ![](empty.png)\n *\n * A simple Observable that only emits the complete notification. It can be used\n * for composing with other Observables, such as in a {@link mergeMap}.\n *\n * ## Examples\n *\n * Log complete notification\n *\n * ```ts\n * import { EMPTY } from 'rxjs';\n *\n * EMPTY.subscribe({\n * next: () => console.log('Next'),\n * complete: () => console.log('Complete!')\n * });\n *\n * // Outputs\n * // Complete!\n * ```\n *\n * Emit the number 7, then complete\n *\n * ```ts\n * import { EMPTY, startWith } from 'rxjs';\n *\n * const result = EMPTY.pipe(startWith(7));\n * result.subscribe(x => console.log(x));\n *\n * // Outputs\n * // 7\n * ```\n *\n * Map and flatten only odd numbers to the sequence `'a'`, `'b'`, `'c'`\n *\n * ```ts\n * import { interval, mergeMap, of, EMPTY } from 'rxjs';\n *\n * const interval$ = interval(1000);\n * const result = interval$.pipe(\n * mergeMap(x => x % 2 === 1 ? of('a', 'b', 'c') : EMPTY),\n * );\n * result.subscribe(x => console.log(x));\n *\n * // Results in the following to the console:\n * // x is equal to the count on the interval, e.g. (0, 1, 2, 3, ...)\n * // x will occur every 1000ms\n * // if x % 2 is equal to 1, print a, b, c (each on its own)\n * // if x % 2 is not equal to 1, nothing will be output\n * ```\n *\n * @see {@link Observable}\n * @see {@link NEVER}\n * @see {@link of}\n * @see {@link throwError}\n */\nexport const EMPTY = new Observable((subscriber) => subscriber.complete());\n\n/**\n * @param scheduler A {@link SchedulerLike} to use for scheduling\n * the emission of the complete notification.\n * @deprecated Replaced with the {@link EMPTY} constant or {@link scheduled} (e.g. `scheduled([], scheduler)`). Will be removed in v8.\n */\nexport function empty(scheduler?: SchedulerLike) {\n return scheduler ? emptyScheduled(scheduler) : EMPTY;\n}\n\nfunction emptyScheduled(scheduler: SchedulerLike) {\n return new Observable((subscriber) => scheduler.schedule(() => subscriber.complete()));\n}\n", "import { SchedulerLike } from '../types';\nimport { isFunction } from './isFunction';\n\nexport function isScheduler(value: any): value is SchedulerLike {\n return value && isFunction(value.schedule);\n}\n", "import { SchedulerLike } from '../types';\nimport { isFunction } from './isFunction';\nimport { isScheduler } from './isScheduler';\n\nfunction last(arr: T[]): T | undefined {\n return arr[arr.length - 1];\n}\n\nexport function popResultSelector(args: any[]): ((...args: unknown[]) => unknown) | undefined {\n return isFunction(last(args)) ? args.pop() : undefined;\n}\n\nexport function popScheduler(args: any[]): SchedulerLike | undefined {\n return isScheduler(last(args)) ? args.pop() : undefined;\n}\n\nexport function popNumber(args: any[], defaultValue: number): number {\n return typeof last(args) === 'number' ? args.pop()! : defaultValue;\n}\n", "export const isArrayLike = ((x: any): x is ArrayLike => x && typeof x.length === 'number' && typeof x !== 'function');", "import { isFunction } from \"./isFunction\";\n\n/**\n * Tests to see if the object is \"thennable\".\n * @param value the object to test\n */\nexport function isPromise(value: any): value is PromiseLike {\n return isFunction(value?.then);\n}\n", "import { InteropObservable } from '../types';\nimport { observable as Symbol_observable } from '../symbol/observable';\nimport { isFunction } from './isFunction';\n\n/** Identifies an input as being Observable (but not necessary an Rx Observable) */\nexport function isInteropObservable(input: any): input is InteropObservable {\n return isFunction(input[Symbol_observable]);\n}\n", "import { isFunction } from './isFunction';\n\nexport function isAsyncIterable(obj: any): obj is AsyncIterable {\n return Symbol.asyncIterator && isFunction(obj?.[Symbol.asyncIterator]);\n}\n", "/**\n * Creates the TypeError to throw if an invalid object is passed to `from` or `scheduled`.\n * @param input The object that was passed.\n */\nexport function createInvalidObservableTypeError(input: any) {\n // TODO: We should create error codes that can be looked up, so this can be less verbose.\n return new TypeError(\n `You provided ${\n input !== null && typeof input === 'object' ? 'an invalid object' : `'${input}'`\n } where a stream was expected. You can provide an Observable, Promise, ReadableStream, Array, AsyncIterable, or Iterable.`\n );\n}\n", "export function getSymbolIterator(): symbol {\n if (typeof Symbol !== 'function' || !Symbol.iterator) {\n return '@@iterator' as any;\n }\n\n return Symbol.iterator;\n}\n\nexport const iterator = getSymbolIterator();\n", "import { iterator as Symbol_iterator } from '../symbol/iterator';\nimport { isFunction } from './isFunction';\n\n/** Identifies an input as being an Iterable */\nexport function isIterable(input: any): input is Iterable {\n return isFunction(input?.[Symbol_iterator]);\n}\n", "import { ReadableStreamLike } from '../types';\nimport { isFunction } from './isFunction';\n\nexport async function* readableStreamLikeToAsyncGenerator(readableStream: ReadableStreamLike): AsyncGenerator {\n const reader = readableStream.getReader();\n try {\n while (true) {\n const { value, done } = await reader.read();\n if (done) {\n return;\n }\n yield value!;\n }\n } finally {\n reader.releaseLock();\n }\n}\n\nexport function isReadableStreamLike(obj: any): obj is ReadableStreamLike {\n // We don't want to use instanceof checks because they would return\n // false for instances from another Realm, like an + +

Note you can also use the ros2 topic pub command to move the robot. For example, to fly to a position:

+ +

Shutdown

+

To shutdown and remove docker containers:

+
docker compose down
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/docs/ground_control_station/index.html b/docs/ground_control_station/index.html new file mode 100644 index 00000000..8146eb6b --- /dev/null +++ b/docs/ground_control_station/index.html @@ -0,0 +1,2657 @@ + + + + + + + + + + + + + + + + + + + + + + + Ground Control Station - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

Ground Control Station

+

The Ground Control Station (GCS) is for operators to monitor and control the robots.

+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ + + +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/docs/ground_control_station/multi_robot/multi_robot/index.html b/docs/ground_control_station/multi_robot/multi_robot/index.html new file mode 100644 index 00000000..b2d1b9f7 --- /dev/null +++ b/docs/ground_control_station/multi_robot/multi_robot/index.html @@ -0,0 +1,2661 @@ + + + + + + + + + + + + + + + + + + + + + + + Multi robot - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

Multi robot

+ + + + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ + + +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/docs/ground_control_station/usage/user_interface/index.html b/docs/ground_control_station/usage/user_interface/index.html new file mode 100644 index 00000000..a17a0822 --- /dev/null +++ b/docs/ground_control_station/usage/user_interface/index.html @@ -0,0 +1,2661 @@ + + + + + + + + + + + + + + + + + + + + + + + User interface - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

User interface

+ + + + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ + + +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/docs/index.html b/docs/index.html new file mode 100644 index 00000000..0bb367d1 --- /dev/null +++ b/docs/index.html @@ -0,0 +1,2644 @@ + + + + + + + + + + + + + + + + + + + + + Home - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

AirStack Boilerplate

+

Welcome to the AirStack Boilerplate! This repository template serves to kickstart the development of your own robotics autonomy stack. You're encouraged to customize your version in any way to best suit your project's needs.

+

This boilerplate is maintained by the AirLab at Carnegie Mellon University's Robotics Institute.

+

Please head to our Getting Started page to start.

+

Overview +AirStack

+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ + + +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/docs/overview.png b/docs/overview.png new file mode 100644 index 00000000..73f5a3a2 Binary files /dev/null and b/docs/overview.png differ diff --git a/docs/real_world/index.html b/docs/real_world/index.html new file mode 100644 index 00000000..71f19fe9 --- /dev/null +++ b/docs/real_world/index.html @@ -0,0 +1,2655 @@ + + + + + + + + + + + + + + + + + + + + + + + Real World Overview - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

Real World Overview

+

Fly robots in da wild.

+

That's wild.

+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ + + +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/docs/real_world/installation/index.html b/docs/real_world/installation/index.html new file mode 100644 index 00000000..a30e0730 --- /dev/null +++ b/docs/real_world/installation/index.html @@ -0,0 +1,2653 @@ + + + + + + + + + + + + + + + + + + + + + + + Installation on Hardware - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

Installation on Hardware

+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ + + +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/docs/robot/airstack_system_diagram.png b/docs/robot/airstack_system_diagram.png new file mode 100644 index 00000000..8438ea68 Binary files /dev/null and b/docs/robot/airstack_system_diagram.png differ diff --git a/docs/robot/autonomy/0_interface/index.html b/docs/robot/autonomy/0_interface/index.html new file mode 100644 index 00000000..d26bb310 --- /dev/null +++ b/docs/robot/autonomy/0_interface/index.html @@ -0,0 +1,2727 @@ + + + + + + + + + + + + + + + + + + + + + + + Robot Interface - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

Robot Interface

+

The interface defines the communication between the autonomy stack running on the onboard computer and the robot's control unit. +For example, for drones it converts the control commands from the autonomy stack into MAVLink messages for the flight controller.

+

TODO: This is not our diagram, must replace. +Interface Diagram

+

The code is located under AirStack/ros_ws/src/robot/autonomy/0_interface/.

+

Launch

+

Launch files are under src/robot/autonomy/0_interface/interface_bringup/launch.

+

The main launch command is ros2 launch interface_bringup interface.launch.xml.

+

RobotInterface

+

Package robot_interface is a ROS2 node that interfaces with the robot's hardware. +The RobotInterface gets robot state and forwards it to the autonomy stack, +and also translates control commands from the autonomy stack into the command for the underlying hardware. +Note the base class is unimplemented. +Specific implementations should extend class RobotInterface in robot_interface.hpp, for example class MAVROSInterface.

+

State

+

The RobotInterface class broadcasts the robot's pose as a TF2 transform. +It also publishes the robot's odometry as a nav_msgs/Odometry message to $(env ROBOT_NAME)/0_interface/robot_0_interface/odometry.

+

Commands

+

The commands are variations of the two main command modes: Attitude control and Position control. +These are reflected in MAVLink and supported by both PX4 and Ardupilot.

+

The RobotInterface node subscribes to:

+
    +
  • /$(env ROBOT_NAME)/interface/cmd_attitude_thrust of type mav_msgs/AttitudeThrust.msg
    • +
  • /$(env ROBOT_NAME)/interface/cmd_rate_thrust of type mav_msgs/RateThrust.msg
    • +
  • /$(env ROBOT_NAME)/interface/cmd_roll_pitch_yawrate_thrust of type mav_msgs/RollPitchYawrateThrust.msg
    • +
  • /$(env ROBOT_NAME)/interface/cmd_torque_thrust of type mav_msgs/TorqueThrust.msg
    • +
  • /$(env ROBOT_NAME)/interface/cmd_velocity of type geometry_msgs/TwistStamped.msg
    • +
  • /$(env ROBOT_NAME)/interface/cmd_position of type geometry_msgs/PoseStamped.msg
    • +
+

All messages are in the robot's body frame, except velocity and position which use the frame specified by the message header.

+

MAVROSInterface

+

The available implementation in AirStack is called MAVROSInterface implemented in mavros_interface.cpp. It simply forwards the control commands to the Ascent flight controller (based on Ardupilot) using MAVROS.

+

Custom Robot Interface

+

If you're using a different robot control unit with its own custom API, then you need to create an associated RobotInterface. Implementations should do the following:

+

Broadcast State

+

Implementations of RobotInterface should obtain the robot's pose and broadcast it as a TF2 transform.

+

Should look something like:

+
// callback function triggered by some loop
+void your_callback_function(){
+    // ...
+    geometry_msgs::msg::TransformStamped t;
+    // populate the transform, e.g.:
+    t.header = // some header
+    t.transform.translation.x = // some value
+    t.transform.translation.y = // some value
+    t.transform.translation.z = // some value
+    t.transform.rotation = // some quaternion
+    // Send the transformation
+    this->tf_broadcaster_->sendTransform(t);
+    // ...
+}
+
+

TODO: our code doesn't currently do it like this, it instead uses an external odometry_conversion node.

+

Override Command Handling

+

Should override all virtual functions in robot_interface.hpp:

+
    +
  • cmd_attitude_thrust_callback
    • +
  • cmd_rate_thrust_callback
    • +
  • cmd_roll_pitch_yawrate_thrust_callback
    • +
  • cmd_torque_thrust_callback
    • +
  • cmd_velocity_callback
    • +
  • cmd_position_callback
    • +
  • request_control
    • +
  • arm
    • +
  • disarm
    • +
  • is_armed
    • +
  • has_control
    • +
+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ + + +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/docs/robot/autonomy/0_interface/mavlink.webp b/docs/robot/autonomy/0_interface/mavlink.webp new file mode 100644 index 00000000..c852a76d Binary files /dev/null and b/docs/robot/autonomy/0_interface/mavlink.webp differ diff --git a/docs/robot/autonomy/1_sensors/index.html b/docs/robot/autonomy/1_sensors/index.html new file mode 100644 index 00000000..4f0774b2 --- /dev/null +++ b/docs/robot/autonomy/1_sensors/index.html @@ -0,0 +1,2664 @@ + + + + + + + + + + + + + + + + + + + + + + + Index - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

Index

+ +

We'll fill this with different things like the ZED-X package, LiDAR, etc

+

Launch

+

Launch files are under src/robot/autonomy/sensors/sensors_bringup/launch.

+

The main launch command is ros2 launch sensors_bringup sensors.launch.xml.

+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ + + +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/docs/robot/autonomy/2_perception/index.html b/docs/robot/autonomy/2_perception/index.html new file mode 100644 index 00000000..fa80fc42 --- /dev/null +++ b/docs/robot/autonomy/2_perception/index.html @@ -0,0 +1,2669 @@ + + + + + + + + + + + + + + + + + + + + + + + Perception - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

Perception

+

These modules process raw sensor data into useful information for the robot. For example: for detecting obstacles, localizing the robot, and recognizing objects.

+

Perception modules typically output topics in image space or point cloud space. This information then gets aggregated into global and local world models later in the pipeline.

+

Common perception modules include:

+
    +
  • semantic segmentation
    • +
  • VIO (Visual Inertial Odometry)
    • +
+

Launch

+

Launch files are under src/robot/autonomy/perception/perception_bringup/launch.

+

The main launch command is ros2 launch perception_bringup perception.launch.xml.

+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ + + +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/docs/robot/autonomy/2_perception/state_estimation/index.html b/docs/robot/autonomy/2_perception/state_estimation/index.html new file mode 100644 index 00000000..19490461 --- /dev/null +++ b/docs/robot/autonomy/2_perception/state_estimation/index.html @@ -0,0 +1,2664 @@ + + + + + + + + + + + + + + + + + + + + + + + State estimation - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

State estimation

+ +

Hey Yuheng your stuff goes here:)

+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ + + +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/docs/robot/autonomy/3_local/controls/index.html b/docs/robot/autonomy/3_local/controls/index.html new file mode 100644 index 00000000..1a13d55e --- /dev/null +++ b/docs/robot/autonomy/3_local/controls/index.html @@ -0,0 +1,2662 @@ + + + + + + + + + + + + + + + + + + + + + + + Controls - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

Controls

+

Controls dictate the actuation of the robot. They are responsible for taking in sensor data and producing control commands.

+

The controller should publish control commands directly to topics defined by the Robot Interface.

+

Currently the AirStack uses a custom controller called "Trajectory Controller".

+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ + + +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/docs/robot/autonomy/3_local/index.html b/docs/robot/autonomy/3_local/index.html new file mode 100644 index 00000000..f72a8087 --- /dev/null +++ b/docs/robot/autonomy/3_local/index.html @@ -0,0 +1,2663 @@ + + + + + + + + + + + + + + + + + + + + + + + Local Packages - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

Local Packages

+

The local module includes packages that are specific to the local autonomy of the robot. This includes local mapping, planning, and control.

+

Launch

+

Launch files are under src/robot/autonomy/local/local_bringup/launch.

+

The main launch command is ros2 launch local_bringup local.launch.xml.

+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ + + +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/docs/robot/autonomy/3_local/planning/index.html b/docs/robot/autonomy/3_local/planning/index.html new file mode 100644 index 00000000..fb695810 --- /dev/null +++ b/docs/robot/autonomy/3_local/planning/index.html @@ -0,0 +1,2662 @@ + + + + + + + + + + + + + + + + + + + + + + + Local Planning - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

Local Planning

+

Part of the local planner is the Waypoint Manager.

+

The Waypoint Manager subscribes to the global waypoints and the drone's current position and publishes the next waypoint to the local planner.

+

We plan for this baseline to be DROAN

+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ + + +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/docs/robot/autonomy/3_local/world_model/index.html b/docs/robot/autonomy/3_local/world_model/index.html new file mode 100644 index 00000000..a0dad496 --- /dev/null +++ b/docs/robot/autonomy/3_local/world_model/index.html @@ -0,0 +1,2659 @@ + + + + + + + + + + + + + + + + + + + + + + + Local World Model - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

Local World Model

+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ + + +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/docs/robot/autonomy/4_global/index.html b/docs/robot/autonomy/4_global/index.html new file mode 100644 index 00000000..6a9f2411 --- /dev/null +++ b/docs/robot/autonomy/4_global/index.html @@ -0,0 +1,2663 @@ + + + + + + + + + + + + + + + + + + + + + + + Global Packages - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

Global Packages

+

The global packages include global world models and planners.

+

Launch

+

Launch files are under src/robot/autonomy/global/global_bringup/launch.

+

The main launch command is ros2 launch global_bringup global.launch.xml.

+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ + + +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/docs/robot/autonomy/4_global/planning/global_trajectory.png b/docs/robot/autonomy/4_global/planning/global_trajectory.png new file mode 100644 index 00000000..8dd6da42 Binary files /dev/null and b/docs/robot/autonomy/4_global/planning/global_trajectory.png differ diff --git a/docs/robot/autonomy/4_global/planning/index.html b/docs/robot/autonomy/4_global/planning/index.html new file mode 100644 index 00000000..b6de8647 --- /dev/null +++ b/docs/robot/autonomy/4_global/planning/index.html @@ -0,0 +1,2724 @@ + + + + + + + + + + + + + + + + + + + + + + + Planning - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

Planning

+

global_trajectory_diagram

+

Global planners output a high level, coarse trajectory for the robot to follow.

+

A trajectory is a spatial path plus a schedule. +This means each waypoint in the trajectory has a time associated with it, indicating when the robot should reach that waypoint. +These timestamps are fed to the local planner and controller to determine velocity and acceleration.

+

If a waypoint's header timestamp is empty, the local planner should assume there's no time constraint and follow the trajectory at its own pace.

+

The global planner should make a trajectory that is collision-free according to the global map. +However, avoiding fine obstacles is delegated to the local planner that operates at a faster rate.

+

For the structure of the package, the global planner node should not include any logic to generate the path. This should be located in a seperate logic class and be seperated from ROS. This will allow more modularity in the future for testing and easy interface changes.

+

We intend the global planners to be modular. AirStack implements a basic Random Walk planner as a baseline. +Feel free to implement your own through the following interfaces.

+

ROS Interfaces

+

Global planners are meant to be modules that can be swapped out easily. +They can be thought of as different high level behaviors for the robot to follow. +Consider that multiple global planners may be run in parallel, for example by some ensemble planner node that chooses the best plan for the current situation.

+

As such, the global planner should be implemented as a ROS2 node that accepts runtime mission parameters in a custom PlanRequest.msg and +publishes a plan to its local ~/global_plan topic.

+

The best global plan should then be forwarded or remapped to /$(env ROBOT_NAME)/global_plan for the local planner to follow.

+
sequenceDiagram
+  autonumber
+  Global Manager->>Global Planner: ~/plan_request (your_planner/PlanRequest.msg)
+  loop Planning
+      Global Planner-->>Global Manager: heartbeat feedback
+  end
+  Global Planner->>Global Manager: ~/global_plan (nav_msgs/Path.msg)
+  Global Manager->>Local Planner: /$ROBOT_NAME/global_plan_reference (nav_msgs/Path.msg)
+  Local Planner->>Global Manager: /$ROBOT_NAME/global_plan_eta (nav_msgs/Path.msg)
+

Subscribe: Plan Request

+

Your custom PlanRequest.msg defines the parameters that your global planner needs to generate a plan. +It will be sent on the ~/plan_request topic.

+

Some common parameters may be the following: +

# PlanRequest.msg
+std_msgs/Duration timeout  # maximum time to spend planning
+geometry_msgs/Polygon bounds # boundary that the plan must stay within
+

+

Publish: Global Plan

+

The global planner must publish a message of type nav_msgs/Path to ~/global_plan. +The message defines high level waypoints to reach by a given time.

+

The nav_msgs/Path message type contains a header field and poses field.

+
    +
  • The top level header of nav_msgs/Path message should contain the coordinate frame of the trajectory, and its timestamp should indicate when the trajectory was published.
    • +
  • Within the poses field, each geometry_msgs/PoseStamped's header should contain a timestamp that indicates when that waypoint should be reached
    • +
+
nav_msgs/Path.msg
+    - std_msgs/Header header
+        - time stamp: when the trajectory was generated
+        - frame_id: the coordinate frame of the trajectory
+    - geometry_msgs/PoseStamped[] poses: the trajectory
+        - geometry_msgs/PoseStamped pose
+            - std_msgs/Header header
+                - time stamp: when the waypoint should be reached
+                - string frame_id: the coordinate frame of the waypoint
+            - geometry_msgs/Pose pose: the position and orientation of the waypoint
+
+

Publish: Heartbeat

+

For long-running global planners, it's recommended to publish a heartbeat message to ~/heartbeat. This way the calling node can know that the global planner is still running and hasn't crashed.

+

Additional Subscribers

+

In general, the global planner needs to access components of the world model such as the map and drone state.

+

The most common map is Occupancy Grids that is published by TODO node.

+

The global planner can also access the robot's current state and expected state in the future. For example, if the global planner takes 20 seconds to plan a trajectory, +it can query where the robot expects to be in 20 seconds. This ROS2 service is available under TODO.

+

The global planner can do whatever it wants internally with this information.

+

Example Planners

+

Random Walk planner

+

The random walk planner replans when the robot is getting close to the goal. The random walk planner is a trivial planner that generates a plan by randomly selecting a direction to move in. The random walk planner is useful for testing the robot's ability to follow a plan.

+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ + + +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/docs/robot/autonomy/4_global/world_model/index.html b/docs/robot/autonomy/4_global/world_model/index.html new file mode 100644 index 00000000..1d8ea4c2 --- /dev/null +++ b/docs/robot/autonomy/4_global/world_model/index.html @@ -0,0 +1,2661 @@ + + + + + + + + + + + + + + + + + + + + + + + World Model - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

World Model

+

Global world models are responsible for maintaining a representation of the world that is used by the global planner to generate a plan. This representation is typically a map of the environment, but can also include other information such as the location of other robots, obstacles, and goals.

+

The current placeholder world model is a voxelized map representation called VDB Mapping.

+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ + + +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/docs/robot/autonomy/5_behavior/behavior_executive/index.html b/docs/robot/autonomy/5_behavior/behavior_executive/index.html new file mode 100644 index 00000000..cc06a6b8 --- /dev/null +++ b/docs/robot/autonomy/5_behavior/behavior_executive/index.html @@ -0,0 +1,2683 @@ + + + + + + + + + + + + + + + + + + + + + + + Behavior Executive - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

Behavior Executive

+

The behavior executive reads which actions are active from the behavior tree and implements the behavior which these actions should perform and sets the status of the actions to SUCCESS, RUNNING, or FAILURE. It also sets the status of conditions as either SUCCESS or FAILURE.

+

A typical way of implementing the behavior for an action is the following in the 20 Hz timer callback:

+
if(action->is_active()){
+  if(action->active_has_changed()){
+    // This is only true when the when the action transitions between active/inactive
+    // so this block of code will only run once whenever the action goes from being inactive to active.
+    // You might put a service call here and then call action->set_success() or action->set_failure()
+    // based on the result returned by the service call.
+  }
+
+  // Code here will get executed each iteration.
+  // You might call action->set_running() while you are doing work here.
+}
+
+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ + + +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/docs/robot/autonomy/5_behavior/behavior_tree/index.html b/docs/robot/autonomy/5_behavior/behavior_tree/index.html new file mode 100644 index 00000000..65cb2b47 --- /dev/null +++ b/docs/robot/autonomy/5_behavior/behavior_tree/index.html @@ -0,0 +1,2833 @@ + + + + + + + + + + + + + + + + + + + + + + + Behavior Trees - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

Behavior Trees

+

Defines how a task in terms of conditions and actions which the user +implements.

+

Other types of nodes, control flow and decorator nodes, control which +conditions will be checked and which actions will be activated.

+

Nodes have statuses of either SUCCESS, RUNNING or FAILURE.

+

+

Why Behavior Trees?

+

Maintainable - Easy to modify

+

Scalable - Parts of sub-trees are modular and can be encapsulated

+

Reusable - Sub-trees can be reused in different places

+

Clear visualization and interpretation

+

Types of Nodes

+
    +
  • Execution Nodes
      +
    • Condition Nodes
      • +
    • Action Nodes
      • +
    +
    • +
  • Decorator Nodes
      +
    • Not Node
      • +
    +
    • +
  • Control Flow Nodes
      +
    • Sequence Nodes
      • +
    • Fallback Nodes
      • +
    +
    • +
+

Execution Nodes - Condition Nodes

+

Condition nodes have a status of either SUCCESS or FAILURE

+

+

Execution Nodes - Action Nodes

+

Action nodes can either be active or inactive

+

An inactive node's status is not checked by the behavior tree, it is +shown in white

+

below

+

An active node's status is checked, it can either be SUCCESS (green), +RUNNING (blue) or FAILURE (red)

+

green whitebluered

+

Decorator Nodes - Not Nodes

+

The not node must have one condition node has a child and inverts the +status of the child.

+

If the child's status is SUCCESS, the not node's status will be FAILURE.

+

If the child's status is FAILURE, the not node's status will be SUCCESS.

+

+

Control Flow Nodes - Fallback Nodes

+

These nodes are shown with a ?

+

This node returns FAILURE if and only if all of its children return +FAILURE

+

If one of its children return RUNNING or SUCCESS, it returns RUNNING or +SUCCESS and no subsequent children's statuses are check

+

Below shows a typical example, where an action will only be performed if +all of the preceding conditions are false. In this case a drone will only be +armed if it is not already armed, it is in offboard mode and it is stationary

+

+

Control Flow Nodes - Sequence Nodes

+

These nodes are shown with a "->"

+

This node returns SUCCESS if and only if all of its children return +SUCCESS

+

If one of its children return RUNNING or FAILURE, it returns RUNNING or +FAILURE and no subsequent children's statuses are check

+

Below shows a typical example where preceding conditions must be true in +order for an action to be performed. In this case the drone will land if the IMU +times out and it is in offboard mode

+

+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ + + +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/docs/robot/autonomy/5_behavior/index.html b/docs/robot/autonomy/5_behavior/index.html new file mode 100644 index 00000000..844d4961 --- /dev/null +++ b/docs/robot/autonomy/5_behavior/index.html @@ -0,0 +1,2663 @@ + + + + + + + + + + + + + + + + + + + + + + + Behavior - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

Behavior

+

The behavior module is responsible for the high-level decision making of the robot. This includes deciding what actions to take based on the current state of the robot and the world around it. The behavior module is responsible for coordinating the actions of the local and global modules to achieve the robot's goals.

+

Launch

+

Launch files are under src/robot/autonomy/behavior/behavior_bringup/launch.

+

The main launch command is ros2 launch behavior_bringup behavior.launch.xml.

+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ + + +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/docs/robot/autonomy/5_behavior/media/image1.png b/docs/robot/autonomy/5_behavior/media/image1.png new file mode 100644 index 00000000..135407ae Binary files /dev/null and b/docs/robot/autonomy/5_behavior/media/image1.png differ diff --git a/docs/robot/autonomy/5_behavior/media/image10.png b/docs/robot/autonomy/5_behavior/media/image10.png new file mode 100644 index 00000000..81ecbd25 Binary files /dev/null and b/docs/robot/autonomy/5_behavior/media/image10.png differ diff --git a/docs/robot/autonomy/5_behavior/media/image2.png b/docs/robot/autonomy/5_behavior/media/image2.png new file mode 100644 index 00000000..efd5b594 Binary files /dev/null and b/docs/robot/autonomy/5_behavior/media/image2.png differ diff --git a/docs/robot/autonomy/5_behavior/media/image3.png b/docs/robot/autonomy/5_behavior/media/image3.png new file mode 100644 index 00000000..6b7f368d Binary files /dev/null and b/docs/robot/autonomy/5_behavior/media/image3.png differ diff --git a/docs/robot/autonomy/5_behavior/media/image4.png b/docs/robot/autonomy/5_behavior/media/image4.png new file mode 100644 index 00000000..ba20ef61 Binary files /dev/null and b/docs/robot/autonomy/5_behavior/media/image4.png differ diff --git a/docs/robot/autonomy/5_behavior/media/image5.png b/docs/robot/autonomy/5_behavior/media/image5.png new file mode 100644 index 00000000..35647b04 Binary files /dev/null and b/docs/robot/autonomy/5_behavior/media/image5.png differ diff --git a/docs/robot/autonomy/5_behavior/media/image6.png b/docs/robot/autonomy/5_behavior/media/image6.png new file mode 100644 index 00000000..3aefd3b4 Binary files /dev/null and b/docs/robot/autonomy/5_behavior/media/image6.png differ diff --git a/docs/robot/autonomy/5_behavior/media/image7.png b/docs/robot/autonomy/5_behavior/media/image7.png new file mode 100644 index 00000000..ae0061e0 Binary files /dev/null and b/docs/robot/autonomy/5_behavior/media/image7.png differ diff --git a/docs/robot/autonomy/5_behavior/media/image8.png b/docs/robot/autonomy/5_behavior/media/image8.png new file mode 100644 index 00000000..c6a313d6 Binary files /dev/null and b/docs/robot/autonomy/5_behavior/media/image8.png differ diff --git a/docs/robot/autonomy/5_behavior/media/image9.png b/docs/robot/autonomy/5_behavior/media/image9.png new file mode 100644 index 00000000..83265b26 Binary files /dev/null and b/docs/robot/autonomy/5_behavior/media/image9.png differ diff --git a/docs/robot/autonomy/index.html b/docs/robot/autonomy/index.html new file mode 100644 index 00000000..5640509d --- /dev/null +++ b/docs/robot/autonomy/index.html @@ -0,0 +1,2614 @@ + + + + + + + + + + + + + + + + + + + Autonomy Modules - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+ +
+ + + +
+ + + +
+ +
+ + + +
+
+
+ +
+ Copyright © 2024 - 2030 AirLab CMU +
+ + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + +
+ +
+
+
+ +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/docs/robot/common_topics/index.html b/docs/robot/common_topics/index.html new file mode 100644 index 00000000..fd8b8d8a --- /dev/null +++ b/docs/robot/common_topics/index.html @@ -0,0 +1,2728 @@ + + + + + + + + + + + + + + + + + + + + + + + Common topics - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

Common topics

+ + + + + + + + + + + + + + + + + + + + + +
TopicTypeDescription
/$ROBOT_NAME/odometrynav_msgs/OdometryBest estimate of robot odometry
/$ROBOT_NAME/global_plannav_msgs/PathCurrent target global trajectory for the robot to follow. See global planning for more details.
+

System Diagram

+

AirStack System Diagram

+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ + + +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/docs/robot/index.html b/docs/robot/index.html new file mode 100644 index 00000000..8e2ddf19 --- /dev/null +++ b/docs/robot/index.html @@ -0,0 +1,2661 @@ + + + + + + + + + + + + + + + + + + + + + + + Robot - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

Robot

+

Launch Structure

+

Each high-level module has a *_bringup package that contains the launch files for that module. The launch files are located in the launch directory of the *_bringup package. The launch files are named *.launch.(xml/yaml/py) and can be launched with ros2 launch <module_name>_bringup <module_name>.launch.(xml/yaml/py).

+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ + + +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/docs/robot/logging/data_offloading/index.html b/docs/robot/logging/data_offloading/index.html new file mode 100644 index 00000000..97b5d153 --- /dev/null +++ b/docs/robot/logging/data_offloading/index.html @@ -0,0 +1,2600 @@ + + + + + + + + + + + + + + + + + + + Data offloading - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

Data offloading

+ + + + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ +
+ + + +
+
+
+ +
+ Copyright © 2024 - 2030 AirLab CMU +
+ + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + +
+ +
+
+
+ +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/docs/robot/logging/index.html b/docs/robot/logging/index.html new file mode 100644 index 00000000..bd5438ab --- /dev/null +++ b/docs/robot/logging/index.html @@ -0,0 +1,2659 @@ + + + + + + + + + + + + + + + + + + + + + + + Logging - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

Logging

+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ + + +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/docs/robot/logging/rosbags/index.html b/docs/robot/logging/rosbags/index.html new file mode 100644 index 00000000..68273c01 --- /dev/null +++ b/docs/robot/logging/rosbags/index.html @@ -0,0 +1,2600 @@ + + + + + + + + + + + + + + + + + + + Rosbags - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

Rosbags

+ + + + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ +
+ + + +
+
+
+ +
+ Copyright © 2024 - 2030 AirLab CMU +
+ + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + +
+ +
+
+
+ +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/docs/robot/static_transforms/index.html b/docs/robot/static_transforms/index.html new file mode 100644 index 00000000..caac6ebe --- /dev/null +++ b/docs/robot/static_transforms/index.html @@ -0,0 +1,2664 @@ + + + + + + + + + + + + + + + + + + + + + + + Index - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

Index

+ +

Frame Conventions

+

Each robot has its own map frame that represents the starting position of the robot. +The map frame is expected to be in ENU (East-North-Up) convention.

+

The robot is in the base_link frame.

+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ + + +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/docs/robot/static_transforms/tf_tree.pdf b/docs/robot/static_transforms/tf_tree.pdf new file mode 100644 index 00000000..cae14669 Binary files /dev/null and b/docs/robot/static_transforms/tf_tree.pdf differ diff --git a/docs/simulation/docker_network/index.html b/docs/simulation/docker_network/index.html new file mode 100644 index 00000000..49f4f3a8 --- /dev/null +++ b/docs/simulation/docker_network/index.html @@ -0,0 +1,2618 @@ + + + + + + + + + + + + + + + + + + + Docker network - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

Docker network

+ +

Overview

+

The details of the docker containers setup is in the docker-compose.yaml file in the AirStack directory.

+

Isaac Sim, the ground control station, and robots all get created on the same default Docker bridge network. +This lets them communicate with ROS2 on the same network.

+

Each robot has its own ROS_DOMAIN_ID.

+

SSH into Robots

+

The ground-control-station and docker-robot containers are setup with ssh daemon, so you can ssh into the containers using the IP address.

+

You can get the IP address of each container by running the following command:

+
docker inspect -f '{{range.NetworkSettings.Networks}}{{.IPAddress}}{{end}}' [CONTAINER-NAME]
+
+

Then ssh in, for example:

+
ssh root@172.18.0.6
+
+

The ssh password is airstack.

+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ +
+ + + +
+
+
+ +
+ Copyright © 2024 - 2030 AirLab CMU +
+ + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + +
+ +
+
+
+ +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/docs/simulation/index.html b/docs/simulation/index.html new file mode 100644 index 00000000..37f803df --- /dev/null +++ b/docs/simulation/index.html @@ -0,0 +1,2654 @@ + + + + + + + + + + + + + + + + + + + + + + + Simulation - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

Simulation

+

We primarily support Isaac Sim. In the future we plan to support Gazebo.

+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ + + +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/docs/simulation/isaac_sim/ascent_node.png b/docs/simulation/isaac_sim/ascent_node.png new file mode 100644 index 00000000..bb3020b9 Binary files /dev/null and b/docs/simulation/isaac_sim/ascent_node.png differ diff --git a/docs/simulation/isaac_sim/ascent_sitl_extension/index.html b/docs/simulation/isaac_sim/ascent_sitl_extension/index.html new file mode 100644 index 00000000..7cdb7cc6 --- /dev/null +++ b/docs/simulation/isaac_sim/ascent_sitl_extension/index.html @@ -0,0 +1,2721 @@ + + + + + + + + + + + + + + + + + + + + + + + AirLab AirStack Extension - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

AirLab AirStack Extension

+

The AirStack extension for IsaacSim does two main things. It creates an Ascent Omnigraph Node which runs the Ascent SITL and updates the position of a drone model in IsaacSim based on the SITL. It also creates a panel for listing, attaching to, and killing tmux sessions.

+

Ascent OmniGraph Node

+

The Ascent OmniGraph node takes as input a domain id, node namespace and drone prim. It runs the Ascent SITL, mavproxy, and mavros and takes care of keeping the SITL time synced with IsaacSim's time. Mavros is run using the inputted domain id and node namespace. The drone prim's position is set based off of the position of the drone in the SITL. The drone prim doesn't do collision and will pass through objects in the IsaacSim world.

+

The way the SITL is synced with IsaacSim is by running the SITL in gdb with a breakpoint on the functin that advances the SITL time. Every time this function is called, our code is run by injecting a library using the LD_PRELOAD trick. Our code runs a client socket that talks to a server socket running in the AirStack IsaacSim extension which tells it how long to sleep based off the current SITL and IsaacSim time.

+

The Ascent OmniGraph node is shown below:

+

Ascent OmniGraph Node

+

TMUX Panel

+

This is a panel for listing, attaching to, and killing any running TMUX sessions. The Ascent SITL, mavproxy, and mavros are run in a TMUX sesion, so this is mainly for debugging those and probably doesn't need to be interacted with by most users. A list of TMUX sessions is displayed in the panel. It doesn't auto refresh so you have to manually click the refresh button to display any changes in the list of sessions. For each session, there is an Attach button and a Kill button. The Attach button will bring up an xterm window with the TMUX session. The Kill button will kill the TMUX session.

+

The TMUX panel is shown below:

+

TMUX Panel

+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ + + +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/docs/simulation/isaac_sim/export_stages_from_unreal/index.html b/docs/simulation/isaac_sim/export_stages_from_unreal/index.html new file mode 100644 index 00000000..ce433503 --- /dev/null +++ b/docs/simulation/isaac_sim/export_stages_from_unreal/index.html @@ -0,0 +1,2746 @@ + + + + + + + + + + + + + + + + + + + + + + + Export Unreal Engine to Isaac Sim - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

Export Unreal Engine to Isaac Sim

+

A robot needs a scene to interact in. A scene can be created in any 3D modeling program, though we have found it easiest to export stages from Unreal Engine. This document explains how to export an Unreal Engine environment to an Isaac Sim stage, then how to convert the stage to a physics-enabled scene.

+

Exporting Unreal Engine Environments to Isaac Sim Stages

+

Generally, Unreal Engine environments can be found on Epic Games' Fab Marketplace. For example, the Open World Demo Collection is a free collection of outdoor environments.

+

The below video explains how to export an Unreal Engine environment to an Isaac Sim stage.

+ + +

You can save this file as [YOUR_ENVIRONMENT_NAME].stage.usd.

+

Export Tips

+

Complexity: +Generally, it works best to export levels that are designed for UE4.27 and below. The advanced rendering features from UE5, i.e. nanite and lumen, aren't compatible with Omniverse. +If the level is optimized for the older UE4, then it's more compatible.

+

Omniverse doesn't perform well with large amounts of vegetation. Anything with complex vegetation takes a long long time to load. Procedural foliage doesn't export well either. Simple geometries like buildings and rocks work better.

+

That said you can still achieve photorealism by substituting complex geometries for high quality textures. Isaac seems to do fine with high quality textures.

+

Optimization: After exporting, edit the file with USD Composer and run the Scene Optimizer extension for faster performance. USD Composer can be installed via Omniverse Launcher.

+

Verify the Scale: The Omniverse exporter exports in centimeters, but Isaac Sim natively works in meters. For consistency, follow these steps to change the scene units to be meters.

+

To check the scale of the scene, you can add a cube in Isaac Sim and compare it to the exported scene. The cube is 1m x 1m x 1m.

+

Turn a Stage into a Physics-Enabled Scene

+

Adding physics to the stage is as simple as adding a Physics property with the "Colliders Preset", as described in the Isaac docs. +Then save the scene as [YOUR_ENVIRONMENT_NAME].scene.usd to clarify that it's a physics-enabled scene.

+

You're now ready to add robots to the scene on the next page.

+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ + + +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/docs/simulation/isaac_sim/index.html b/docs/simulation/isaac_sim/index.html new file mode 100644 index 00000000..1b3a7fd7 --- /dev/null +++ b/docs/simulation/isaac_sim/index.html @@ -0,0 +1,2675 @@ + + + + + + + + + + + + + + + + + + + + + + + Isaac Sim - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

Isaac Sim

+

The primary simulator we support is NVIDIA Isaac Sim. +We chose Isaac Sim as the best balance between photorealism and physics simulation.

+

USD File Naming Conventions

+

AirStack uses the following file naming conventions:

+

Purely 3D graphics

+
    +
  • +

    *.prop.usd ⟵ simply a 3D model with materials, typically encompassing just a single object. Used for individual assets or objects (as mentioned earlier), representing reusable props.

    +
    • +
  • +

    *.stage.usd ⟵ an environment composed of many props, but with no physics, no simulation, no robots. simply scene graphics

    +
    • +
+

Simulation-ready

+
    +
  • +

    *.robot.usd ⟵ a prop representing a robot plus ROS2 topic and TF publishers, physics, etc.

    +
    • +
  • +

    *.scene.usd ⟵ an environment PLUS physics, simulation, or robots

    +
    • +
+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ + + +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/docs/simulation/isaac_sim/omnigraph_config.png b/docs/simulation/isaac_sim/omnigraph_config.png new file mode 100644 index 00000000..874d098f Binary files /dev/null and b/docs/simulation/isaac_sim/omnigraph_config.png differ diff --git a/docs/simulation/isaac_sim/scene_setup/index.html b/docs/simulation/isaac_sim/scene_setup/index.html new file mode 100644 index 00000000..e3749972 --- /dev/null +++ b/docs/simulation/isaac_sim/scene_setup/index.html @@ -0,0 +1,2751 @@ + + + + + + + + + + + + + + + + + + + + + + + AirStack Scene Setup - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

AirStack Scene Setup

+

Creating a New Scene with Robots

+

The easiest way is to reference and copy an existing scene.

+

ROS Publishers Through OmniGraph

+

Configure Robot Name, ROS_DOMAIN_ID, and Topic Namespaces

+

Under the Spirit drone prim is an ActionGraph component, which is an Omnigraph. This component is used to configure the ROS publishers for the robot. The ActionGraph component has the following fields to configure:

+
    +
  • robot_name: The name of the robot. This is used as the top-level namespace for ROS topics.
    • +
  • domain_id: The ROS domain ID. This is used as the ROS_DOMAIN_ID for DDS networking.
    • +
+

The Omnigraph has subgraphs for each ROS publisher type. For example, TFs, Images, and PointClouds. The top-level robot_name and domain_id fields get fed into the subgraphs. The Topic Namespaces field should be set to the topic namespace in the subgraphs. This is used to namespace the ROS topics.

+

Image of omnigraphs

+

Customizing the Omnigraph

+

Common pre-built graphs for ROS may be added through the top menu bar: Isaac Utils > Common OmniGraphs. +This is helpful for creating various sensor publishers.

+

We recommend organizing your work into sub-graphs. +Copy your omnigraph template them into the top-level Omnigraph component, named "ActionGraph". Connect the robot_name and domain_id fields to your workflow. Then, select all the nodes in your workflow, right-click, and create a subgraph.

+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ + + +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/docs/simulation/isaac_sim/tmux_panel.png b/docs/simulation/isaac_sim/tmux_panel.png new file mode 100644 index 00000000..3550fd7a Binary files /dev/null and b/docs/simulation/isaac_sim/tmux_panel.png differ diff --git a/docs/stylesheets/extra.css b/docs/stylesheets/extra.css new file mode 100644 index 00000000..c87744e8 --- /dev/null +++ b/docs/stylesheets/extra.css @@ -0,0 +1,9 @@ +:root { + --md-primary-fg-color: #EE0F0F; + --md-primary-fg-color--light: #ECB7B7; + --md-primary-fg-color--dark: #90030C; + } + + [data-md-color-scheme="slate"] { + --md-hue: 210; + } diff --git a/ground_control_station/docker/Dockerfile.cot2planner_agent b/ground_control_station/docker/Dockerfile.cot2planner_agent new file mode 100644 index 00000000..01b8af47 --- /dev/null +++ b/ground_control_station/docker/Dockerfile.cot2planner_agent @@ -0,0 +1,34 @@ +# Base image with ROS2 Humble +# Base image with ROS2 Humble +FROM ros:humble + +ARG ROS_WS_DIR + +# Set working directory +WORKDIR ${ROS_WS_DIR} + +# Install necessary dependencies +RUN apt-get update && apt-get install -y \ + python3-colcon-common-extensions \ + ros-humble-ros2cli \ + python3-pip \ + libglib2.0-dev && \ + rm -rf /var/lib/apt/lists/* + +# Upgrade pip and install Python dependencies +RUN pip3 install --upgrade pip \ + && pip3 install setuptools==57.5.0 pytak pyyaml \ + && pip3 install paho-mqtt + +# Copy the local 'ros2tak_tools' directory into the container +COPY ground_control_station/ros_ws/src/ros2tak_tools ${ROS_WS_DIR}/src/ros2tak_tools + +# Copy the common airstack ros_package into the container +COPY common/ros_packages/airstack_msgs ${ROS_WS_DIR}/src/airstack_msgs + +# Build the ROS2 workspace +RUN /bin/bash -c "source /opt/ros/humble/setup.bash && \ + colcon build --symlink-install --packages-select airstack_msgs && \ + colcon build --symlink-install" + + diff --git a/ground_control_station/docker/Dockerfile.gst-ros-bridge b/ground_control_station/docker/Dockerfile.gst-ros-bridge new file mode 100644 index 00000000..ddc6e2ad --- /dev/null +++ b/ground_control_station/docker/Dockerfile.gst-ros-bridge @@ -0,0 +1,72 @@ +FROM nvidia/cuda:12.4.1-devel-ubuntu22.04 + +# Install humble +RUN apt-get update && apt-get install -y locales && \ + locale-gen en_US en_US.UTF-8 && \ + update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8 +ENV LANG=en_US.UTF-8 +ENV DEBIAN_FRONTEND=noninteractive + +RUN apt-get update && \ + apt-get install -y software-properties-common && \ + apt-add-repository universe + +RUN apt-get update && \ + apt-get install -y curl + +RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \ + -o /usr/share/keyrings/ros-archive-keyring.gpg && \ + echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | tee /etc/apt/sources.list.d/ros2.list > /dev/null + +RUN apt-get update && apt-get install -y \ + ros-dev-tools \ + ros-humble-ros-base \ + ros-humble-image-view + +# Utilities +RUN apt-get update && apt-get install -y \ + tmux \ + vim \ + wget + +# Install DIVOTrack dependencies +RUN apt-get update && apt-get install -y \ + python3-pip \ + python3-venv + +# Install ros-gst-bridge +RUN apt-get update && apt-get install -y \ + libgstreamer1.0-dev \ + libgstreamer-plugins-base1.0-dev \ + libgstreamer-plugins-bad1.0-dev \ + gstreamer1.0-plugins-base \ + gstreamer1.0-plugins-good \ + gstreamer1.0-plugins-bad \ + gstreamer1.0-plugins-ugly \ + gstreamer1.0-libav \ + gstreamer1.0-tools \ + gstreamer1.0-x \ + gstreamer1.0-alsa +RUN mkdir -p /ros_ws/src +WORKDIR /ros_ws +RUN git clone https://github.com/BrettRD/ros-gst-bridge.git src/ros-gst-bridge +RUN /bin/bash -c "source /opt/ros/humble/setup.bash && \ + rosdep init && \ + rosdep update && \ + rosdep install --from-paths /ros_ws/src/ --ignore-src -r -y && \ + colcon build" + +RUN apt-get update && apt-get install -y \ + libnvidia-decode-525-server \ + libnvidia-encode-525-server + +# Disable shared memory for ROS +COPY resources/fastrtps-profile.xml /fastrtps-profile.xml +ENV FASTRTPS_DEFAULT_PROFILES_FILE=/fastrtps-profile.xml + +RUN echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc +RUN echo "source /ros_ws/install/setup.bash" >> ~/.bashrc + +# Entrypoint +CMD ["/bin/bash"] + diff --git a/ground_control_station/docker/Dockerfile.ros2cot_agent b/ground_control_station/docker/Dockerfile.ros2cot_agent new file mode 100644 index 00000000..bfd90692 --- /dev/null +++ b/ground_control_station/docker/Dockerfile.ros2cot_agent @@ -0,0 +1,30 @@ +# Base image with ROS2 Humble +# Base image with ROS2 Humble +FROM ros:humble + +ARG ROS_WS_DIR + +# Set working directory +WORKDIR ${ROS_WS_DIR} + +# Install necessary dependencies +RUN apt-get update && apt-get install -y \ + python3-colcon-common-extensions \ + ros-humble-ros2cli \ + python3-pip \ + libglib2.0-dev && \ + rm -rf /var/lib/apt/lists/* + +# Upgrade pip and install Python dependencies +RUN pip3 install --upgrade pip \ + && pip3 install setuptools==57.5.0 pytak pyyaml \ + && pip3 install paho-mqtt + +# Copy the local 'ros2tak_tools' directory into the container +COPY /ros_ws/src/ros2tak_tools ${ROS_WS_DIR}/src/ros2tak_tools + +# Build the ROS2 workspace +RUN /bin/bash -c "source /opt/ros/humble/setup.bash && \ + colcon build --symlink-install" + + diff --git a/ground_control_station/docker/Dockerfile.tak_publisher b/ground_control_station/docker/Dockerfile.tak_publisher new file mode 100644 index 00000000..999469f5 --- /dev/null +++ b/ground_control_station/docker/Dockerfile.tak_publisher @@ -0,0 +1,12 @@ +# Base image with Python 3 +FROM python:3.10-slim + +ARG ROS_WS_DIR + +# Set working directory +WORKDIR ${ROS_WS_DIR} + +# Install necessary dependencies +# Install necessary dependencies +RUN pip install --no-cache-dir asyncio pytak pyyaml cryptography paho-mqtt + diff --git a/ground_control_station/docker/Dockerfile.tak_subscriber b/ground_control_station/docker/Dockerfile.tak_subscriber new file mode 100644 index 00000000..999469f5 --- /dev/null +++ b/ground_control_station/docker/Dockerfile.tak_subscriber @@ -0,0 +1,12 @@ +# Base image with Python 3 +FROM python:3.10-slim + +ARG ROS_WS_DIR + +# Set working directory +WORKDIR ${ROS_WS_DIR} + +# Install necessary dependencies +# Install necessary dependencies +RUN pip install --no-cache-dir asyncio pytak pyyaml cryptography paho-mqtt + diff --git a/ground_control_station/docker/docker-compose.yaml b/ground_control_station/docker/docker-compose.yaml new file mode 100644 index 00000000..1416b82c --- /dev/null +++ b/ground_control_station/docker/docker-compose.yaml @@ -0,0 +1,198 @@ +services: + ground-control-station: + image: TODO + container_name: ground-control-station + entrypoint: "" + command: > + bash -c "ssh service restart; + tmux new -d -s gcs_bringup + && tmux send-keys -t gcs_bringup 'ros2 launch gcs_bringup gcs.launch.xml' ENTER + && sleep infinity" + # Interactive shell + stdin_open: true # docker run -i + tty: true # docker run -t + # Needed to display graphical applications + ipc: host + privileged: true + networks: + - airstack_network + environment: + - DISPLAY + - QT_X11_NO_MITSHM=1 + deploy: + # let it use the GPU + resources: + reservations: + devices: + - driver: nvidia # https://stackoverflow.com/a/70761193 + count: 1 + capabilities: [gpu] + ports: + # for ssh + - 2222:22 + volumes: + # display stuff + - $HOME/.Xauthority:/root/.Xauthority + - /tmp/.X11-unix:/tmp/.X11-unix + # developer stuff + - .bashrc:/root/.bashrc:rw # bash config + - .bash_history:/root/.bash_history:rw # save cmdline history + - /var/run/docker.sock:/var/run/docker.sock # access docker API for container name + # autonomy stack stuff + - ../../common/ros_ws:/root/common/ros_ws:rw # common ROS packages + - ../ros_ws:/root/ros_ws:rw # gcs-specific ROS packages + + +####################### GSTREAMER TO ROS TOPICS ###################### + gst-ros-bridge-topic1: + container_name: "${PROJECT_NAME}-gst_ros_bridge1" + image: "${PROJECT_NAME}/gcs/gst-ros-bridge" + build: + context: . + dockerfile: Dockerfile.gst-ros-bridge + command: > + /bin/bash -c 'source /ros_ws/install/setup.bash && gst-launch-1.0 --gst-plugin-path=/ros_ws/install/gst_bridge/lib/gst_bridge/ + rtspsrc location="${CAMERA1_STREAM_IP}" latency=0 ! + rtph265depay ! h265parse ! avdec_h265 ! videoconvert ! + rosimagesink ros-topic="${CAMERA1_ROS_TOPIC}"' + environment: + - DISPLAY=${DISPLAY} + - DOCKER_BUILDKIT=0 + - CAMERA1_STREAM_IP=${CAMERA1_STREAM_IP} + - CAMERA1_ROS_TOPIC=${CAMERA1_ROS_TOPIC} + volumes: + - /tmp/.X11-unix:/tmp/.X11-unix + network_mode: host + + +####################### ROS2TAK TOOLS ###################### + ############### MQTT for the GCS + mqtt: + container_name: "mqtt" + image: eclipse-mosquitto:2.0.20 + restart: always + volumes: + - ../ros_ws/src/ros2tak_tools/mosquitto/config:/mosquitto/config + - ../ros_ws/src/ros2tak_tools/mosquitto/data:/mosquitto/data + - ../ros_ws/src/ros2tak_tools/mosquitto/log:/mosquitto/log + env_file: + - .env + ports: + - "1883:1883" + - "9001:9001" + healthcheck: + test: [ "CMD", "mosquitto_pub", "-h", "localhost", "-t", "healthcheck", "-m", "ping", "-u", "${MQTT_USERNAME}", "-P", "${MQTT_PASSWORD}" ] + interval: 5s + timeout: 3s + retries: 2 + start_period: 5s + networks: + - airstack_network + + ################## ROS2COT_AGENT + ros2cot_agent: + build: + context: ../ + dockerfile: docker/Dockerfile.ros2cot_agent + args: + - ROS_WS_DIR=${ROS_WS_DIR} + image: "${PROJECT_NAME}/gcs/ros2cot_agent" + container_name: "${PROJECT_NAME}-ros2cot_agent" + stdin_open: true + tty: true + restart: unless-stopped + depends_on: + mqtt: + condition: service_healthy + networks: + - airstack_network + command: [ + "/bin/bash", + "-c", + "source /opt/ros/humble/setup.bash && \ + source $ROS_WS_DIR/install/setup.bash && \ + ./install/ros2tak_tools/bin/ros2cot_agent --config $ROS_WS_DIR/$ROS2TAK_TOOLS_CONFIG_DIR/$ROS2TAK_TOOLS_CONFIG_FILENAME" + ] + + # ################### TAK_PUBLISHER + tak_publisher: + build: + context: ../ + dockerfile: docker/Dockerfile.tak_publisher + args: + - ROS_WS_DIR=${ROS_WS_DIR} + image: "${PROJECT_NAME}/gcs/tak_publisher" + container_name: "${PROJECT_NAME}-tak_publisher" + stdin_open: true + tty: true + restart: unless-stopped + depends_on: + mqtt: + condition: service_healthy + networks: + - airstack_network + volumes: + - ../ros_ws/src/ros2tak_tools/:${ROS_WS_DIR}/src/ros2tak_tools/ + command: [ + "python3", + "$TAK_PUBLISHER_FILEPATH", + "--config", + "$ROS2TAK_TOOLS_CONFIG_DIR/$ROS2TAK_TOOLS_CONFIG_FILENAME" + ] + + ################### TAK_SUBSCRIBER + tak_subscriber: + build: + context: ../ + dockerfile: docker/Dockerfile.tak_subscriber + args: + - ROS_WS_DIR=${ROS_WS_DIR} + image: "${PROJECT_NAME}/gcs/tak_subscriber" + container_name: "${PROJECT_NAME}-tak_subscriber" + stdin_open: true + tty: true + restart: unless-stopped + depends_on: + mqtt: + condition: service_healthy + networks: + - airstack_network + volumes: + - ../ros_ws/src/ros2tak_tools/:${ROS_WS_DIR}/src/ros2tak_tools/ + command: [ + "python3", + "$TAK_SUBSCRIBER_FILEPATH", + "--config", + "$ROS2TAK_TOOLS_CONFIG_DIR/$ROS2TAK_TOOLS_CONFIG_FILENAME" + ] + + + ################## ROS2COT_AGENT + cot2planner_agent: + build: + context: ../../ + dockerfile: ground_control_station/docker/Dockerfile.cot2planner_agent + args: + - ROS_WS_DIR=${ROS_WS_DIR} + image: "${PROJECT_NAME}/gcs/cot2planner_agent" + container_name: "${PROJECT_NAME}-cot2planner_agent" + stdin_open: true + tty: true + restart: unless-stopped + depends_on: + mqtt: + condition: service_healthy + networks: + - airstack_network + command: [ + "/bin/bash", + "-c", + "source /opt/ros/humble/setup.bash && \ + source $ROS_WS_DIR/install/setup.bash && \ + ./install/ros2tak_tools/bin/cot2planner_agent --config $ROS_WS_DIR/$ROS2TAK_TOOLS_CONFIG_DIR/$ROS2TAK_TOOLS_CONFIG_FILENAME" + ] + +########### NETWORKS ########### +networks: + airstack_network: + driver: bridge diff --git a/ground_control_station/docker/resources/fastrtps-profile.xml b/ground_control_station/docker/resources/fastrtps-profile.xml new file mode 100644 index 00000000..edacf502 --- /dev/null +++ b/ground_control_station/docker/resources/fastrtps-profile.xml @@ -0,0 +1,19 @@ + + + + + CustomUdpTransport + UDPv4 + + + + + + + CustomUdpTransport + + + false + + + diff --git a/ground_control_station/installation/index.html b/ground_control_station/installation/index.html new file mode 100644 index 00000000..9e5ad9d7 --- /dev/null +++ b/ground_control_station/installation/index.html @@ -0,0 +1,2602 @@ + + + + + + + + + + + + + + + + + + + Index - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

Index

+ +

Scripts to install on machine, todo. +Maybe something with ansible? +Bash scripts could work too.

+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ +
+ + + +
+
+
+ +
+ Copyright © 2024 - 2030 AirLab CMU +
+ + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + +
+ +
+
+
+ +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/ground_control_station/ros_ws/index.html b/ground_control_station/ros_ws/index.html new file mode 100644 index 00000000..355e6c09 --- /dev/null +++ b/ground_control_station/ros_ws/index.html @@ -0,0 +1,2603 @@ + + + + + + + + + + + + + + + + + + + Index - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

Index

+ +

Eventually we will have some ground control station that enables commanding all robots simultaneously.

+

Envision a GUI like an RTS game, where you have a global 3D map you can move around, and can also see and control your "units" on the map. +A minimap will help with understanding where you are. +Envision the map being built up over time in 3D as your agents explore.

+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ +
+ + + +
+
+
+ +
+ Copyright © 2024 - 2030 AirLab CMU +
+ + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + +
+ +
+
+
+ +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/ground_control_station/ros_ws/src/mission_manager/CMakeLists.txt b/ground_control_station/ros_ws/src/mission_manager/CMakeLists.txt new file mode 100644 index 00000000..461c7ef1 --- /dev/null +++ b/ground_control_station/ros_ws/src/mission_manager/CMakeLists.txt @@ -0,0 +1,69 @@ +cmake_minimum_required(VERSION 3.8) +project(mission_manager) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set(CGAL_DATA_DIR ".") + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(airstack_msgs REQUIRED) +find_package(grid_map_ros REQUIRED) +find_package(CGAL REQUIRED) + +add_executable(mission_manager_node + src/mission_manager_node.cpp + src/MissionManager.cpp + src/BeliefMap.cpp) + +add_executable(example_search_request_node + src/example_search_request.cpp) + +ament_target_dependencies(mission_manager_node rclcpp std_msgs airstack_msgs grid_map_ros) +ament_target_dependencies(example_search_request_node rclcpp airstack_msgs) + +target_include_directories(mission_manager_node PUBLIC + $ + $ + ${CGAL_INCLUDE_DIRS}) + +target_include_directories(example_search_request_node PUBLIC + $ + $) + +target_link_libraries(mission_manager_node + ${CGAL_LIBRARIES} # Link CGAL libraries +) + +target_compile_features(mission_manager_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +target_compile_features(example_search_request_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 + +install(TARGETS mission_manager_node + DESTINATION lib/${PROJECT_NAME}) + +install(TARGETS example_search_request_node + DESTINATION lib/${PROJECT_NAME}) + +install( + DIRECTORY launch config + DESTINATION share/${PROJECT_NAME} +) + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/ground_control_station/ros_ws/src/mission_manager/config/mission_manager.rviz b/ground_control_station/ros_ws/src/mission_manager/config/mission_manager.rviz new file mode 100644 index 00000000..d257ef6f --- /dev/null +++ b/ground_control_station/ros_ws/src/mission_manager/config/mission_manager.rviz @@ -0,0 +1,172 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /GridMap1 + Splitter Ratio: 0.5 + Tree Height: 872 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: false + Class: grid_map_rviz_plugin/GridMap + Color: 200; 200; 200 + Color Layer: probability + Color Transformer: IntensityLayer + Enabled: true + Height Layer: probability + Height Transformer: GridMapLayer + History Length: 1 + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 2 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: GridMap + Show Grid Lines: true + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /search_map_basestation + Use Rainbow: true + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Search Space Allocation Debugger + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /task_allocation_viz + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 660.692138671875 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 92.63066101074219 + Y: 60.50679016113281 + Z: -87.42770385742188 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.5697963237762451 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.71858024597168 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1163 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001bf000003f1fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000003f1000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003f1fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000003f1000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000004a6000003f100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1920 + X: 1920 + Y: 0 diff --git a/ground_control_station/ros_ws/src/mission_manager/config/mission_manager.yaml b/ground_control_station/ros_ws/src/mission_manager/config/mission_manager.yaml new file mode 100644 index 00000000..79d2d397 --- /dev/null +++ b/ground_control_station/ros_ws/src/mission_manager/config/mission_manager.yaml @@ -0,0 +1,11 @@ +mission_manager_node: + ros__parameters: + grid_cell_size: 5.0 + visualize_search_allocation: true + max_number_agents: 3 + active_agent_check_n_seconds: 5.0 + min_agent_altitude_to_be_active: 3.0 + time_till_agent_not_valid: 10.0 + max_planning_time: 5.0 + budget: 100.0 + desired_speed: 5.0 \ No newline at end of file diff --git a/ground_control_station/ros_ws/src/mission_manager/include/mission_manager/BeliefMap.h b/ground_control_station/ros_ws/src/mission_manager/include/mission_manager/BeliefMap.h new file mode 100644 index 00000000..e550085e --- /dev/null +++ b/ground_control_station/ros_ws/src/mission_manager/include/mission_manager/BeliefMap.h @@ -0,0 +1,70 @@ +#ifndef BELIEFMAP_H +#define BELIEFMAP_H + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "airstack_msgs/msg/search_mission_request.hpp" +#include "airstack_msgs/msg/search_prior.hpp" +#include "airstack_msgs/msg/keep_out_zone.hpp" +#include "airstack_msgs/msg/belief_map_data.hpp" + +class BeliefMap +{ +public: + + std::vector> polygon_bounds; + + // std::vector prior_list; // list of priors + + // map of structs to keep track of various traits at each grid point + // std::vector> priority; + // std::vector> sensor_model_id; + + BeliefMap(); + bool reset_map(rclcpp::Logger logger, airstack_msgs::msg::SearchMissionRequest search_mission_request, double grid_cell_size); + bool update_map(rclcpp::Logger logger, const airstack_msgs::msg::BeliefMapData::SharedPtr new_belief_data); + grid_map::GridMap map_; + + bool is_initialized() const + { + return !map_.getSize()(0) == 0; + } + + double get_min_x() const + { + return min_x; + } + double get_max_x() const + { + return max_x; + } + double get_min_y() const + { + return min_y; + } + double get_max_y() const + { + return max_y; + } + +private: + //search map allocation + double min_x = DBL_MAX; + double max_x = -DBL_MAX; + double min_y = DBL_MAX; + double max_y = -DBL_MAX; + double grid_cell_size_; + + +}; + +#endif // BELIEFMAP_H \ No newline at end of file diff --git a/ground_control_station/ros_ws/src/mission_manager/include/mission_manager/MissionManager.h b/ground_control_station/ros_ws/src/mission_manager/include/mission_manager/MissionManager.h new file mode 100644 index 00000000..b961951d --- /dev/null +++ b/ground_control_station/ros_ws/src/mission_manager/include/mission_manager/MissionManager.h @@ -0,0 +1,105 @@ +#ifndef MISSIONMANAGER_H_INCLUDED +#define MISSIONMANAGER_H_INCLUDED + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" +#include +#include "geometry_msgs/msg/point32.hpp" +#include +#include "airstack_msgs/msg/search_mission_request.hpp" +#include "airstack_msgs/msg/task_assignment.hpp" +#include "mission_manager/BeliefMap.h" + +typedef CGAL::Exact_predicates_inexact_constructions_kernel K; +typedef K::Point_2 Point; +typedef CGAL::Alpha_shape_vertex_base_2 Vb; +typedef CGAL::Alpha_shape_face_base_2 Fb; +typedef CGAL::Triangulation_data_structure_2 Tds; +typedef CGAL::Delaunay_triangulation_2 Triangulation_2; +typedef CGAL::Alpha_shape_2 Alpha_shape_2; +typedef Alpha_shape_2::Edge Edge; +typedef CGAL::Segment_2 Segment; + +struct ClusterPoint +{ + double x, y; + int cluster; + double cluster_dist; + + ClusterPoint(double _x, double _y, int _cluster_idx, double _cluster_dist) + { + x = _x; + y = _y; + cluster = _cluster_idx; + cluster_dist = _cluster_dist; + } +}; + + struct CompareClusterPoint +{ + bool operator()(const ClusterPoint& a, const ClusterPoint& b) const + { + return a.cluster_dist > b.cluster_dist; // Min-heap based on cluster_dist + } +}; + + +class MissionManager +{ + public: + MissionManager(int max_number_agents, double active_agent_check_n_seconds, double min_agent_altitude_to_be_active, double time_till_agent_not_valid); + + std::vector assign_tasks(rclcpp::Logger logger, + const airstack_msgs::msg::SearchMissionRequest &plan_request, + rclcpp::Publisher::SharedPtr pub, + bool visualize_search_allocation, + double max_planning_time, + double budget, + double desired_speed); + bool check_agent_changes(rclcpp::Logger logger, uint8_t robot_id, geometry_msgs::msg::Pose robot_pose, rclcpp::Time current_time); + bool check_target_changes(rclcpp::Logger logger, std::string target_list, rclcpp::Time current_time); + std::vector get_valid_agents() const { return valid_agents_; } + BeliefMap belief_map_; // TODO make private + + private: + int max_number_agents_; + rclcpp::Duration active_agent_check_n_seconds_; + double min_agent_altitude_to_be_active_; + rclcpp::Duration time_till_agent_not_valid_; + std::vector time_of_last_call_; + rclcpp::Time time_of_last_check_; + std::vector agent_poses_; + std::vector valid_agents_; + int scenario_coutner_ = 0; + + //search map allocation + std::vector>> allocate_search_map(rclcpp::Logger logger, + int number_of_search_tasks, + const airstack_msgs::msg::SearchMissionRequest &plan_request, + std::vector> &cluster_centroids, + std::vector> &clusters); + std::vector> calculate_cluster_centroids(rclcpp::Logger logger, int num_agents, const airstack_msgs::msg::SearchMissionRequest &plan_request, std::vector &CGAL_bounds) const; + std::vector> calculate_clusters(rclcpp::Logger logger, int num_agents, std::vector> cluster_centroids, const std::vector &CGAL_bounds) const; + std::vector>> calculate_cluster_bounds(rclcpp::Logger logger, std::vector> clusters) const; + + }; + #endif /* MISSIONMANAGER_H_INCLUDED */ \ No newline at end of file diff --git a/ground_control_station/ros_ws/src/mission_manager/include/mission_manager/mission_manager_node.h b/ground_control_station/ros_ws/src/mission_manager/include/mission_manager/mission_manager_node.h new file mode 100644 index 00000000..ecc329d0 --- /dev/null +++ b/ground_control_station/ros_ws/src/mission_manager/include/mission_manager/mission_manager_node.h @@ -0,0 +1,212 @@ +#ifndef MISSION_MANAGER_NODE_H_INCLUDED +#define MISSION_MANAGER_NODE_H_INCLUDED + +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +// #include "airstack_common/ros2_helper.hpp" +#include "std_msgs/msg/string.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "visualization_msgs/msg/marker_array.hpp" +#include "airstack_msgs/msg/search_mission_request.hpp" +#include "airstack_msgs/msg/task_assignment.hpp" +#include "airstack_msgs/msg/belief_map_data.hpp" +#include "mission_manager/BeliefMap.h" +#include "mission_manager/MissionManager.h" + +/* +Subscirbers +- all the drones odoms +- search map updates +- targets + +Publisher +- task assignment with plan request. Be together to avoid timing issues + +When receive mission request or change in number of targets or change in number of drones +- check how many targets +- check how many drones +- see how many left for search +- divide up the space based on number of drones +- assign nearest drone to each task (targets and search regions) +- send plan request to each drone +*/ + + +class MissionManagerNode : public rclcpp::Node +{ + public: + MissionManagerNode() + : Node("mission_manager") + { + double min_agent_altitude_to_be_active; + double active_agent_check_n_seconds; + double time_till_agent_not_valid; + + // grid_cell_size_ = airstack::get_param(this, "grid_cell_size", 10.0); + this->declare_parameter("grid_cell_size", 10.0); + this->declare_parameter("visualize_search_allocation", false); + this->declare_parameter("max_number_agents", 5); + this->declare_parameter("active_agent_check_n_seconds", 5.0); + this->declare_parameter("min_agent_altitude_to_be_active", 2.0); + this->declare_parameter("time_till_agent_not_valid", 10.0); + this->declare_parameter("max_planning_time", 5.0); + this->declare_parameter("budget", 100.0); + this->declare_parameter("desired_speed", 4.0); + this->get_parameter("grid_cell_size", grid_cell_size_); + this->get_parameter("visualize_search_allocation", visualize_search_allocation_); + this->get_parameter("max_number_agents", max_number_agents_); + this->get_parameter("active_agent_check_n_seconds", active_agent_check_n_seconds); + this->get_parameter("min_agent_altitude_to_be_active", min_agent_altitude_to_be_active); + this->get_parameter("time_till_agent_not_valid", time_till_agent_not_valid); + this->get_parameter("max_planning_time", max_planning_time_); + this->get_parameter("budget", budget_); + this->get_parameter("desired_speed", desired_speed_); + + mission_subscriber_ = this->create_subscription( + "search_mission_request", 1, std::bind(&MissionManagerNode::search_mission_request_callback, this, std::placeholders::_1)); + + // Create subscribers and publishers for max number of agents + for (uint8_t i = 0; i < max_number_agents_; i++) + { + std::string topic_name = "agent_" + std::to_string(i) + "/odom"; + agent_odoms_subs_.push_back( + this->create_subscription( + topic_name, 1, + [this, i](const nav_msgs::msg::Odometry::SharedPtr msg) { + this->agent_odom_callback(msg, i); + } + ) + ); + + std::string agent_topic = "agent_" + std::to_string(i) + "/plan_request"; + plan_request_pubs_.push_back( + this->create_publisher(agent_topic, 10)); + } + + tracked_targets_sub_ = this->create_subscription( + "tracked_targets", 1, std::bind(&MissionManagerNode::tracked_targets_callback, this, std::placeholders::_1)); + + belief_map_sub_ = this->create_subscription( + "belief_map_updates", 1, std::bind(&MissionManagerNode::belief_map_callback, this, std::placeholders::_1)); + + mission_manager_ = std::make_shared(this->max_number_agents_, active_agent_check_n_seconds, min_agent_altitude_to_be_active, time_till_agent_not_valid); + + // TODO: set param for rate, make sure not communicated over network + search_map_publisher_ = this->create_publisher( + "search_map_basestation", rclcpp::QoS(1).transient_local()); + + viz_pub_ = this->create_publisher("task_allocation_viz", 10.0); + + timer_ = this->create_wall_timer( + std::chrono::seconds(1), std::bind(&MissionManagerNode::search_map_publisher, this)); + } + + private: + /* --- ROS SPECIFIC --- */ + + // Publisher + std::vector::SharedPtr> plan_request_pubs_; + rclcpp::Publisher::SharedPtr viz_pub_; + rclcpp::Publisher::SharedPtr search_map_publisher_; + + // Subscribers + rclcpp::Subscription::SharedPtr mission_subscriber_; + rclcpp::Subscription::SharedPtr tracked_targets_sub_; + std::vector::SharedPtr> agent_odoms_subs_; + rclcpp::Subscription::SharedPtr belief_map_sub_; + + + /* --- MEMBER ATTRIBUTES --- */ + + rclcpp::TimerBase::SharedPtr timer_; + double grid_cell_size_; + bool visualize_search_allocation_; + int max_number_agents_; // TODO: get from param server + airstack_msgs::msg::SearchMissionRequest latest_search_mission_request_; + std::shared_ptr mission_manager_; + double max_planning_time_; + double budget_; + double desired_speed_; + + void publish_tasks(std::vector tasks) const + { + std::vector valid_agents = this->mission_manager_->get_valid_agents(); + for (uint8_t i = 0; i < this->max_number_agents_; i++) + { + if (valid_agents[i]) + { + plan_request_pubs_[i]->publish(tasks[i]); + RCLCPP_INFO(this->get_logger(), "Published task assignment for agent %d", i); + } + } + } + + void search_map_publisher() + { + if (this->mission_manager_->belief_map_.is_initialized()) + { + this->mission_manager_->belief_map_.map_.setTimestamp(this->now().nanoseconds()); + std::unique_ptr message; + message = grid_map::GridMapRosConverter::toMessage(this->mission_manager_->belief_map_.map_); + RCLCPP_DEBUG( + this->get_logger(), "Publishing grid map (timestamp %f).", + rclcpp::Time(message->header.stamp).nanoseconds() * 1e-9); + search_map_publisher_->publish(std::move(message)); + } + } + + void search_mission_request_callback(const airstack_msgs::msg::SearchMissionRequest::SharedPtr msg) + { + RCLCPP_INFO(this->get_logger(), "Received new search mission request"); + latest_search_mission_request_ = *msg; + + // TODO: visualize the seach mission request + this->mission_manager_->belief_map_.reset_map(this->get_logger(), *msg, grid_cell_size_); + this->publish_tasks(this->mission_manager_->assign_tasks( + this->get_logger(), latest_search_mission_request_, + viz_pub_, visualize_search_allocation_, + max_planning_time_, budget_, desired_speed_)); + } + + void agent_odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg, const uint8_t &robot_id) + { + RCLCPP_INFO(this->get_logger(), "Received agent odom for robot %d", robot_id); + if (this->mission_manager_->check_agent_changes(this->get_logger(), robot_id, msg->pose.pose, this->now())) + { + this->publish_tasks(this->mission_manager_->assign_tasks( + this->get_logger(), latest_search_mission_request_, + viz_pub_, visualize_search_allocation_, + max_planning_time_, budget_, desired_speed_)); + } + } + + void tracked_targets_callback(const std_msgs::msg::String::SharedPtr msg) + { + RCLCPP_INFO(this->get_logger(), "Received target track list '%s'", msg->data.c_str()); + // TODO: save the list of tracked target + + // Check if change in the number of targets or id numbers + if (this->mission_manager_->check_target_changes(this->get_logger(), msg->data, this->now())) + { + this->publish_tasks(this->mission_manager_->assign_tasks( + this->get_logger(), latest_search_mission_request_, + viz_pub_, visualize_search_allocation_, + max_planning_time_, budget_, desired_speed_)); + } + } + + void belief_map_callback(const airstack_msgs::msg::BeliefMapData::SharedPtr msg) const + { + RCLCPP_INFO_STREAM(this->get_logger(), "Received new belief map data at x: " << msg->x_start << " y: " << msg->y_start); + + this->mission_manager_->belief_map_.update_map(this->get_logger(), msg); + } + +}; +#endif /* MISSION_MANAGER_NODE_H_INCLUDED */ \ No newline at end of file diff --git a/ground_control_station/ros_ws/src/mission_manager/include/mission_manager/visualization.h b/ground_control_station/ros_ws/src/mission_manager/include/mission_manager/visualization.h new file mode 100644 index 00000000..0e09127c --- /dev/null +++ b/ground_control_station/ros_ws/src/mission_manager/include/mission_manager/visualization.h @@ -0,0 +1,101 @@ +#ifndef VISUALIZATIONS_MM_H_INCLUDED +#define VISUALIZATIONS_MM_H_INCLUDED + +#include +#include + +#include "mission_manager/MissionManager.h" + + +visualization_msgs::msg::MarkerArray visualize_multi_agent_search_request(int num_agents, + std::vector> cluster_centroids, + std::vector> clusters, + std::vector>> convex_hulls) +{ + visualization_msgs::msg::MarkerArray ma; + + visualization_msgs::msg::Marker m; + m.header.frame_id = "map"; + m.id = 0; + m.type = visualization_msgs::msg::Marker::CUBE_LIST; + m.action = visualization_msgs::msg::Marker::ADD; + m.pose.orientation.w = 1.0; + m.scale.x = 10.0; + m.scale.y = 10.0; + m.scale.z = 10.0; + m.color.a = 1.0; + m.color.r = 1.0; + m.color.g = 1.0; + m.color.b = 1.0; + + for(std::vector centroid : cluster_centroids) + { + geometry_msgs::msg::Point new_point; + new_point.x = centroid[0]; + new_point.y = centroid[1]; + new_point.z = 5.0; + m.points.push_back(new_point); + } + ma.markers.push_back(m); + + //publish visualization for different regions + std::vector> agent_colors = {{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}}; + for(int agent_idx = 0; agent_idx < num_agents; ++agent_idx) + { + visualization_msgs::msg::Marker cluster_m; + cluster_m.header.frame_id = "map"; + cluster_m.id = agent_idx + 1; + cluster_m.type = visualization_msgs::msg::Marker::CUBE_LIST; + cluster_m.action = visualization_msgs::msg::Marker::ADD; + cluster_m.pose.orientation.w = 1.0; + cluster_m.scale.x = 1.0; + cluster_m.scale.y = 1.0; + cluster_m.scale.z = 1.0; + cluster_m.color.a = 0.5; + std::vector agent_color = agent_colors[agent_idx]; + cluster_m.color.r = agent_color[0]; + cluster_m.color.g = agent_color[1]; + cluster_m.color.b = agent_color[2]; + + for(ClusterPoint cp : clusters[agent_idx]) + { + geometry_msgs::msg::Point new_point; + new_point.x = cp.x; + new_point.y = cp.y; + new_point.z = 1.0; + cluster_m.points.push_back(new_point); + } + ma.markers.push_back(cluster_m); + } + + //publish visualization for the convex hull of each region + for(auto agent_idx = 0u; agent_idx < convex_hulls.size(); ++agent_idx) + { + visualization_msgs::msg::Marker convex_hull_m; + convex_hull_m.header.frame_id = "map"; + convex_hull_m.id = agent_idx + 100; + convex_hull_m.type = visualization_msgs::msg::Marker::LINE_STRIP; + convex_hull_m.action = visualization_msgs::msg::Marker::ADD; + convex_hull_m.pose.orientation.w = 1.0; + convex_hull_m.scale.x = 2.0; + convex_hull_m.color.a = 1.0; + std::vector agent_color = agent_colors[agent_idx]; + convex_hull_m.color.r = agent_color[0]; + convex_hull_m.color.g = agent_color[1]; + convex_hull_m.color.b = agent_color[2]; + + auto convex_hull = convex_hulls[agent_idx]; + for(auto point : convex_hull) + { + // RCLCPP_INFO_STREAM(logger, point[0] << " " << point[1]); + geometry_msgs::msg::Point new_point; + new_point.x = point[0]; + new_point.y = point[1]; + new_point.z = 1.0; + convex_hull_m.points.push_back(new_point); + } + ma.markers.push_back(convex_hull_m); + } + return ma; +} +#endif /* VISUALIZATIONS_MM_H_INCLUDED */ \ No newline at end of file diff --git a/ground_control_station/ros_ws/src/mission_manager/index.html b/ground_control_station/ros_ws/src/mission_manager/index.html new file mode 100644 index 00000000..ffc9b0ef --- /dev/null +++ b/ground_control_station/ros_ws/src/mission_manager/index.html @@ -0,0 +1,2613 @@ + + + + + + + + + + + + + + + + + + + Mission Manager - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

Mission Manager

+

This package handles the allocation of tasks for the multiple agents. +It assigned agents to search or track. For search it divides the search space.

+
colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release
+
+

Debugging this node +

ros2 run --prefix 'gdb -ex run --args'  mission_manager mission_manager_node
+

+
ros2 launch mission_manager mission_manager_launch.py
+ros2 run rviz2 rviz2
+
+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ +
+ + + +
+
+
+ +
+ Copyright © 2024 - 2030 AirLab CMU +
+ + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + +
+ +
+
+
+ +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/ground_control_station/ros_ws/src/mission_manager/launch/mission_manager_launch.py b/ground_control_station/ros_ws/src/mission_manager/launch/mission_manager_launch.py new file mode 100644 index 00000000..15cadc46 --- /dev/null +++ b/ground_control_station/ros_ws/src/mission_manager/launch/mission_manager_launch.py @@ -0,0 +1,53 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + + # Get the package share directory + package_share_directory = get_package_share_directory('mission_manager') + print(package_share_directory) + + # Define the path to the configuration file + visualization_config_file = os.path.join(package_share_directory, 'config', 'grid_map.yaml') + rviz_config_file = os.path.join(package_share_directory, 'config', 'mission_manager.rviz') + + config = os.path.join( + package_share_directory, + 'config', + 'mission_manager.yaml' + ) + + mission_manager = Node( + package='mission_manager', + # namespace='mission_manager', + executable='mission_manager_node', + output="screen", + name='mission_manager_node', + parameters=[config] + ) + + # grid_map_visualization_node = Node( + # package='grid_map_visualization', + # executable='grid_map_visualization', + # name='grid_map_visualization', + # output='screen', + # parameters=[visualization_config_file] + # ) + + rviz2_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + output='screen', + arguments=['-d', rviz_config_file] + ) + + ld = LaunchDescription() + + ld.add_action(rviz2_node) + # ld.add_action(grid_map_visualization_node) + ld.add_action(mission_manager) + + return ld \ No newline at end of file diff --git a/ground_control_station/ros_ws/src/mission_manager/package.xml b/ground_control_station/ros_ws/src/mission_manager/package.xml new file mode 100644 index 00000000..9f7a7373 --- /dev/null +++ b/ground_control_station/ros_ws/src/mission_manager/package.xml @@ -0,0 +1,28 @@ + + + + mission_manager + 0.0.0 + + This package handles the allocation of tasks for the multiple agents. + It assigned agents to search or track. For search it divides the search space. + + root + TODO: License declaration + + ament_cmake + rclcpp + std_msgs + airstack_msgs + grid_map + visualization_msgs + + ament_lint_auto + ament_lint_common + + ros2launch + + + ament_cmake + + diff --git a/ground_control_station/ros_ws/src/mission_manager/src/BeliefMap.cpp b/ground_control_station/ros_ws/src/mission_manager/src/BeliefMap.cpp new file mode 100644 index 00000000..889868e2 --- /dev/null +++ b/ground_control_station/ros_ws/src/mission_manager/src/BeliefMap.cpp @@ -0,0 +1,135 @@ +#include "mission_manager/BeliefMap.h" + + +BeliefMap::BeliefMap() +: map_(std::vector({"probability", "priority"})) +{ } + +bool BeliefMap::reset_map(rclcpp::Logger logger, airstack_msgs::msg::SearchMissionRequest search_mission_request, double grid_cell_size) +{ + RCLCPP_INFO(logger, "Resetting map"); + // Setting up map. + // Get the max and min x and y values from the search area + grid_cell_size_ = grid_cell_size; + min_x = DBL_MAX; + max_x = -DBL_MAX; + min_y = DBL_MAX; + max_y = -DBL_MAX; + for (const auto& point : search_mission_request.search_bounds.points) + { + if (point.x < min_x) + { + min_x = point.x; + } + if (point.x > max_x) + { + max_x = point.x; + } + if (point.y < min_y) + { + min_y = point.y; + } + if (point.y > max_y) + { + max_y = point.y; + } + } + // TODO param for grid resolution + // TODO fix coordinate frames. Match airstack to IPP to grid map. grid map is NWU, IPP is ENU, airstack is ... + // round to nearest resolution + min_x = std::floor(min_x / grid_cell_size_) * grid_cell_size_; + max_x = std::ceil(max_x / grid_cell_size_) * grid_cell_size_; + min_y = std::floor(min_y / grid_cell_size_) * grid_cell_size_; + max_y = std::ceil(max_y / grid_cell_size_) * grid_cell_size_; + double x_length = std::abs(max_x - min_x); + double y_length = std::abs(max_y - min_y); + + map_.setGeometry(grid_map::Length(x_length, y_length), + grid_cell_size_, + grid_map::Position(std::round(min_x + x_length / 2.0), + std::round(min_y + y_length / 2.0))); + map_.setFrameId("map"); // TODO update to correct frame or set TF frame somewhere. There is already a map frame + map_.clearAll(); + + for (auto& search_prior : search_mission_request.search_priors) + { + if (search_prior.grid_prior_type == airstack_msgs::msg::SearchPrior::POLYGON_PRIOR) + { + if (search_prior.value.size() > 1 || search_prior.priority.size() > 1) + { + RCLCPP_ERROR(logger, "Polygon priors with multiple values or priorities not valid"); + continue; + } + grid_map::Polygon polygon; + polygon.setFrameId(map_.getFrameId()); + for (const auto& point : search_prior.points_list.points) + { + polygon.addVertex(grid_map::Position(point.x, point.y)); // TODO coordinate frame + } + grid_map::Matrix& probability_data = map_["probability"]; + grid_map::Matrix& priority_data = map_["priority"]; + for (grid_map::PolygonIterator iterator(map_, polygon); + !iterator.isPastEnd(); ++iterator) + { + // TODO: Check if the cell is in the search boundary (CGAL)? Not necessary I think. + const grid_map::Index index(*iterator); + float& current_value = probability_data(index(0), index(1)); + float& current_priority = priority_data(index(0), index(1)); + + if (std::isnan(current_value) || (current_value * current_priority) < (search_prior.value[0]*search_prior.priority[0])) { + current_value = search_prior.value[0]; + current_priority = search_prior.priority[0]; + } + } + } + else if (search_prior.grid_prior_type == airstack_msgs::msg::SearchPrior::LINE_SEG_PRIOR) + { + RCLCPP_ERROR(logger, "Line segment priors not implemented"); + } + else if (search_prior.grid_prior_type == airstack_msgs::msg::SearchPrior::POINT_PRIOR) + { + RCLCPP_ERROR(logger, "Point priors not implemented"); + } + else + { + RCLCPP_ERROR(logger, "Unknown grid prior type"); + } + } + + + return true; +} + +bool BeliefMap::update_map(rclcpp::Logger logger, const airstack_msgs::msg::BeliefMapData::SharedPtr new_belief_data) +{ + if (new_belief_data->x_start >= new_belief_data->x_end || + new_belief_data->y_start >= new_belief_data->y_end) + { + RCLCPP_ERROR(logger, "Received belief map data with origin issues"); + RCLCPP_WARN_STREAM(logger, "x_start: " << new_belief_data->x_start << " x_end: " << new_belief_data->x_end); + RCLCPP_WARN_STREAM(logger, "y_start: " << new_belief_data->y_start << " y_end: " << new_belief_data->y_end); + return false; + } + + grid_map::Index submapStartIndex(new_belief_data->x_start, new_belief_data->y_start); // check coordinate frame! + grid_map::Index submapBufferSize(new_belief_data->x_end - new_belief_data->x_start, + new_belief_data->y_end - new_belief_data->y_start); // check coordinate frame! + int cur_index_num = 0; + grid_map::Matrix& probability_data = map_["probability"]; + for (grid_map::SubmapIterator iterator(map_, submapStartIndex, submapBufferSize); + !iterator.isPastEnd(); ++iterator) + { + // map_.at("probability", *iterator) = 1.0; + const grid_map::Index index(*iterator); + // always save with the lower value + // TODO: Check not overwriting the values that are outside of the drone search area + if (new_belief_data->map_values[cur_index_num] != 0 && + new_belief_data->map_values[cur_index_num] < probability_data(index(0), index(1))) + { + // Scale the value back from 0-UIN16_MAX to 0-1 + probability_data(index(0), index(1)) = static_cast(new_belief_data->map_values[cur_index_num]) / static_cast(UINT16_MAX); // TODO: ensure maps are encoded correctly to match the iterator + } + cur_index_num++; + } + return true; +} diff --git a/ground_control_station/ros_ws/src/mission_manager/src/MissionManager.cpp b/ground_control_station/ros_ws/src/mission_manager/src/MissionManager.cpp new file mode 100644 index 00000000..0e99e798 --- /dev/null +++ b/ground_control_station/ros_ws/src/mission_manager/src/MissionManager.cpp @@ -0,0 +1,429 @@ +#include "mission_manager/MissionManager.h" +#include "mission_manager/visualization.h" + +/* +Empty Constructor +*/ +MissionManager::MissionManager(int max_number_agents, double active_agent_check_n_seconds, double min_agent_altitude_to_be_active, double time_till_agent_not_valid) : + max_number_agents_(max_number_agents), + active_agent_check_n_seconds_(rclcpp::Duration::from_seconds(active_agent_check_n_seconds)), + min_agent_altitude_to_be_active_(min_agent_altitude_to_be_active), + time_till_agent_not_valid_(rclcpp::Duration::from_seconds(time_till_agent_not_valid)) +{ + rclcpp::Time default_time(0, 0, RCL_ROS_TIME); + time_of_last_call_.resize(max_number_agents_, default_time); + time_of_last_check_ = default_time; + agent_poses_.resize(max_number_agents_); + valid_agents_.resize(max_number_agents_, false); +} + +bool MissionManager::check_agent_changes(rclcpp::Logger logger, uint8_t robot_id, geometry_msgs::msg::Pose robot_pose, rclcpp::Time current_time) +{ + RCLCPP_INFO(logger, "Checking agent changes"); + + time_of_last_call_[robot_id] = current_time; + agent_poses_[robot_id] = robot_pose; + + // Only check at the specified number of loops + if (time_of_last_check_ - current_time < active_agent_check_n_seconds_) + { + return false; + } + time_of_last_check_ = current_time; + + // Check how many agents have reported in the last x seconds + // If change in the agents reporting, reassign tasks + std::vector curr_valid_agents{false, false, false, false, false}; + rclcpp::Duration time_till_agent_not_valid = rclcpp::Duration::from_seconds(10.0); + for (uint8_t i = 0; i < max_number_agents_; i++) + { + if (current_time - time_of_last_call_[i] < time_till_agent_not_valid && + agent_poses_[i].position.z > min_agent_altitude_to_be_active_) + { + curr_valid_agents[i] = true; + } + } + bool change_in_agents = false; + if (curr_valid_agents != valid_agents_) + { + change_in_agents = true; + } + valid_agents_ = curr_valid_agents; + + return change_in_agents; +} + +bool MissionManager::check_target_changes(rclcpp::Logger logger, std::string target_list, rclcpp::Time current_time) +{ + // TODO + RCLCPP_INFO_STREAM(logger, "Checking target changes at time " + << current_time.nanoseconds() << " with target list " << target_list); + return false; +} + +std::vector> MissionManager::calculate_cluster_centroids(rclcpp::Logger logger, int num_agents, const airstack_msgs::msg::SearchMissionRequest &search_mission_request, std::vector &CGAL_bounds) const +{ + std::random_device dev; + std::mt19937 rng(dev()); + for(auto& point : search_mission_request.search_bounds.points) + { + // add to vector of bounds + CGAL_bounds.push_back(Point(point.x, point.y)); + } + double min_x = this->belief_map_.get_min_x(); + double max_x = this->belief_map_.get_max_x(); + double min_y = this->belief_map_.get_min_y(); + double max_y = this->belief_map_.get_max_y(); + + std::vector> boundary = {{min_x, min_y},{max_x, min_y},{max_x, max_y},{min_x, max_y}}; + + //divide bounds for the search area based on number of agents + double x_step_size = (max_x - min_x) / num_agents; + double y_step_size = (max_y - min_y) / num_agents; + + std::vector> rand_centroids; + if(num_agents == 3) + { + rand_centroids = {{20.0,60.0}, {142.0, 40.0}, {128.0, 119.0}}; + } + + //generate bounds for each subregion + while(static_cast(rand_centroids.size()) < num_agents) // Assumes rand centroids not crazy big + { + int i = rand_centroids.size(); + + //calculate bounds for random sampling + double min_x_sample = min_x + i * x_step_size; + double min_y_sample = min_y + i * y_step_size; + double max_x_sample = min_x + (i+1) * x_step_size; + double max_y_sample = min_y + (i+1) * y_step_size; + + //create uniform distributions to sample from + std::uniform_real_distribution rand_x_dist(min_x_sample, max_x_sample); + std::uniform_real_distribution rand_y_dist(min_y_sample, max_y_sample); + + //generate random samples + double rand_x = rand_x_dist(rng); + double rand_y = rand_y_dist(rng); + + //check if this random sample is within bounds + if (CGAL::bounded_side_2(CGAL_bounds.begin(), + CGAL_bounds.end(), + Point(rand_x, rand_y), K()) == CGAL::ON_BOUNDED_SIDE) + { + rand_centroids.push_back({rand_x, rand_y}); + RCLCPP_DEBUG_STREAM(logger, "Search mission centroid:" << rand_x << " " << rand_y); + } + } + return rand_centroids; +} + +double compute_dist(double x1, double y1, double x2, double y2) { + return std::sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1)); +} + +std::vector> MissionManager::calculate_clusters(rclcpp::Logger logger, int num_agents, std::vector> cluster_centroids, const std::vector &CGAL_bounds) const +{ + double min_x = this->belief_map_.get_min_x(); + double max_x = this->belief_map_.get_max_x(); + double min_y = this->belief_map_.get_min_y(); + double max_y = this->belief_map_.get_max_y(); + //calculate points in bounds + std::priority_queue, CompareClusterPoint> cluster_pq; + for(int x_idx = min_x; x_idx < max_x; ++x_idx) + { + for(int y_idx = min_y; y_idx < max_y; ++y_idx) + { + if (CGAL::bounded_side_2(CGAL_bounds.begin(), + CGAL_bounds.end(), + Point(x_idx, y_idx), K()) == CGAL::ON_BOUNDED_SIDE) + { + //find closest and farthest centroids to this point + int closest_centroid_idx = -1; + int farthest_centroid_idx = -1; + double min_dist = std::numeric_limits::infinity(); + double max_dist = -std::numeric_limits::infinity(); + for(int centroid_idx = 0; centroid_idx < static_cast(cluster_centroids.size()); ++centroid_idx) // Assumes rand centroids not crazy big + { + double dist = compute_dist(cluster_centroids[centroid_idx][0], cluster_centroids[centroid_idx][1], x_idx, y_idx); + if(dist < min_dist) + { + min_dist = dist; + closest_centroid_idx = centroid_idx; + } + if(dist > max_dist) + { + max_dist = dist; + farthest_centroid_idx = centroid_idx; + } + } + if (closest_centroid_idx == -1 || farthest_centroid_idx == -1) + { + RCLCPP_ERROR_STREAM(logger, "No closest or farthest centroid found for point " << x_idx << " " << y_idx); + } + cluster_pq.push(ClusterPoint(x_idx, y_idx, closest_centroid_idx, min_dist - max_dist)); + } + } + } + //actual do the clustering now + std::vector cluster_full; + std::vector> clusters; + int cluster_size = cluster_pq.size() / num_agents; + for(int i = 0; i < num_agents; ++i) + { + clusters.push_back({}); + cluster_full.push_back(false); + } + while(true) + { + ClusterPoint point = cluster_pq.top(); + // RCLCPP_INFO_STREAM(logger, point.cluster_dist << " " << point.x << " " << point.y); + cluster_pq.pop(); + + //check the size of preferred cluster + if(static_cast(clusters[point.cluster].size()) < cluster_size) // Assumes clusters not crazy big + { + clusters[point.cluster].push_back(point); + } + else if(!cluster_full[point.cluster]) + { + //pop all remaining elements off priority queue and resort + std::vector remaining_points; + while(!cluster_pq.empty()) + { + ClusterPoint point = cluster_pq.top(); + remaining_points.push_back(point); + cluster_pq.pop(); + } + //iterate through vector and resort + for(ClusterPoint cp : remaining_points) + { + //find closest and farthest centroids to this point + int closest_centroid_idx = -1; + int farthest_centroid_idx = -1; + double min_dist = std::numeric_limits::infinity(); + double max_dist = -std::numeric_limits::infinity(); + for(int centroid_idx = 0; centroid_idx < static_cast(cluster_centroids.size()); ++centroid_idx) // Assumes not crazy number of centroids + { + if(centroid_idx != point.cluster and !cluster_full[centroid_idx]) //check that it's not the existing full centroid OR a previous full centroid + { + double dist = compute_dist(cluster_centroids[centroid_idx][0], cluster_centroids[centroid_idx][1], cp.x, cp.y); + if(dist < min_dist) + { + min_dist = dist; + closest_centroid_idx = centroid_idx; + } + if(dist > max_dist) + { + max_dist = dist; + farthest_centroid_idx = centroid_idx; + } + } + } + if (closest_centroid_idx == -1 || farthest_centroid_idx == -1) + { + RCLCPP_ERROR_STREAM(logger, "V2: No closest or farthest centroid found for point " << cp.x << " " << cp.y); + } + cluster_pq.push(ClusterPoint(cp.x, cp.y, closest_centroid_idx, min_dist - max_dist)); + } + //set cluster as full so we don't redo this procedure till another cluster is full + cluster_full[point.cluster] = true; + // RCLCPP_INFO_STREAM(logger, "Cluster " << point.cluster << " full"); + } + // RCLCPP_INFO_STREAM(logger, cluster_pq.size()); + if(cluster_pq.empty()) + { + break; + } + } + return clusters; +} + +std::vector>> MissionManager::calculate_cluster_bounds(rclcpp::Logger logger, std::vector> clusters) const +{ + std::vector>> alpha_regions; + std::unordered_map, Segment, boost::hash>> boundary_edges; + if (clusters.size() == 0) + { + RCLCPP_ERROR(logger, "No clusters found"); + } + for(std::vector cluster : clusters) + { + //convert points inside polygon to cgal points + std::vector cgal_points; + for(ClusterPoint point : cluster) + { + cgal_points.push_back(Point(point.x, point.y)); + } + //calculate alpha shape formed by the polygon points + double alpha = 0.5; + Alpha_shape_2 A(cgal_points.begin(), cgal_points.end(), alpha, Alpha_shape_2::REGULARIZED); + //extract the boundary edges for the alpha shape + for (auto it = A.alpha_shape_edges_begin(); it != A.alpha_shape_edges_end(); ++it) + { + if (A.classify(*it) == Alpha_shape_2::REGULAR) + { + auto segment = A.segment(*it); + // Get the source and target points of the segment + Point source = segment.source(); + // Point target = segment.target(); + //put into our hashmap + boundary_edges[std::make_pair(source.x(), source.y())] = segment; + } + } + //grab the first key in the dictionary and set it's value as our start segment + std::pair start_key; + auto it = boundary_edges.begin(); + if (it != boundary_edges.end()) + { + start_key = it->first; + } + Segment start_seg = boundary_edges[start_key]; + Segment curr_seg = start_seg; + + //iterate through the segments and form the polygon + std::vector> region; + while(true) + { + //we want to find the target of our current segment as the source for a new segment + Point curr_seg_target = curr_seg.target(); + region.push_back({curr_seg_target.x(), curr_seg_target.y()}); + + //check if the current node target equals the start node source (we completed the loop) + if(curr_seg_target == start_seg.source()) + { + break; + } + + std::pair curr_key = std::make_pair(curr_seg_target.x(), curr_seg_target.y()); + if(boundary_edges.find(curr_key) != boundary_edges.end()) + { + Segment next_seg = boundary_edges[curr_key]; + + // RCLCPP_INFO_STREAM(logger, "(" << curr_seg.source().x() << "," << curr_seg.source().y() + // << ") (" << curr_seg.target().x() << "," << curr_seg.target().y() + // << ") -> (" << next_seg.source().x() << "," << next_seg.source().y() + // << ") (" << next_seg.target().x() << "," << next_seg.target().y() << ")"); + + curr_seg = next_seg; + } + } + alpha_regions.push_back(region); + } + return alpha_regions; +} + +std::vector>> MissionManager::allocate_search_map(rclcpp::Logger logger, + int number_of_search_tasks, + const airstack_msgs::msg::SearchMissionRequest &search_mission_request, + std::vector> &cluster_centroids, + std::vector> &clusters) +{ + std::vector CGAL_bounds; + //calculate random centroids + cluster_centroids = calculate_cluster_centroids(logger, number_of_search_tasks, search_mission_request, CGAL_bounds); + //calculate the clusters + clusters = calculate_clusters(logger, number_of_search_tasks, cluster_centroids, CGAL_bounds); + //calculate boundaries of clusters formed + return calculate_cluster_bounds(logger, clusters); +} + +std::vector MissionManager::assign_tasks( + rclcpp::Logger logger, + const airstack_msgs::msg::SearchMissionRequest &search_mission_request, + rclcpp::Publisher::SharedPtr pub, + bool visualize_search_allocation, + double max_planning_time, + double budget, + double desired_speed) +{ + RCLCPP_INFO(logger, "Assigning tasks to drones"); + + // Find how many active robots + // int num_active_agents = std::accumulate(valid_agents_.begin(), valid_agents_.end(), 0); + int num_active_agents = 3; //MAGIC NUMBER. TODO: UPDATE FOR LINE ABOVE WHEN REST OF SYSTEM COMPLETE + + // Decide how many search vs track tasks to assign + int number_of_track_tasks = 0; // TODO + int number_of_search_tasks = std::max(num_active_agents - number_of_track_tasks, 0); + RCLCPP_INFO_STREAM(logger, "Assigning " << number_of_search_tasks << " search tasks and " << number_of_track_tasks << " track tasks"); + if (number_of_search_tasks > 3) + { + RCLCPP_ERROR(logger, "Too many search tasks requested for the current implementation"); + return {}; + } + if (number_of_search_tasks == 0) + { + RCLCPP_WARN(logger, "No activate agents available for search tasks"); + return {}; + } + + // Send out the request for the search map division + std::vector> cluster_centroids; + std::vector> clusters; + std::vector>> cluster_bounds = allocate_search_map(logger, number_of_search_tasks, search_mission_request, cluster_centroids, clusters); + + //convert to task assignment + std::vector task_assignments(max_number_agents_); + // TODO: should allocate both search and track. Right now assumes all search tasks + for(int i = 0; i < max_number_agents_; ++i) + { + if (!valid_agents_[i]) + { + continue; + } + task_assignments[i].assigned_task_type = airstack_msgs::msg::TaskAssignment::SEARCH; + task_assignments[i].assigned_task_number = i; + task_assignments[i].plan_request.start_pose = agent_poses_[i]; + task_assignments[i].plan_request.max_planning_time = max_planning_time; + task_assignments[i].plan_request.maximum_range = budget; + + task_assignments[i].plan_request.desired_speed = desired_speed; + task_assignments[i].plan_request.search_bounds = search_mission_request.search_bounds; + + task_assignments[i].plan_request.clear_tree = true; + task_assignments[i].plan_request.scenario = scenario_coutner_; + + + grid_map::Polygon search_prior_polygon; + search_prior_polygon.setFrameId(this->belief_map_.map_.getFrameId()); + for(std::vector coord : cluster_bounds[i]) + { + search_prior_polygon.addVertex(grid_map::Position(coord[0], coord[1])); // TODO coordinate frame + geometry_msgs::msg::Point32 new_point; + new_point.x = coord[0]; + new_point.y = coord[1]; + new_point.z = 0.0; + task_assignments[i].plan_request.search_bounds.points.push_back(new_point); + } + + // Copy over the cell probabilities and priorities into new task + airstack_msgs::msg::SearchPrior task_search_prior; + geometry_msgs::msg::Point32 point_prior; + grid_map::Matrix& probability_data = this->belief_map_.map_["probability"]; + grid_map::Matrix& priority_data = this->belief_map_.map_["priority"]; + for (grid_map::PolygonIterator iterator(this->belief_map_.map_, search_prior_polygon); + !iterator.isPastEnd(); ++iterator) + { + const grid_map::Index index(*iterator); + grid_map::Position position; + this->belief_map_.map_.getPosition(*iterator, position); + point_prior.x = position.x(); + point_prior.y = position.y(); + point_prior.z = 0.0; + task_search_prior.points_list.points.push_back(point_prior); + task_search_prior.value.push_back(probability_data(index(0), index(1))); + task_search_prior.priority.push_back(priority_data(index(0), index(1))); + } + task_assignments[i].plan_request.search_priors.push_back(task_search_prior); + } + + //publish visualizations for search request + if (visualize_search_allocation) + { + pub->publish(visualize_multi_agent_search_request(num_active_agents, cluster_centroids, clusters, cluster_bounds)); + } + + scenario_coutner_++; + return task_assignments; +} \ No newline at end of file diff --git a/ground_control_station/ros_ws/src/mission_manager/src/example_search_request.cpp b/ground_control_station/ros_ws/src/mission_manager/src/example_search_request.cpp new file mode 100644 index 00000000..4e4d669d --- /dev/null +++ b/ground_control_station/ros_ws/src/mission_manager/src/example_search_request.cpp @@ -0,0 +1,170 @@ +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" +#include "airstack_msgs/msg/search_mission_request.hpp" +#include "airstack_msgs/msg/search_prior.hpp" +#include "airstack_msgs/msg/keep_out_zone.hpp" +#include "geometry_msgs/msg/polygon.hpp" +#include "geometry_msgs/msg/point32.hpp" + + +using namespace std::chrono_literals; + +/* This example creates a subclass of Node and uses std::bind() to register a +* member function as a callback from the timer. */ + +class MinimalPublisher : public rclcpp::Node +{ + public: + MinimalPublisher() + : Node("minimal_publisher") + { + publisher_ = this->create_publisher("search_mission_request", 10); + airstack_msgs::msg::SearchMissionRequest msg; + msg.header.stamp = this->now(); + msg.header.frame_id = "map"; // TODO + + geometry_msgs::msg::Polygon search_bounds; + geometry_msgs::msg::Point32 point; + point.x = 0; + point.y = 0; + point.z = 0; + search_bounds.points.push_back(point); + point.x = 100; + point.y = -28; + point.z = 0; + search_bounds.points.push_back(point); + point.x = 230; + point.y = 0; + point.z = 0; + search_bounds.points.push_back(point); + point.x = 250; + point.y = 100; + point.z = 0; + search_bounds.points.push_back(point); + point.x = 130; + point.y = 156; + point.z = 0; + search_bounds.points.push_back(point); + point.x = 110; + point.y = 191; + point.z = 0; + search_bounds.points.push_back(point); + point.x = -40; + point.y = 141; + point.z = 0; + search_bounds.points.push_back(point); + msg.search_bounds = search_bounds; + + airstack_msgs::msg::SearchPrior search_prior; + search_prior.value.push_back(0.5); + search_prior.priority.push_back(10.0); + search_prior.sensor_model_id.push_back(0); + search_prior.grid_prior_type = 1; + search_prior.header.frame_id = "local_enu"; + geometry_msgs::msg::Polygon grid_bounds; + point.x = 0; + point.y = 160; + point.z = 0; + grid_bounds.points.push_back(point); + point.x = 12; + point.y = 135; + point.z = 0; + grid_bounds.points.push_back(point); + point.x = 92; + point.y = 150; + point.z = 0; + grid_bounds.points.push_back(point); + point.x = 80; + point.y = 175; + point.z = 0; + grid_bounds.points.push_back(point); + search_prior.points_list = grid_bounds; + msg.search_priors.push_back(search_prior); + + airstack_msgs::msg::SearchPrior search_prior2; + search_prior2.value.push_back(0.2); + search_prior2.priority.push_back(1.0); + search_prior2.sensor_model_id.push_back(0); + search_prior2.grid_prior_type = 1; + search_prior2.header.frame_id = "local_enu"; + geometry_msgs::msg::Polygon points_list; + point.x = 0; + point.y = 0; + point.z = 0; + points_list.points.push_back(point); + point.x = 100; + point.y = -28; + point.z = 0; + points_list.points.push_back(point); + point.x = 230; + point.y = 0; + point.z = 0; + points_list.points.push_back(point); + point.x = 250; + point.y = 100; + point.z = 0; + points_list.points.push_back(point); + point.x = 130; + point.y = 156; + point.z = 0; + points_list.points.push_back(point); + point.x = 110; + point.y = 191; + point.z = 0; + points_list.points.push_back(point); + point.x = -40; + point.y = 141; + point.z = 0; + points_list.points.push_back(point); + search_prior2.points_list = points_list; + msg.search_priors.push_back(search_prior2); + + + + publisher_->publish(msg); + RCLCPP_INFO(this->get_logger(), "Publishing example search request"); + // Shutdown after publishing + // rclcpp::shutdown(); + } + + private: + rclcpp::Publisher::SharedPtr publisher_; +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} + + +/* +# keep out zone +keep_out_zones: + - + header: + frame_id: "local_enu" + x: 0 + y: 139 + radius: 15 + - + header: + frame_id: "local_enu" + x: 192 + y: -21 + radius: 10 + - + header: + frame_id: "local_enu" + x: 210 + y: -10 + radius: 8 + +*/ \ No newline at end of file diff --git a/ground_control_station/ros_ws/src/mission_manager/src/mission_manager_node.cpp b/ground_control_station/ros_ws/src/mission_manager/src/mission_manager_node.cpp new file mode 100644 index 00000000..d3038e53 --- /dev/null +++ b/ground_control_station/ros_ws/src/mission_manager/src/mission_manager_node.cpp @@ -0,0 +1,10 @@ + +#include "mission_manager/mission_manager_node.h" + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/config/config.yaml b/ground_control_station/ros_ws/src/ros2tak_tools/config/config.yaml new file mode 100644 index 00000000..80550269 --- /dev/null +++ b/ground_control_station/ros_ws/src/ros2tak_tools/config/config.yaml @@ -0,0 +1,100 @@ +# Configuration for the Basestation Syetem interacting ROS and TAK server. + +# Preliminary: +# Tak-server: Server that receives CoT messages and sends them to the TAK clients (e.g., ATAK, WinTAK, etc.) +# COT (Cursor on Target) message: A message format used by TAK clients to share location and other information with/without Tak-server. + +# Instructions: +# (1) Please read the comments in the configuration file to understand the configuration parameters. +# (2) The confguration key names are case-sensitive. Please use the same key names as mentioned in the comments. + +# Maintainer: Aditya Rauniyar (rauniyar@cmu.edu) + + +project: + name: Airstack # Name of the project. This will be used to name the services. + +robot: + count: 6 # Number of robots to subscribe to (e.g., 1 for robot1, 2 for robot2 and robot3, etc.) + gps_topicname: '/iphone{n}/gps' # Topic name pattern for GPS data. Use {n} as a placeholder for the robot number. + +trackers: + - name: 'base' # Note stable GPS reading + ip: '10.223.132.129' + input_port: 2947 + - name: 'target1' + ip: '10.223.132.61' + input_port: 2947 + - name: 'target2' # Name of the tracker. This can be the target or the robot streaming GPS data. + ip: '10.223.118.110' # IP address of the tracker. (Testing using Doodle labs mesh rider) + input_port: 2947 # Port of the Radio link to receive GPS data. + - name: 'target3' # NOTE: Stable GPS reading + ip: '10.223.118.94' + input_port: 2947 + - name: 'drone2' + ip: '10.4.1.20' + input_port: 2947 + + +tak_server: + cot_url: # URL for the TAK server where CoT events are sent. + pytak_tls_client_cert: # Path to the client certificate for TLS connection. + pytak_tls_client_key: # Path to the client key for TLS connection. + + +mqtt: + host: mqtt # TODO: This should be set as container name for MQTT Docker + port: 1883 + username: airlab + password: # Enter your password here + +services: + host: '127.0.0.1' # Host settings can be specified here (e.g., localhost or specific IP address). + + # NOTE: + # (1) The publishers and subscribers are in reference to the TAK server. + # (2) The name of the service would be in the format of _ (e.g., dsta_tak_publisher). + publisher: + tak_publisher: + # this serivce is used to publish CoT messages from HOSTIP:PORT to the TAK server. + topic_name: to_tak + + mediator: + ros2cot_agent: + # this service is used to generate COT messages from ROS messages and send them to HOSTIP:PORT. + topic_name: to_tak + cot2ros_agent: + # this service is used to generate ROS messages from HOSTIP:PORT to ROS topics. + # TAK_Subscriber (below) service has more information on the ROS topics. + topic_name: from_tak # Port for the ROS publisher service. + cot2planner_agent: + # this service is used to generate ROS messages from HOSTIP:PORT to ROS topics. + # TAK_Subscriber (below) service has more information on the ROS topics. + topic_name: planner_events # Topic name at MQTT for the subscriber service that sends COT messages subscribed from the TAK server. + + subscriber: + tak_subscriber: + # this service is used to subscribe to CoT messages from TAK server and send them to HOSTIP:PORT. + filter_messages: # Type of messages to subscribe to. Options: + - name: 'target' + # ROS Topic name to publish the target messages. Use {n} as a placeholder for the robot number. + # If provided, the topic name will be formatted with the robot number extracted from the message name. + ros_topic_name: '/target{n}/gps/gt' + ros_msg_type: NavSatFix # ROS message type for the target messages. + mqtt_topic_name: target_from_tak # Topic name at MQTT for the subscriber service that sends COT messages subscribed from the TAK server. + - name: 'iphone' + # ROS Topic name to publish the target messages. Use {n} as a placeholder for the robot number. + # If provided, the topic name will be formatted with the robot number extracted from the message name. + ros_topic_name: '/iphone{n}/gps/gt' + ros_msg_type: NavSatFix # ROS message type for the target messages. + mqtt_topic_name: iphone_from_tak # Topic name at MQTT for the subscriber service that sends COT messages subscribed from the TAK server. + - name: 'base' + ros_topic_name: '/basestation/gps' + ros_msg_type: NavSatFix # ROS message type for the target messages. + mqtt_topic_name: base_from_tak # Topic name at MQTT for the subscriber service that sends COT messages subscribed from the TAK server. + - name: 'planner' + ros_topic_name: '/planner/planconfig' # ROS Topic name to publish the shapes messages. + ros_msg_type: MarkerArray # ROS message type for the shapes messages. + mqtt_topic_name: planner_events # Topic name at MQTT for the subscriber service that sends COT messages subscribed from the TAK server. + # target: Target messages + # planner: Planner messages \ No newline at end of file diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/index.html b/ground_control_station/ros_ws/src/ros2tak_tools/index.html new file mode 100644 index 00000000..0b61cca3 --- /dev/null +++ b/ground_control_station/ros_ws/src/ros2tak_tools/index.html @@ -0,0 +1,2610 @@ + + + + + + + + + + + + + + + + + + + TAK Tools ROS 2 Package - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

TAK Tools ROS 2 Package

+

ros2tak_tools is a ROS 2 package designed for integrating TAK (Tactical Assault Kit) functionalities within ROS 2. It includes tools for publishing and subscribing to Cursor-On-Target (CoT) events, interfacing with TAK servers, and setting up search and rescue missions.

+

Features

+
    +
  • ROS 2 to TAK Communication: Send ROS 2 messages to TAK using CoT events.
    • +
  • CoT to ROS 2 Communication: Receive CoT events from TAK and publish as ROS 2 messages.
    • +
  • Mission Planning: Custom tools to create and manage search missions using CoT and ROS.
    • +
+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ +
+ + + +
+
+
+ +
+ Copyright © 2024 - 2030 AirLab CMU +
+ + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + +
+ +
+
+
+ +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/mosquitto/config/mosquitto.conf b/ground_control_station/ros_ws/src/ros2tak_tools/mosquitto/config/mosquitto.conf new file mode 100644 index 00000000..9cec7c97 --- /dev/null +++ b/ground_control_station/ros_ws/src/ros2tak_tools/mosquitto/config/mosquitto.conf @@ -0,0 +1,905 @@ +# Config file for mosquitto +# +# See mosquitto.conf(5) for more information. +# +# Default values are shown, uncomment to change. +# +# Use the # character to indicate a comment, but only if it is the +# very first character on the line. + +# ================================================================= +# General configuration +# ================================================================= + +# Use per listener security settings. +# +# It is recommended this option be set before any other options. +# +# If this option is set to true, then all authentication and access control +# options are controlled on a per listener basis. The following options are +# affected: +# +# acl_file +# allow_anonymous +# allow_zero_length_clientid +# auto_id_prefix +# password_file +# plugin +# plugin_opt_* +# psk_file +# +# Note that if set to true, then a durable client (i.e. with clean session set +# to false) that has disconnected will use the ACL settings defined for the +# listener that it was most recently connected to. +# +# The default behaviour is for this to be set to false, which maintains the +# setting behaviour from previous versions of mosquitto. +#per_listener_settings false + + +# This option controls whether a client is allowed to connect with a zero +# length client id or not. This option only affects clients using MQTT v3.1.1 +# and later. If set to false, clients connecting with a zero length client id +# are disconnected. If set to true, clients will be allocated a client id by +# the broker. This means it is only useful for clients with clean session set +# to true. +#allow_zero_length_clientid true + +# If allow_zero_length_clientid is true, this option allows you to set a prefix +# to automatically generated client ids to aid visibility in logs. +# Defaults to 'auto-' +#auto_id_prefix auto- + +# This option affects the scenario when a client subscribes to a topic that has +# retained messages. It is possible that the client that published the retained +# message to the topic had access at the time they published, but that access +# has been subsequently removed. If check_retain_source is set to true, the +# default, the source of a retained message will be checked for access rights +# before it is republished. When set to false, no check will be made and the +# retained message will always be published. This affects all listeners. +#check_retain_source true + +# QoS 1 and 2 messages will be allowed inflight per client until this limit +# is exceeded. Defaults to 0. (No maximum) +# See also max_inflight_messages +#max_inflight_bytes 0 + +# The maximum number of QoS 1 and 2 messages currently inflight per +# client. +# This includes messages that are partway through handshakes and +# those that are being retried. Defaults to 20. Set to 0 for no +# maximum. Setting to 1 will guarantee in-order delivery of QoS 1 +# and 2 messages. +#max_inflight_messages 20 + +# For MQTT v5 clients, it is possible to have the server send a "server +# keepalive" value that will override the keepalive value set by the client. +# This is intended to be used as a mechanism to say that the server will +# disconnect the client earlier than it anticipated, and that the client should +# use the new keepalive value. The max_keepalive option allows you to specify +# that clients may only connect with keepalive less than or equal to this +# value, otherwise they will be sent a server keepalive telling them to use +# max_keepalive. This only applies to MQTT v5 clients. The default, and maximum +# value allowable, is 65535. +# +# Set to 0 to allow clients to set keepalive = 0, which means no keepalive +# checks are made and the client will never be disconnected by the broker if no +# messages are received. You should be very sure this is the behaviour that you +# want. +# +# For MQTT v3.1.1 and v3.1 clients, there is no mechanism to tell the client +# what keepalive value they should use. If an MQTT v3.1.1 or v3.1 client +# specifies a keepalive time greater than max_keepalive they will be sent a +# CONNACK message with the "identifier rejected" reason code, and disconnected. +# +#max_keepalive 65535 + +# For MQTT v5 clients, it is possible to have the server send a "maximum packet +# size" value that will instruct the client it will not accept MQTT packets +# with size greater than max_packet_size bytes. This applies to the full MQTT +# packet, not just the payload. Setting this option to a positive value will +# set the maximum packet size to that number of bytes. If a client sends a +# packet which is larger than this value, it will be disconnected. This applies +# to all clients regardless of the protocol version they are using, but v3.1.1 +# and earlier clients will of course not have received the maximum packet size +# information. Defaults to no limit. Setting below 20 bytes is forbidden +# because it is likely to interfere with ordinary client operation, even with +# very small payloads. +#max_packet_size 0 + +# QoS 1 and 2 messages above those currently in-flight will be queued per +# client until this limit is exceeded. Defaults to 0. (No maximum) +# See also max_queued_messages. +# If both max_queued_messages and max_queued_bytes are specified, packets will +# be queued until the first limit is reached. +#max_queued_bytes 0 + +# Set the maximum QoS supported. Clients publishing at a QoS higher than +# specified here will be disconnected. +#max_qos 2 + +# The maximum number of QoS 1 and 2 messages to hold in a queue per client +# above those that are currently in-flight. Defaults to 1000. Set +# to 0 for no maximum (not recommended). +# See also queue_qos0_messages. +# See also max_queued_bytes. +#max_queued_messages 1000 +# +# This option sets the maximum number of heap memory bytes that the broker will +# allocate, and hence sets a hard limit on memory use by the broker. Memory +# requests that exceed this value will be denied. The effect will vary +# depending on what has been denied. If an incoming message is being processed, +# then the message will be dropped and the publishing client will be +# disconnected. If an outgoing message is being sent, then the individual +# message will be dropped and the receiving client will be disconnected. +# Defaults to no limit. +#memory_limit 0 + +# This option sets the maximum publish payload size that the broker will allow. +# Received messages that exceed this size will not be accepted by the broker. +# The default value is 0, which means that all valid MQTT messages are +# accepted. MQTT imposes a maximum payload size of 268435455 bytes. +#message_size_limit 0 + +# This option allows the session of persistent clients (those with clean +# session set to false) that are not currently connected to be removed if they +# do not reconnect within a certain time frame. This is a non-standard option +# in MQTT v3.1. MQTT v3.1.1 and v5.0 allow brokers to remove client sessions. +# +# Badly designed clients may set clean session to false whilst using a randomly +# generated client id. This leads to persistent clients that connect once and +# never reconnect. This option allows these clients to be removed. This option +# allows persistent clients (those with clean session set to false) to be +# removed if they do not reconnect within a certain time frame. +# +# The expiration period should be an integer followed by one of h d w m y for +# hour, day, week, month and year respectively. For example +# +# persistent_client_expiration 2m +# persistent_client_expiration 14d +# persistent_client_expiration 1y +# +# The default if not set is to never expire persistent clients. +#persistent_client_expiration + +# Write process id to a file. Default is a blank string which means +# a pid file shouldn't be written. +# This should be set to /var/run/mosquitto/mosquitto.pid if mosquitto is +# being run automatically on boot with an init script and +# start-stop-daemon or similar. +#pid_file + +# Set to true to queue messages with QoS 0 when a persistent client is +# disconnected. These messages are included in the limit imposed by +# max_queued_messages and max_queued_bytes +# Defaults to false. +# This is a non-standard option for the MQTT v3.1 spec but is allowed in +# v3.1.1. +#queue_qos0_messages false + +# Set to false to disable retained message support. If a client publishes a +# message with the retain bit set, it will be disconnected if this is set to +# false. +#retain_available true + +# Disable Nagle's algorithm on client sockets. This has the effect of reducing +# latency of individual messages at the potential cost of increasing the number +# of packets being sent. +#set_tcp_nodelay false + +# Time in seconds between updates of the $SYS tree. +# Set to 0 to disable the publishing of the $SYS tree. +#sys_interval 10 + +# The MQTT specification requires that the QoS of a message delivered to a +# subscriber is never upgraded to match the QoS of the subscription. Enabling +# this option changes this behaviour. If upgrade_outgoing_qos is set true, +# messages sent to a subscriber will always match the QoS of its subscription. +# This is a non-standard option explicitly disallowed by the spec. +#upgrade_outgoing_qos false + +# When run as root, drop privileges to this user and its primary +# group. +# Set to root to stay as root, but this is not recommended. +# If set to "mosquitto", or left unset, and the "mosquitto" user does not exist +# then it will drop privileges to the "nobody" user instead. +# If run as a non-root user, this setting has no effect. +# Note that on Windows this has no effect and so mosquitto should be started by +# the user you wish it to run as. +#user mosquitto + +# ================================================================= +# Listeners +# ================================================================= + +# Listen on a port/ip address combination. By using this variable +# multiple times, mosquitto can listen on more than one port. If +# this variable is used and neither bind_address nor port given, +# then the default listener will not be started. +# The port number to listen on must be given. Optionally, an ip +# address or host name may be supplied as a second argument. In +# this case, mosquitto will attempt to bind the listener to that +# address and so restrict access to the associated network and +# interface. By default, mosquitto will listen on all interfaces. +# Note that for a websockets listener it is not possible to bind to a host +# name. +# +# On systems that support Unix Domain Sockets, it is also possible +# to create a # Unix socket rather than opening a TCP socket. In +# this case, the port number should be set to 0 and a unix socket +# path must be provided, e.g. +# listener 0 /tmp/mosquitto.sock +# +# listener port-number [ip address/host name/unix socket path] +listener 1883 0.0.0.0 +listener 9001 0.0.0.0 + +# By default, a listener will attempt to listen on all supported IP protocol +# versions. If you do not have an IPv4 or IPv6 interface you may wish to +# disable support for either of those protocol versions. In particular, note +# that due to the limitations of the websockets library, it will only ever +# attempt to open IPv6 sockets if IPv6 support is compiled in, and so will fail +# if IPv6 is not available. +# +# Set to `ipv4` to force the listener to only use IPv4, or set to `ipv6` to +# force the listener to only use IPv6. If you want support for both IPv4 and +# IPv6, then do not use the socket_domain option. +# +#socket_domain + +# Bind the listener to a specific interface. This is similar to +# the [ip address/host name] part of the listener definition, but is useful +# when an interface has multiple addresses or the address may change. If used +# with the [ip address/host name] part of the listener definition, then the +# bind_interface option will take priority. +# Not available on Windows. +# +# Example: bind_interface eth0 +#bind_interface + +# When a listener is using the websockets protocol, it is possible to serve +# http data as well. Set http_dir to a directory which contains the files you +# wish to serve. If this option is not specified, then no normal http +# connections will be possible. +#http_dir + +# The maximum number of client connections to allow. This is +# a per listener setting. +# Default is -1, which means unlimited connections. +# Note that other process limits mean that unlimited connections +# are not really possible. Typically the default maximum number of +# connections possible is around 1024. +#max_connections -1 + +# The listener can be restricted to operating within a topic hierarchy using +# the mount_point option. This is achieved be prefixing the mount_point string +# to all topics for any clients connected to this listener. This prefixing only +# happens internally to the broker; the client will not see the prefix. +#mount_point + +# Choose the protocol to use when listening. +# This can be either mqtt or websockets. +# Certificate based TLS may be used with websockets, except that only the +# cafile, certfile, keyfile, ciphers, and ciphers_tls13 options are supported. +protocol websockets + +# Set use_username_as_clientid to true to replace the clientid that a client +# connected with with its username. This allows authentication to be tied to +# the clientid, which means that it is possible to prevent one client +# disconnecting another by using the same clientid. +# If a client connects with no username it will be disconnected as not +# authorised when this option is set to true. +# Do not use in conjunction with clientid_prefixes. +# See also use_identity_as_username. +# This does not apply globally, but on a per-listener basis. +#use_username_as_clientid + +# Change the websockets headers size. This is a global option, it is not +# possible to set per listener. This option sets the size of the buffer used in +# the libwebsockets library when reading HTTP headers. If you are passing large +# header data such as cookies then you may need to increase this value. If left +# unset, or set to 0, then the default of 1024 bytes will be used. +#websockets_headers_size + +# ----------------------------------------------------------------- +# Certificate based SSL/TLS support +# ----------------------------------------------------------------- +# The following options can be used to enable certificate based SSL/TLS support +# for this listener. Note that the recommended port for MQTT over TLS is 8883, +# but this must be set manually. +# +# See also the mosquitto-tls man page and the "Pre-shared-key based SSL/TLS +# support" section. Only one of certificate or PSK encryption support can be +# enabled for any listener. + +# Both of certfile and keyfile must be defined to enable certificate based +# TLS encryption. + +# Path to the PEM encoded server certificate. +#certfile + +# Path to the PEM encoded keyfile. +#keyfile + +# If you wish to control which encryption ciphers are used, use the ciphers +# option. The list of available ciphers can be optained using the "openssl +# ciphers" command and should be provided in the same format as the output of +# that command. This applies to TLS 1.2 and earlier versions only. Use +# ciphers_tls1.3 for TLS v1.3. +#ciphers + +# Choose which TLS v1.3 ciphersuites are used for this listener. +# Defaults to "TLS_AES_256_GCM_SHA384:TLS_CHACHA20_POLY1305_SHA256:TLS_AES_128_GCM_SHA256" +#ciphers_tls1.3 + +# If you have require_certificate set to true, you can create a certificate +# revocation list file to revoke access to particular client certificates. If +# you have done this, use crlfile to point to the PEM encoded revocation file. +#crlfile + +# To allow the use of ephemeral DH key exchange, which provides forward +# security, the listener must load DH parameters. This can be specified with +# the dhparamfile option. The dhparamfile can be generated with the command +# e.g. "openssl dhparam -out dhparam.pem 2048" +#dhparamfile + +# By default an TLS enabled listener will operate in a similar fashion to a +# https enabled web server, in that the server has a certificate signed by a CA +# and the client will verify that it is a trusted certificate. The overall aim +# is encryption of the network traffic. By setting require_certificate to true, +# the client must provide a valid certificate in order for the network +# connection to proceed. This allows access to the broker to be controlled +# outside of the mechanisms provided by MQTT. +#require_certificate false + +# cafile and capath define methods of accessing the PEM encoded +# Certificate Authority certificates that will be considered trusted when +# checking incoming client certificates. +# cafile defines the path to a file containing the CA certificates. +# capath defines a directory that will be searched for files +# containing the CA certificates. For capath to work correctly, the +# certificate files must have ".crt" as the file ending and you must run +# "openssl rehash " each time you add/remove a certificate. +#cafile +#capath + + +# If require_certificate is true, you may set use_identity_as_username to true +# to use the CN value from the client certificate as a username. If this is +# true, the password_file option will not be used for this listener. +#use_identity_as_username false + +# ----------------------------------------------------------------- +# Pre-shared-key based SSL/TLS support +# ----------------------------------------------------------------- +# The following options can be used to enable PSK based SSL/TLS support for +# this listener. Note that the recommended port for MQTT over TLS is 8883, but +# this must be set manually. +# +# See also the mosquitto-tls man page and the "Certificate based SSL/TLS +# support" section. Only one of certificate or PSK encryption support can be +# enabled for any listener. + +# The psk_hint option enables pre-shared-key support for this listener and also +# acts as an identifier for this listener. The hint is sent to clients and may +# be used locally to aid authentication. The hint is a free form string that +# doesn't have much meaning in itself, so feel free to be creative. +# If this option is provided, see psk_file to define the pre-shared keys to be +# used or create a security plugin to handle them. +#psk_hint + +# When using PSK, the encryption ciphers used will be chosen from the list of +# available PSK ciphers. If you want to control which ciphers are available, +# use the "ciphers" option. The list of available ciphers can be optained +# using the "openssl ciphers" command and should be provided in the same format +# as the output of that command. +#ciphers + +# Set use_identity_as_username to have the psk identity sent by the client used +# as its username. Authentication will be carried out using the PSK rather than +# the MQTT username/password and so password_file will not be used for this +# listener. +#use_identity_as_username false + + +# ================================================================= +# Persistence +# ================================================================= + +# If persistence is enabled, save the in-memory database to disk +# every autosave_interval seconds. If set to 0, the persistence +# database will only be written when mosquitto exits. See also +# autosave_on_changes. +# Note that writing of the persistence database can be forced by +# sending mosquitto a SIGUSR1 signal. +#autosave_interval 1800 + +# If true, mosquitto will count the number of subscription changes, retained +# messages received and queued messages and if the total exceeds +# autosave_interval then the in-memory database will be saved to disk. +# If false, mosquitto will save the in-memory database to disk by treating +# autosave_interval as a time in seconds. +#autosave_on_changes false + +# Save persistent message data to disk (true/false). +# This saves information about all messages, including +# subscriptions, currently in-flight messages and retained +# messages. +# retained_persistence is a synonym for this option. +persistence true + +# The filename to use for the persistent database, not including +# the path. +#persistence_file mosquitto.db + +# Location for persistent database. +# Default is an empty string (current directory). +# Set to e.g. /var/lib/mosquitto if running as a proper service on Linux or +# similar. +persistence_location /mosquitto/data + + +# ================================================================= +# Logging +# ================================================================= + +# Places to log to. Use multiple log_dest lines for multiple +# logging destinations. +# Possible destinations are: stdout stderr syslog topic file dlt +# +# stdout and stderr log to the console on the named output. +# +# syslog uses the userspace syslog facility which usually ends up +# in /var/log/messages or similar. +# +# topic logs to the broker topic '$SYS/broker/log/', +# where severity is one of D, E, W, N, I, M which are debug, error, +# warning, notice, information and message. Message type severity is used by +# the subscribe/unsubscribe log_types and publishes log messages to +# $SYS/broker/log/M/susbcribe or $SYS/broker/log/M/unsubscribe. +# +# The file destination requires an additional parameter which is the file to be +# logged to, e.g. "log_dest file /var/log/mosquitto.log". The file will be +# closed and reopened when the broker receives a HUP signal. Only a single file +# destination may be configured. +# +# The dlt destination is for the automotive `Diagnostic Log and Trace` tool. +# This requires that Mosquitto has been compiled with DLT support. +# +# Note that if the broker is running as a Windows service it will default to +# "log_dest none" and neither stdout nor stderr logging is available. +# Use "log_dest none" if you wish to disable logging. +# log_dest file /mosquitto/log/mosquitto.log + +# Types of messages to log. Use multiple log_type lines for logging +# multiple types of messages. +# Possible types are: debug, error, warning, notice, information, +# none, subscribe, unsubscribe, websockets, all. +# Note that debug type messages are for decoding the incoming/outgoing +# network packets. They are not logged in "topics". +#log_type error +#log_type warning +#log_type notice +#log_type information + + +# If set to true, client connection and disconnection messages will be included +# in the log. +#connection_messages true + +# If using syslog logging (not on Windows), messages will be logged to the +# "daemon" facility by default. Use the log_facility option to choose which of +# local0 to local7 to log to instead. The option value should be an integer +# value, e.g. "log_facility 5" to use local5. +#log_facility + +# If set to true, add a timestamp value to each log message. +#log_timestamp true + +# Set the format of the log timestamp. If left unset, this is the number of +# seconds since the Unix epoch. +# This is a free text string which will be passed to the strftime function. To +# get an ISO 8601 datetime, for example: +# log_timestamp_format %Y-%m-%dT%H:%M:%S +#log_timestamp_format + +# Change the websockets logging level. This is a global option, it is not +# possible to set per listener. This is an integer that is interpreted by +# libwebsockets as a bit mask for its lws_log_levels enum. See the +# libwebsockets documentation for more details. "log_type websockets" must also +# be enabled. +#websockets_log_level 0 + + +# ================================================================= +# Security +# ================================================================= + +# If set, only clients that have a matching prefix on their +# clientid will be allowed to connect to the broker. By default, +# all clients may connect. +# For example, setting "secure-" here would mean a client "secure- +# client" could connect but another with clientid "mqtt" couldn't. +#clientid_prefixes + +# Boolean value that determines whether clients that connect +# without providing a username are allowed to connect. If set to +# false then a password file should be created (see the +# password_file option) to control authenticated client access. +# +# Defaults to false, unless there are no listeners defined in the configuration +# file, in which case it is set to true, but connections are only allowed from +# the local machine. +# allow_anonymous false + +# ----------------------------------------------------------------- +# Default authentication and topic access control +# ----------------------------------------------------------------- + +# Control access to the broker using a password file. This file can be +# generated using the mosquitto_passwd utility. If TLS support is not compiled +# into mosquitto (it is recommended that TLS support should be included) then +# plain text passwords are used, in which case the file should be a text file +# with lines in the format: +# username:password +# The password (and colon) may be omitted if desired, although this +# offers very little in the way of security. +# +# See the TLS client require_certificate and use_identity_as_username options +# for alternative authentication options. If a plugin is used as well as +# password_file, the plugin check will be made first. +password_file /mosquitto/config/pwfile + +# Access may also be controlled using a pre-shared-key file. This requires +# TLS-PSK support and a listener configured to use it. The file should be text +# lines in the format: +# identity:key +# The key should be in hexadecimal format without a leading "0x". +# If an plugin is used as well, the plugin check will be made first. +#psk_file + +# Control access to topics on the broker using an access control list +# file. If this parameter is defined then only the topics listed will +# have access. +# If the first character of a line of the ACL file is a # it is treated as a +# comment. +# Topic access is added with lines of the format: +# +# topic [read|write|readwrite|deny] +# +# The access type is controlled using "read", "write", "readwrite" or "deny". +# This parameter is optional (unless contains a space character) - if +# not given then the access is read/write. can contain the + or # +# wildcards as in subscriptions. +# +# The "deny" option can used to explicity deny access to a topic that would +# otherwise be granted by a broader read/write/readwrite statement. Any "deny" +# topics are handled before topics that grant read/write access. +# +# The first set of topics are applied to anonymous clients, assuming +# allow_anonymous is true. User specific topic ACLs are added after a +# user line as follows: +# +user mosquitto +# +# The username referred to here is the same as in password_file. It is +# not the clientid. +# +# +# If is also possible to define ACLs based on pattern substitution within the +# topic. The patterns available for substition are: +# +# %c to match the client id of the client +# %u to match the username of the client +# +# The substitution pattern must be the only text for that level of hierarchy. +# +# The form is the same as for the topic keyword, but using pattern as the +# keyword. +# Pattern ACLs apply to all users even if the "user" keyword has previously +# been given. +# +# If using bridges with usernames and ACLs, connection messages can be allowed +# with the following pattern: +# pattern write $SYS/broker/connection/%c/state +# +# pattern [read|write|readwrite] +# +# Example: +# +# pattern write sensor/%u/data +# +# If an plugin is used as well as acl_file, the plugin check will be +# made first. +#acl_file + +# ----------------------------------------------------------------- +# External authentication and topic access plugin options +# ----------------------------------------------------------------- + +# External authentication and access control can be supported with the +# plugin option. This is a path to a loadable plugin. See also the +# plugin_opt_* options described below. +# +# The plugin option can be specified multiple times to load multiple +# plugins. The plugins will be processed in the order that they are specified +# here. If the plugin option is specified alongside either of +# password_file or acl_file then the plugin checks will be made first. +# +# If the per_listener_settings option is false, the plugin will be apply to all +# listeners. If per_listener_settings is true, then the plugin will apply to +# the current listener being defined only. +# +# This option is also available as `auth_plugin`, but this use is deprecated +# and will be removed in the future. +# +#plugin + +# If the plugin option above is used, define options to pass to the +# plugin here as described by the plugin instructions. All options named +# using the format plugin_opt_* will be passed to the plugin, for example: +# +# This option is also available as `auth_opt_*`, but this use is deprecated +# and will be removed in the future. +# +# plugin_opt_db_host +# plugin_opt_db_port +# plugin_opt_db_username +# plugin_opt_db_password + + +# ================================================================= +# Bridges +# ================================================================= + +# A bridge is a way of connecting multiple MQTT brokers together. +# Create a new bridge using the "connection" option as described below. Set +# options for the bridges using the remaining parameters. You must specify the +# address and at least one topic to subscribe to. +# +# Each connection must have a unique name. +# +# The address line may have multiple host address and ports specified. See +# below in the round_robin description for more details on bridge behaviour if +# multiple addresses are used. Note that if you use an IPv6 address, then you +# are required to specify a port. +# +# The direction that the topic will be shared can be chosen by +# specifying out, in or both, where the default value is out. +# The QoS level of the bridged communication can be specified with the next +# topic option. The default QoS level is 0, to change the QoS the topic +# direction must also be given. +# +# The local and remote prefix options allow a topic to be remapped when it is +# bridged to/from the remote broker. This provides the ability to place a topic +# tree in an appropriate location. +# +# For more details see the mosquitto.conf man page. +# +# Multiple topics can be specified per connection, but be careful +# not to create any loops. +# +# If you are using bridges with cleansession set to false (the default), then +# you may get unexpected behaviour from incoming topics if you change what +# topics you are subscribing to. This is because the remote broker keeps the +# subscription for the old topic. If you have this problem, connect your bridge +# with cleansession set to true, then reconnect with cleansession set to false +# as normal. +#connection +#address [:] [[:]] +#topic [[[out | in | both] qos-level] local-prefix remote-prefix] + +# If you need to have the bridge connect over a particular network interface, +# use bridge_bind_address to tell the bridge which local IP address the socket +# should bind to, e.g. `bridge_bind_address 192.168.1.10` +#bridge_bind_address + +# If a bridge has topics that have "out" direction, the default behaviour is to +# send an unsubscribe request to the remote broker on that topic. This means +# that changing a topic direction from "in" to "out" will not keep receiving +# incoming messages. Sending these unsubscribe requests is not always +# desirable, setting bridge_attempt_unsubscribe to false will disable sending +# the unsubscribe request. +#bridge_attempt_unsubscribe true + +# Set the version of the MQTT protocol to use with for this bridge. Can be one +# of mqttv50, mqttv311 or mqttv31. Defaults to mqttv311. +#bridge_protocol_version mqttv311 + +# Set the clean session variable for this bridge. +# When set to true, when the bridge disconnects for any reason, all +# messages and subscriptions will be cleaned up on the remote +# broker. Note that with cleansession set to true, there may be a +# significant amount of retained messages sent when the bridge +# reconnects after losing its connection. +# When set to false, the subscriptions and messages are kept on the +# remote broker, and delivered when the bridge reconnects. +#cleansession false + +# Set the amount of time a bridge using the lazy start type must be idle before +# it will be stopped. Defaults to 60 seconds. +#idle_timeout 60 + +# Set the keepalive interval for this bridge connection, in +# seconds. +#keepalive_interval 60 + +# Set the clientid to use on the local broker. If not defined, this defaults to +# 'local.'. If you are bridging a broker to itself, it is important +# that local_clientid and clientid do not match. +#local_clientid + +# If set to true, publish notification messages to the local and remote brokers +# giving information about the state of the bridge connection. Retained +# messages are published to the topic $SYS/broker/connection//state +# unless the notification_topic option is used. +# If the message is 1 then the connection is active, or 0 if the connection has +# failed. +# This uses the last will and testament feature. +#notifications true + +# Choose the topic on which notification messages for this bridge are +# published. If not set, messages are published on the topic +# $SYS/broker/connection//state +#notification_topic + +# Set the client id to use on the remote end of this bridge connection. If not +# defined, this defaults to 'name.hostname' where name is the connection name +# and hostname is the hostname of this computer. +# This replaces the old "clientid" option to avoid confusion. "clientid" +# remains valid for the time being. +#remote_clientid + +# Set the password to use when connecting to a broker that requires +# authentication. This option is only used if remote_username is also set. +# This replaces the old "password" option to avoid confusion. "password" +# remains valid for the time being. +#remote_password + +# Set the username to use when connecting to a broker that requires +# authentication. +# This replaces the old "username" option to avoid confusion. "username" +# remains valid for the time being. +#remote_username + +# Set the amount of time a bridge using the automatic start type will wait +# until attempting to reconnect. +# This option can be configured to use a constant delay time in seconds, or to +# use a backoff mechanism based on "Decorrelated Jitter", which adds a degree +# of randomness to when the restart occurs. +# +# Set a constant timeout of 20 seconds: +# restart_timeout 20 +# +# Set backoff with a base (start value) of 10 seconds and a cap (upper limit) of +# 60 seconds: +# restart_timeout 10 30 +# +# Defaults to jitter with a base of 5 and cap of 30 +#restart_timeout 5 30 + +# If the bridge has more than one address given in the address/addresses +# configuration, the round_robin option defines the behaviour of the bridge on +# a failure of the bridge connection. If round_robin is false, the default +# value, then the first address is treated as the main bridge connection. If +# the connection fails, the other secondary addresses will be attempted in +# turn. Whilst connected to a secondary bridge, the bridge will periodically +# attempt to reconnect to the main bridge until successful. +# If round_robin is true, then all addresses are treated as equals. If a +# connection fails, the next address will be tried and if successful will +# remain connected until it fails +#round_robin false + +# Set the start type of the bridge. This controls how the bridge starts and +# can be one of three types: automatic, lazy and once. Note that RSMB provides +# a fourth start type "manual" which isn't currently supported by mosquitto. +# +# "automatic" is the default start type and means that the bridge connection +# will be started automatically when the broker starts and also restarted +# after a short delay (30 seconds) if the connection fails. +# +# Bridges using the "lazy" start type will be started automatically when the +# number of queued messages exceeds the number set with the "threshold" +# parameter. It will be stopped automatically after the time set by the +# "idle_timeout" parameter. Use this start type if you wish the connection to +# only be active when it is needed. +# +# A bridge using the "once" start type will be started automatically when the +# broker starts but will not be restarted if the connection fails. +#start_type automatic + +# Set the number of messages that need to be queued for a bridge with lazy +# start type to be restarted. Defaults to 10 messages. +# Must be less than max_queued_messages. +#threshold 10 + +# If try_private is set to true, the bridge will attempt to indicate to the +# remote broker that it is a bridge not an ordinary client. If successful, this +# means that loop detection will be more effective and that retained messages +# will be propagated correctly. Not all brokers support this feature so it may +# be necessary to set try_private to false if your bridge does not connect +# properly. +#try_private true + +# Some MQTT brokers do not allow retained messages. MQTT v5 gives a mechanism +# for brokers to tell clients that they do not support retained messages, but +# this is not possible for MQTT v3.1.1 or v3.1. If you need to bridge to a +# v3.1.1 or v3.1 broker that does not support retained messages, set the +# bridge_outgoing_retain option to false. This will remove the retain bit on +# all outgoing messages to that bridge, regardless of any other setting. +#bridge_outgoing_retain true + +# If you wish to restrict the size of messages sent to a remote bridge, use the +# bridge_max_packet_size option. This sets the maximum number of bytes for +# the total message, including headers and payload. +# Note that MQTT v5 brokers may provide their own maximum-packet-size property. +# In this case, the smaller of the two limits will be used. +# Set to 0 for "unlimited". +#bridge_max_packet_size 0 + + +# ----------------------------------------------------------------- +# Certificate based SSL/TLS support +# ----------------------------------------------------------------- +# Either bridge_cafile or bridge_capath must be defined to enable TLS support +# for this bridge. +# bridge_cafile defines the path to a file containing the +# Certificate Authority certificates that have signed the remote broker +# certificate. +# bridge_capath defines a directory that will be searched for files containing +# the CA certificates. For bridge_capath to work correctly, the certificate +# files must have ".crt" as the file ending and you must run "openssl rehash +# " each time you add/remove a certificate. +#bridge_cafile +#bridge_capath + + +# If the remote broker has more than one protocol available on its port, e.g. +# MQTT and WebSockets, then use bridge_alpn to configure which protocol is +# requested. Note that WebSockets support for bridges is not yet available. +#bridge_alpn + +# When using certificate based encryption, bridge_insecure disables +# verification of the server hostname in the server certificate. This can be +# useful when testing initial server configurations, but makes it possible for +# a malicious third party to impersonate your server through DNS spoofing, for +# example. Use this option in testing only. If you need to resort to using this +# option in a production environment, your setup is at fault and there is no +# point using encryption. +#bridge_insecure false + +# Path to the PEM encoded client certificate, if required by the remote broker. +#bridge_certfile + +# Path to the PEM encoded client private key, if required by the remote broker. +#bridge_keyfile + +# ----------------------------------------------------------------- +# PSK based SSL/TLS support +# ----------------------------------------------------------------- +# Pre-shared-key encryption provides an alternative to certificate based +# encryption. A bridge can be configured to use PSK with the bridge_identity +# and bridge_psk options. These are the client PSK identity, and pre-shared-key +# in hexadecimal format with no "0x". Only one of certificate and PSK based +# encryption can be used on one +# bridge at once. +#bridge_identity +#bridge_psk + + +# ================================================================= +# External config files +# ================================================================= + +# External configuration files may be included by using the +# include_dir option. This defines a directory that will be searched +# for config files. All files that end in '.conf' will be loaded as +# a configuration file. It is best to have this as the last option +# in the main file. This option will only be processed from the main +# configuration file. The directory specified must not contain the +# main configuration file. +# Files within include_dir will be loaded sorted in case-sensitive +# alphabetical order, with capital letters ordered first. If this option is +# given multiple times, all of the files from the first instance will be +# processed before the next instance. See the man page for examples. +#include_dir diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/package.xml b/ground_control_station/ros_ws/src/ros2tak_tools/package.xml new file mode 100644 index 00000000..c45ad27f --- /dev/null +++ b/ground_control_station/ros_ws/src/ros2tak_tools/package.xml @@ -0,0 +1,25 @@ + + + + ros2tak_tools + 0.0.0 + TODO: Package description + mission-operator + TODO: License declaration + + rclpy + std_msgs + geometry_msgs + paho-mqtt + pytak + pyyaml + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/resource/ros2tak_tools b/ground_control_station/ros_ws/src/ros2tak_tools/resource/ros2tak_tools new file mode 100644 index 00000000..e69de29b diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/__init__.py b/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/cot2planner_agent.py b/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/cot2planner_agent.py new file mode 100644 index 00000000..a90b50cc --- /dev/null +++ b/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/cot2planner_agent.py @@ -0,0 +1,259 @@ +#!/usr/bin/env python3 + +""" +Description: +This script listens to the MQTT messages from the TAK server and converts them to ROS 2 messages. +Mainly used for Global Planner for getting Search Bounds, Search Priors, and Keep Out Zones. + +Usage: +ros2 run ros2tak_tools cot2planner_agent --config + +Author: +Aditya Rauniyar (2024) + +""" + +import rclpy +from rclpy.node import Node +import xml.etree.ElementTree as ET +import paho.mqtt.client as mqtt +from airstack_msgs.msg import SearchMissionRequest, SearchPrior, KeepOutZone # Import the custom message +from std_msgs.msg import Header +from geometry_msgs.msg import Polygon, Point32 +from rclpy.qos import QoSProfile +import yaml +from enum import Enum +import re + +# Constants and Enums +DEFAULT_FRAME_ID = 'map' + +# Define event types using Enum for better type safety +class EventType(Enum): + POLYGON = "u-d-f" + ROUTE = "b-m-r" + POINT = "u-d-c-p" + CIRCLE = "u-d-c-c" + +def extract_polygon_points(root): + """Extract points for the polygon based on link coordinates.""" + points32 = [] + + # Find all elements and extract their 'point' attribute + for link in root.findall('.//link'): + point_str = link.attrib.get('point') + # Check if point attribute exists and is non-empty. + # WinTAK sometimes generates blank points, ATAK is fine. + if point_str: + try: + # Split the single point into latitude and longitude + lat, lon = map(float, point_str.split(',')) + + # Create a Point32 object with lat and lon, set z=0.0 (altitude is not used here) + point = Point32() + point.x = lat + point.y = lon + point.z = 0.0 # Default z value (altitude not available) + + # Add the point to the list + points32.append(point) + except ValueError as e: + print(f"Warning: Skipping invalid point: {point_str} - Error: {e}") + + return points32 + +def extract_value_priority(callsign): + """Extract value and priority from a callsign like 'planner_sp_v0.2_p3.4'.""" + match = re.search(r'v([0-9.]+)_p([0-9.]+)', callsign) + if match: + value = float(match.group(1)) + priority = float(match.group(2)) + return value, priority + return 0.0, 1.0 # Default values if no match + + +def _process_polygons(root): + """Process XML for 'u-d-f' type and convert to SearchBound and SearchPrior.""" + points32 = extract_polygon_points(root) + # Create ROS message + polygon = Polygon() + + # Set the points for the polygon + polygon.points = points32 + + return polygon + + +class Cot2Planner(Node): + def __init__(self, config_file): + super().__init__('cot2planner') + + # Load configuration + with open(config_file, 'r') as file: + config = yaml.safe_load(file) + + # MQTT Configurations + mqtt_config = config['mqtt'] + self.mqtt_broker = mqtt_config['host'] + self.mqtt_port = int(mqtt_config['port']) + self.mqtt_username = mqtt_config['username'] + self.mqtt_password = mqtt_config['password'] + + # ROS Configurations + # Reading the ros_topic_name for the planner under filter_messages -> planner + planner_config = next( + item for item in config['services']['subscriber']['tak_subscriber']['filter_messages'] + if item['name'] == 'planner' + ) + self.ros_topic = planner_config['ros_topic_name'] # Read the ros_topic_name dynamically + self.mqtt_topicname = planner_config['mqtt_topic_name'] # Read the MQTT topic name for planner events + + # ROS 2 Publisher + qos_profile = QoSProfile(depth=10) + self.publisher = self.create_publisher(SearchMissionRequest, self.ros_topic, qos_profile) + + # Initialize MQTT Client + self.mqtt_client = mqtt.Client() + self.mqtt_client.username_pw_set(self.mqtt_username, self.mqtt_password) + self.mqtt_client.on_message = self._on_mqtt_message + + # Connect to MQTT broker and start loop + self.mqtt_client.connect(self.mqtt_broker, self.mqtt_port) + self.mqtt_client.subscribe(self.mqtt_topicname) # Subscribe to the dynamic planner topic + self.mqtt_client.loop_start() + + # Planner message request to be sent + self.plan_msg_request = self._initialize_ros_message_header(SearchMissionRequest(), frame_id=DEFAULT_FRAME_ID) + + self.get_logger().info(f"Subscribed to MQTT topic '{self.mqtt_topicname}' on broker '{self.mqtt_broker}:{self.mqtt_port}'") + + def _initialize_ros_message_header(self, message, frame_id=DEFAULT_FRAME_ID): + """Initialize the ROS message with header info.""" + message.header = Header() + message.header.stamp = self.get_clock().now().to_msg() + message.header.frame_id = frame_id + return message + + def _on_mqtt_message(self, client, userdata, msg): + """Callback for incoming MQTT messages.""" + try: + # Parse the XML message + root = ET.fromstring(msg.payload.decode('utf-8')) + + # Extract event type from the XML + event_type = root.attrib.get('type') + + # Extract the callsign from the XML + callsign = root.find('.//contact').attrib.get('callsign', '') + + # Handle event types using Enum values + if event_type == EventType.POLYGON.value or event_type == EventType.ROUTE.value or event_type == EventType.POINT.value: + polygon = _process_polygons(root) + + # Check if the callsign contains 'sb' for search bounds + if 'sb' in callsign.lower(): + self.plan_msg_request.search_bounds = polygon + self.get_logger().info(f"Added SearchBounds with callsign '{callsign}' to the SearchMissionRequest") + + event_shape_type = "polygon" if event_type == EventType.POLYGON.value else \ + "route" if event_type == EventType.ROUTE.value else "point" + + # Process as search prior + self._process_search_priors(polygon, call_sign=callsign, shape_type=event_shape_type) + + elif event_type == EventType.CIRCLE.value: + self._process_keep_out_zones(root) + self.get_logger().info(f"Added KeepOutZones with callsign '{callsign}' to the SearchMissionRequest") + else: + self.get_logger().info(f"Event type '{event_type}' not supported") + + # Print the current self.plan_msg_request + # self.get_logger().info(f"Current SearchMissionRequest: {self.plan_msg_request}") + + # Check if the xml contains word "end" in the remarks field mostly the remarks field is kept empty + remarks = root.find('.//remarks').text + if remarks and 'end' in remarks.lower(): + # Publish the SearchMissionRequest message + self.publisher.publish(self.plan_msg_request) + self.get_logger().info(f"Published SearchMissionRequest to '{self.ros_topic}'") + # Reset the plan_msg_request for next mission + self.plan_msg_request = self._initialize_ros_message_header(SearchMissionRequest(), frame_id=DEFAULT_FRAME_ID) + + except (ET.ParseError, mqtt.MQTTException) as e: + self.get_logger().error(f"Error processing MQTT message: {e}") + + def _process_search_priors(self, polygon, call_sign="", shape_type="polygon"): + """Process XML for 'b-m-r' type and convert to SearchBound and SearchPrior.""" + + # Extract the value and priority from the callsign + value, priority = extract_value_priority(call_sign) + + # Create a SearchPrior message + search_prior = self._initialize_ros_message_header(SearchPrior(), frame_id=DEFAULT_FRAME_ID) + + # Set the prior type as polygon + if shape_type == "polygon": + search_prior.grid_prior_type = SearchPrior.POLYGON_PRIOR + elif shape_type == "route": + search_prior.grid_prior_type = SearchPrior.LINE_SEG_PRIOR + elif shape_type == "point": + search_prior.grid_prior_type = SearchPrior.POINT_PRIOR + + # set polygon, value, and priority + search_prior.points_list = polygon + search_prior.value = [value] + search_prior.priority = [priority] + + # Update the plan_msg_request with the search_prior + self.plan_msg_request.search_priors.append(search_prior) + self.get_logger().info(f"Added SearchPriors to the SearchMissionRequest with callSign={call_sign}, " + f"type={shape_type}, value={value}, priority={priority}") + + + def _process_keep_out_zones(self, root): + """Process XML for 'u-d-c-c' type and convert to KeepOutZone.""" + # Extract data from XML + point = root.find('.//point') + if point is not None: + lat = float(point.attrib.get('lat', 0.0)) + lon = float(point.attrib.get('lon', 0.0)) + else: + lat = lon = 0.0 + + ellipse = root.find('.//ellipse') + major_ellipse = float(ellipse.attrib.get('major', 0.0)) if ellipse is not None else 0.0 + + # Create ROS message + message = self._initialize_ros_message_header(KeepOutZone(), frame_id=DEFAULT_FRAME_ID) + + # Set the x, y, z_min, z_max, and radius fields + message.x = lat + message.y = lon + message.z_min = 0.0 + message.z_max = 0.0 + message.radius = major_ellipse # Radius is the major ellipse axis in meters + + # Append the KeepOutZone message to the plan_msg_request + self.plan_msg_request.keep_out_zones.append(message) + + def destroy_node(self): + """Stop MQTT loop and destroy the node.""" + self.mqtt_client.loop_stop() + self.mqtt_client.disconnect() # Explicit disconnect + super().destroy_node() + +def main(args=None): + rclpy.init(args=args) + + import argparse + parser = argparse.ArgumentParser(description="COT to Planner") + parser.add_argument('--config', type=str, required=True, help='Path to the config YAML file.') + args = parser.parse_args() + + cot2planner = Cot2Planner(args.config) + rclpy.spin(cot2planner) + cot2planner.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/ros2cot_agent.py b/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/ros2cot_agent.py new file mode 100644 index 00000000..8eea6447 --- /dev/null +++ b/ground_control_station/ros_ws/src/ros2tak_tools/ros2tak_tools/ros2cot_agent.py @@ -0,0 +1,213 @@ +#!/usr/bin/env python3 + +""" +ROS 2 GPS to CoT Event Publisher + +Author: Aditya Rauniyar (rauniyar@cmu.edu) + +This script acts as a ROS 2 node that subscribes to GPS data from multiple robots +and converts that data into Cursor-On-Target (CoT) events. The CoT events are then sent +over a TCP socket to a designated server. The configuration for the host and port, as +well as the number of robots to subscribe to, is loaded from a YAML configuration file. + +Usage: + 1. Ensure you have Python 3.x installed with the necessary packages: + pip install rclpy sensor_msgs pytak pyyaml + + 2. Create a YAML configuration file (e.g., config.yaml) with the following structure: + project: + name: test + robot: + count: 3 + gps_topicname: '/iphone{n}/gps' # Pattern for topic names + tak_server: + cot_url: {Enter the URL of the CoT server} + pytak_tls_client_cert: '/path/to/cert.pem' + pytak_tls_client_key: '/path/to/key.key' + services: + host: '127.0.0.1' + publisher: + tak_publisher: + port: 10000 + mediator: + ros2cot_agent: + port: 10000 + + 3. Run the script with the following command, specifying the path to the config file: + python your_script.py --config config.yaml + + 4. The script will listen for incoming GPS messages and send CoT events to the + configured server. + +Note: + Ensure the server receiving the CoT events is running and listening on the specified port. +""" + +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import NavSatFix +import paho.mqtt.client as mqtt +import pytak +import xml.etree.ElementTree as ET +import argparse +import socket +import yaml + + +def load_config(file_path): + """Load configuration from a YAML file.""" + with open(file_path, "r") as f: + return yaml.safe_load(f) + + +def create_COT_pos_event(uuid, latitude, longitude, altitude): + """Create a CoT event based on the GPS data.""" + root = ET.Element("event") + root.set("version", "2.0") + root.set("type", "a-h-A-M-A") + root.set("uid", uuid) # Use topic name as UID for identification + root.set("how", "m-g") + root.set("time", pytak.cot_time()) + root.set("start", pytak.cot_time()) + root.set("stale", pytak.cot_time(60)) + + pt_attr = { + "lat": str(latitude), + "lon": str(longitude), + "hae": str(altitude), + "ce": "10", + "le": "10", + } + + ET.SubElement(root, "point", attrib=pt_attr) + + return ET.tostring(root, encoding="utf-8") + + +class ROS2COTPublisher(Node): + def __init__(self, config): + super().__init__("ros2cot_publisher") + self.subscribers = [] + + # Get host and port from the config + self.host = config["services"]["host"] + self.robots_count = config["robot"]["count"] + self.gps_topicname_pattern = config["robot"]["gps_topicname"] + self.project_name = config["project"]["name"] + + # MQTT related configs + self.mqtt_broker = config["mqtt"]["host"] + self.mqtt_port = config["mqtt"]["port"] + self.mqtt_username = config["mqtt"]["username"] + self.mqtt_pwd = config["mqtt"]["password"] + self.mqtt_topicname = config["services"]["mediator"]["ros2cot_agent"][ + "topic_name" + ] + + # Setting MQTT + self.mqtt_client = mqtt.Client() + # Set the username and password + self.mqtt_client.username_pw_set(self.mqtt_username, self.mqtt_pwd) + try: + print(f"Trying to connect to {self.mqtt_broker}:{self.mqtt_port}") + self.mqtt_client.connect(self.mqtt_broker, self.mqtt_port) + self.get_logger().info( + f"Connected to MQTT ({self.mqtt_broker}:{self.mqtt_port})" + ) + except Exception as e: + print(f"Failed to connect or publish: {e}") + + self.get_logger().info( + f"Starting ROS2COTPublisher for project: {self.project_name}" + ) + self.get_logger().info( + f"Subscribing to {self.robots_count} robot(s) on topics matching: {self.gps_topicname_pattern}" + ) + + # Subscribe to GPS topics based on the pattern + for i in range(1, self.robots_count + 1): + topic_name = self.gps_topicname_pattern.format(n=i) + subscriber = self.create_subscription( + NavSatFix, + topic_name, + lambda msg, topic_name=topic_name: self.gps_callback(msg, topic_name), + 10, # QoS depth + ) + self.subscribers.append(subscriber) + self.get_logger().info(f"Subscribed to GPS topic: {topic_name}") + + def gps_callback(self, msg, topic_name): + """Callback for processing GPS data.""" + latitude = msg.latitude + longitude = msg.longitude + altitude = msg.altitude + + # Log the received GPS data + self.get_logger().info( + f"Received GPS from {topic_name}: Lat {latitude}, Lon {longitude}, Alt {altitude}" + ) + + # Create a CoT event from the GPS data + cot_event = create_COT_pos_event( + f"{self.project_name}{topic_name}", latitude, longitude, altitude + ) + + # Send the CoT event over MQTT + self.send_cot_event_over_mqtt(cot_event) + + def send_cot_event_over_mqtt(self, cot_event): + """Send CoT event over the MQTT network""" + try: + self.mqtt_client.publish(self.mqtt_topicname, cot_event) + self.get_logger().info( + f"Message '{cot_event}' published to topic '{self.mqtt_topicname}'" + ) + except: + self.get_logger().error(f"Failed to publish.") + + def send_cot_event_over_network(self, cot_event): + """Send CoT event over a TCP socket to the configured host.""" + try: + with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s: + s.bind((self.host, self.port)) + s.connect((self.host, self.port)) + s.sendall(cot_event) + self.get_logger().info(f"Sent CoT event to {self.host}:{self.port}") + except ConnectionRefusedError: + self.get_logger().error( + f"Connection to {self.host}:{self.port} refused. Ensure the server is running." + ) + except Exception as e: + self.get_logger().error(f"Failed to send CoT event: {e}") + + +def main(args=None): + # Initialize ROS 2 Python client library + rclpy.init(args=args) + + # Use argparse to handle command line arguments + parser = argparse.ArgumentParser(description="ROS to CoT Publisher Node") + parser.add_argument( + "--config", type=str, required=True, help="Path to the config YAML file." + ) + + # Parse the arguments + input_args = parser.parse_args() + + # Load configuration + config = load_config(input_args.config) + + # Create an instance of the ROS2COTPublisher node with the provided configuration + gps_cot_publisher = ROS2COTPublisher(config) + + # Keep the node running to listen to incoming messages + rclpy.spin(gps_cot_publisher) + + # Shutdown and cleanup + gps_cot_publisher.destroy_node() + rclpy.shutdown() + print("Node has shut down cleanly.") + + +if __name__ == "__main__": + main() diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/scripts/tak_publisher.py b/ground_control_station/ros_ws/src/ros2tak_tools/scripts/tak_publisher.py new file mode 100644 index 00000000..bf9a74e5 --- /dev/null +++ b/ground_control_station/ros_ws/src/ros2tak_tools/scripts/tak_publisher.py @@ -0,0 +1,143 @@ +#!/usr/bin/env python3 + +""" +TAK Publisher Script + +Author: Aditya Rauniyar (rauniyar@cmu.edu) + +This script sends Cursor-On-Target (CoT) events to a TAK server by reading COT messages +from a specified localhost TCP socket. The configuration for the TAK server and other +parameters is loaded from a YAML file provided as a command-line argument. + +Usage: + 1. Ensure you have Python 3.x installed with the necessary packages: + pip install asyncio pytak pyyaml + + 2. Create a YAML configuration file (e.g., config.yaml) with the necessary parameters. + + 3. Run the script with the following command: + python your_script.py --config path/to/config.yaml +""" + +import asyncio +import xml.etree.ElementTree as ET +import pytak +import socket +import multiprocessing +import argparse +import yaml +from configparser import ConfigParser +import paho.mqtt.client as mqtt + +class MySender(pytak.QueueWorker): + def __init__(self, tx_queue, config, event_loop): + super().__init__(tx_queue, config["mycottool"]) + + # MQTT parameters + self.mqtt_broker = config['mqtt']['host'] + self.mqtt_port = int(config['mqtt']['port']) + self.mqtt_username = config['mqtt']['username'] + self.mqtt_pwd = config['mqtt']['password'] + self.mqtt_topicname = config['mqtt']['topicname'] + + # Capture the main event loop + self.event_loop = event_loop + + # Set up MQTT client + self.mqtt_client = mqtt.Client() + self.mqtt_client.username_pw_set(self.mqtt_username, self.mqtt_pwd) + self.mqtt_client.on_message = self._on_message_sync # Use sync wrapper + + # Connect to MQTT broker and subscribe to topic + try: + print(f"Connecting to {self.mqtt_broker}:{self.mqtt_port}") + self.mqtt_client.connect(self.mqtt_broker, self.mqtt_port) + self.mqtt_client.subscribe(self.mqtt_topicname) + print(f"Connected and subscribed to MQTT topic '{self.mqtt_topicname}' on broker {self.mqtt_broker}:{self.mqtt_port}") + except Exception as e: + print(f"Failed to connect or subscribe to MQTT: {e}") + + def start_mqtt_loop(self): + """Start MQTT loop in a separate thread.""" + self.mqtt_client.loop_start() + + def _on_message_sync(self, client, userdata, message): + """Synchronous wrapper for MQTT on_message to run handle_data in the main event loop.""" + asyncio.run_coroutine_threadsafe(self.handle_data(client, userdata, message), self.event_loop) + + async def handle_data(self, client, userdata, message): + """Handle incoming MQTT data and put it on the async queue.""" + event = message.payload + await self.put_queue(event) + print(f"Received message on '{self.mqtt_topicname}'") + + async def run(self, number_of_iterations=-1): + self.start_mqtt_loop() + try: + while True: + await asyncio.sleep(0.5) # Keep the loop running + except asyncio.CancelledError: + self.mqtt_client.loop_stop() + print("MQTT loop stopped.") + +async def main(config): + loop = asyncio.get_running_loop() # Capture the main event loop + clitool = pytak.CLITool(config["mycottool"]) + await clitool.setup() + clitool.add_task(MySender(clitool.tx_queue, config, loop)) # Pass the loop + await clitool.run() + +def run_main_in_process(config): + loop = asyncio.get_event_loop() + loop.run_until_complete(main(config)) + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description="TAK Publisher Script") + parser.add_argument('--config', type=str, required=True, help='Path to the config YAML file.') + args = parser.parse_args() + + # Load the YAML configuration + with open(args.config, 'r') as file: + print(f"Loading configuration from {args.config}") + config_data = yaml.safe_load(file) + + # Extract necessary parameters + cot_url = config_data['tak_server']['cot_url'] + pytak_tls_client_cert = config_data['tak_server']['pytak_tls_client_cert'] + pytak_tls_client_key = config_data['tak_server']['pytak_tls_client_key'] + host = config_data['services']['host'] + + # MQTT params + mqtt_broker = config_data['mqtt']['host'] + mqtt_port = config_data['mqtt']['port'] + mqtt_username = config_data['mqtt']['username'] + mqtt_pwd = config_data['mqtt']['password'] + mqtt_topicname = config_data['services']['publisher']['tak_publisher']['topic_name'] + + # Setup config for pytak + config = ConfigParser() + config["mycottool"] = { + "COT_URL": cot_url, + "PYTAK_TLS_CLIENT_CERT": pytak_tls_client_cert, + "PYTAK_TLS_CLIENT_KEY": pytak_tls_client_key, + "PYTAK_TLS_CLIENT_PASSWORD": "atakatak", + "PYTAK_TLS_DONT_VERIFY": "1" + } + config["service"] = { + "host": host, + } + + config["mqtt"] = { + "host": mqtt_broker, + "port": mqtt_port, + "username": mqtt_username, + "password": mqtt_pwd, + "topicname": mqtt_topicname + } + + # Start the asyncio event loop in a separate process + process = multiprocessing.Process(target=run_main_in_process, args=(config,)) + process.start() + + print("Main() is now running in a separate process.") + process.join() \ No newline at end of file diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/scripts/tak_subscriber.py b/ground_control_station/ros_ws/src/ros2tak_tools/scripts/tak_subscriber.py new file mode 100644 index 00000000..004e60ad --- /dev/null +++ b/ground_control_station/ros_ws/src/ros2tak_tools/scripts/tak_subscriber.py @@ -0,0 +1,202 @@ +#!/usr/bin/env python3 + +""" +COT Subscriber from TAK Server + +Author: Aditya Rauniyar (rauniyar@cmu.edu) + +Description: +This script acts as a receiver for Cursor-On-Target (CoT) messages. It handles incoming CoT data from a TAK server +and processes it according to the defined logic. The received messages are then sent to a specified +host IP and port defined in the configuration file. + +Usage: +1. Ensure you have Python 3.x installed with the necessary packages: + pip install pytak + +2. Create a configuration file with the parameters specified. + +3. Run the script: + python your_script.py --config path/to/config.yaml +""" + +import asyncio +import xml.etree.ElementTree as ET +import pytak +import argparse +import socket +import yaml +from configparser import ConfigParser +import logging +import paho.mqtt.client as mqtt + +# Log levels: DEBUG, INFO, WARNING, ERROR, CRITICAL +LOG_LEVEL = "DEBUG" + + + +def load_config(file_path): + """Load configuration from a YAML file.""" + with open(file_path, 'r') as f: + return yaml.safe_load(f) + + +class MyReceiver(pytak.QueueWorker): + """Defines how you will handle events from RX Queue.""" + + def __init__(self, rx_queue, config, filter_names2topic): + super().__init__(rx_queue, config["mycottool"]) + self.host = config["service"]["host"] + + # MQTT parameters + self.mqtt_broker = config['mqtt']['host'] + self.mqtt_port = int(config['mqtt']['port']) + self.mqtt_username = config['mqtt']['username'] + self.mqtt_pwd = config['mqtt']['password'] + + self._logger.info(f"Sending data to {self.host}:{self.mqtt_port}") + self.filter_names2topic = filter_names2topic + self.total_filters = len(self.filter_names2topic) + print(f"Filter messages number: {self.total_filters}") + + # Set up logging config to print debug message + self._logger.setLevel(logging.DEBUG) + + # Set up MQTT client + self.mqtt_client = mqtt.Client() + self.mqtt_client.username_pw_set(self.mqtt_username, self.mqtt_pwd) + + # Connect to MQTT broker and subscribe to topic + try: + self._logger.info(f"Connecting to {self.mqtt_broker}:{self.mqtt_port}") + self.mqtt_client.connect(self.mqtt_broker, self.mqtt_port) + self._logger.info(f"Connected and subscribed to MQTT on broker {self.mqtt_broker}:{self.mqtt_port}") + except Exception as e: + self._logger.error(f"Failed to connect or subscribe to MQTT: {e}") + + + async def handle_data(self, data): + """Handle data from the receive queue.""" + self._logger.debug("Received:\n%s\n", data.decode()) + + # Parse the CoT message to extract necessary fields + try: + root = ET.fromstring(data.decode()) + uuid = root.get("uid") + + self._logger.info(f"Recevied Message: {data}") + mqtt_topic = self.should_send_message(root) + + # Add your filter conditions here + if mqtt_topic: + # Send received data to the specified host and port + # self._logger.info("Sending data to %s:%s", self.host, self.port) + await self.send_to_mqtt(data, mqtt_topic=mqtt_topic) + else: + # self._logger.info("Filtered out message with UID: %s", uuid) + self._logger.debug(ET.tostring(root, encoding='unicode', method='xml')) + + except ET.ParseError as e: + # self._logger.error("Failed to parse CoT message: %s", e) + pass + + def should_send_message(self, root): + """Determine whether to send the message based on filtering criteria.""" + # Iterate over the filter messages and check if their name exists in the XML + for ii in range(self.total_filters): + filter_name = self.filter_names2topic[ii]["name"] + # Check if the filter name exists anywhere in the root element + if self.is_message_relevant(root, filter_name): + mqtt_topic = self.filter_names2topic[ii]["mqtt_topic"] + return mqtt_topic + + return None + + def is_message_relevant(self, root, filter_name): + """Helper function to check if a filter name exists in the XML string.""" + # Convert the XML to a string + xml_string = ET.tostring(root, encoding='unicode', method='xml') + + self._logger.info(f"Checking with filter: {filter_name}") + + # Check if the filter name exists in the XML string + if filter_name in xml_string: + self._logger.debug("Found filter match in XML string: %s", filter_name) + return True + + return False + + async def send_to_mqtt(self, data, mqtt_topic): + """Send CoT event over the MQTT network""" + try: + self.mqtt_client.publish(mqtt_topic, data) + self._logger.info(f"Message published to topic {mqtt_topic}") + self._logger.debug(f"Message: '{data}'") + except: + self._logger.info(f"Failed to publish.") + + + async def run(self): # pylint: disable=arguments-differ + """Read from the receive queue, put data onto handler.""" + while True: + data = await self.queue.get() # Get received CoT from rx_queue + await self.handle_data(data) + + +async def main(): + parser = argparse.ArgumentParser(description="TAK Subscriber Script") + parser.add_argument('--config', type=str, required=True, help='Path to the config YAML file.') + args = parser.parse_args() + + # Load the YAML configuration + with open(args.config, 'r') as file: + config_data = yaml.safe_load(file) + + # Extract necessary parameters from the configuration + cot_url = config_data['tak_server']['cot_url'] + pytak_tls_client_cert = config_data['tak_server']['pytak_tls_client_cert'] + pytak_tls_client_key = config_data['tak_server']['pytak_tls_client_key'] + host = config_data['services']['host'] + filter_messages = config_data['services']['subscriber']['tak_subscriber']['filter_messages'] + # Extract the filter name and corresponding mqtt topic_name + message_name2topic = [{"name": msg['name'], "mqtt_topic": msg["mqtt_topic_name"]} for msg in filter_messages] + + # MQTT params + mqtt_broker = config_data['mqtt']['host'] + # mqtt_broker = "localhost" # Uncomment if running from the host + mqtt_port = config_data['mqtt']['port'] + mqtt_username = config_data['mqtt']['username'] + mqtt_pwd = config_data['mqtt']['password'] + + # Setup config for pytak + config = ConfigParser() + config["mycottool"] = { + "COT_URL": cot_url, + "PYTAK_TLS_CLIENT_CERT": pytak_tls_client_cert, + "PYTAK_TLS_CLIENT_KEY": pytak_tls_client_key, + "PYTAK_TLS_CLIENT_PASSWORD": "atakatak", + "PYTAK_TLS_DONT_VERIFY": "1" + } + + config["service"] = { + "host": host + } + + config["mqtt"] = { + "host": mqtt_broker, + "port": mqtt_port, + "username": mqtt_username, + "password": mqtt_pwd + } + + # Initialize worker queues and tasks. + clitool = pytak.CLITool(config["mycottool"]) + await clitool.setup() + + clitool.add_task(MyReceiver(clitool.rx_queue, config, message_name2topic)) + # Start all tasks. + await clitool.run() + + +if __name__ == "__main__": + asyncio.run(main()) diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/setup.py b/ground_control_station/ros_ws/src/ros2tak_tools/setup.py new file mode 100644 index 00000000..ba6900a2 --- /dev/null +++ b/ground_control_station/ros_ws/src/ros2tak_tools/setup.py @@ -0,0 +1,28 @@ +from setuptools import find_packages, setup + +package_name = 'ros2tak_tools' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='mission-operator', + maintainer_email='mission-operator@todo.todo', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'ros2cot_agent = ros2tak_tools.ros2cot_agent:main', + 'cot2ros_agent = ros2tak_tools.cot2ros_agent:main', + 'cot2planner_agent = ros2tak_tools.cot2planner_agent:main', + ], + }, +) diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/test/test_copyright.py b/ground_control_station/ros_ws/src/ros2tak_tools/test/test_copyright.py new file mode 100644 index 00000000..97a39196 --- /dev/null +++ b/ground_control_station/ros_ws/src/ros2tak_tools/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/test/test_flake8.py b/ground_control_station/ros_ws/src/ros2tak_tools/test/test_flake8.py new file mode 100644 index 00000000..27ee1078 --- /dev/null +++ b/ground_control_station/ros_ws/src/ros2tak_tools/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/ground_control_station/ros_ws/src/ros2tak_tools/test/test_pep257.py b/ground_control_station/ros_ws/src/ros2tak_tools/test/test_pep257.py new file mode 100644 index 00000000..b234a384 --- /dev/null +++ b/ground_control_station/ros_ws/src/ros2tak_tools/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/index.html b/index.html new file mode 100644 index 00000000..206ea238 --- /dev/null +++ b/index.html @@ -0,0 +1,2608 @@ + + + + + + + + + + + + + + + + + + + AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

AirStack Boilerplate

+

Welcome to the AirStack Boilerplate! This repository template serves to kickstart the development of your own robotics autonomy stack. You're encouraged to customize your version in any way to best suit your project's needs.

+

This boilerplate is maintained by the AirLab at Carnegie Mellon University's Robotics Institute.

+

Please head to our Getting Started page to start.

+

Overview +AirStack

+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ +
+ + + +
+
+
+ +
+ Copyright © 2024 - 2030 AirLab CMU +
+ + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + +
+ +
+
+
+ +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/robot/docker/Dockerfile.airstack-dev b/robot/docker/Dockerfile.airstack-dev new file mode 100644 index 00000000..982da2c4 --- /dev/null +++ b/robot/docker/Dockerfile.airstack-dev @@ -0,0 +1,77 @@ +FROM osrf/ros:humble-desktop-full + +WORKDIR /root/ros_ws + +RUN apt update +# Install dev tools +RUN apt install -y \ + vim nano wget curl tree \ + cmake build-essential \ + less htop jq \ + python3-pip \ + tmux \ + gdb + +# Install any additional ROS2 packages +RUN apt update -y && apt install -y \ + ros-dev-tools \ + ros-humble-mavros \ + ros-humble-tf2* \ + ros-humble-stereo-image-proc \ + ros-humble-image-view \ + ros-humble-topic-tools \ + ros-humble-grid-map \ + ros-humble-domain-bridge \ + libcgal-dev + +RUN /opt/ros/humble/lib/mavros/install_geographiclib_datasets.sh + + +# Install Python dependencies +RUN pip3 install \ + empy \ + future \ + lxml \ + matplotlib \ + numpy \ + pkgconfig \ + psutil \ + pygments \ + wheel \ + pymavlink \ + pyyaml \ + requests \ + setuptools \ + six \ + toml \ + scipy + + +# Override install newer openvdb 8.2.0 for compatibility with Ubuntu 22.04 https://bugs.launchpad.net/bugs/1970108 +RUN apt remove -y libopenvdb*; \ + git clone --recurse --branch v8.2.0-debian https://github.com/wyca-robotics/openvdb.git /opt/openvdb && \ + mkdir /opt/openvdb/build && cd /opt/openvdb/build && \ + cmake .. && \ + make -j8 && make install && \ + cd ..; rm -rf /opt/openvdb/build + +# Add ability to SSH +RUN apt-get update && apt-get install -y openssh-server +RUN mkdir /var/run/sshd + +# Password is airstack +RUN echo 'root:airstack' | chpasswd +RUN sed -i 's/#PermitRootLogin prohibit-password/PermitRootLogin yes/' /etc/ssh/sshd_config +RUN sed -i 's/#PasswordAuthentication yes/PasswordAuthentication yes/' /etc/ssh/sshd_config +RUN sed 's@session\s*required\s*pam_loginuid.so@session optional pam_loginuid.so@g' -i /etc/pam.d/sshd + +EXPOSE 22 + + + +# Cleanup. Prevent people accidentally doing git commits as root in Docker +RUN apt purge git -y \ + && apt autoremove -y \ + && apt clean -y \ + && rm -rf /var/lib/apt/lists/* + diff --git a/robot/docker/custom_rosdep.yaml b/robot/docker/custom_rosdep.yaml new file mode 100644 index 00000000..27453029 --- /dev/null +++ b/robot/docker/custom_rosdep.yaml @@ -0,0 +1,2 @@ +# ignore openvdb because on ubuntu 22, it builds openvdb8.2.1 which is incompatible with tbb2021.5. we custom build instead +openvdb: {ubuntu: []} diff --git a/robot/docker/docker-compose.yaml b/robot/docker/docker-compose.yaml new file mode 100644 index 00000000..e45ab63a --- /dev/null +++ b/robot/docker/docker-compose.yaml @@ -0,0 +1,59 @@ +# docker compose file +services: +# ============================================================================== + robot: + image: &airstack-dev-image airlab-storage.andrew.cmu.edu:5001/shared/airstack-dev:v0.5.5 + build: + dockerfile: ./Dockerfile.airstack-dev + tags: + - *airstack-dev-image + entrypoint: "" + # we use tmux send-keys so that the session stays alive + command: > + bash -c "ssh service restart; + tmux new -d -s robot_bringup + && tmux send-keys -t robot_bringup + 'if [ ! -d "ros_ws/install" ]; then bws && sws; fi; + ros2 launch robot_bringup robot.launch.xml' ENTER + && sleep infinity" + # Interactive shell + stdin_open: true # docker run -i + tty: true # docker run -t + # Needed to display graphical applications + privileged: true + networks: + - airstack_network + environment: + - DISPLAY + - QT_X11_NO_MITSHM=1 + deploy: + # let it use the GPU + resources: + reservations: + devices: + - driver: nvidia # https://stackoverflow.com/a/70761193 + count: 1 + capabilities: [ gpu ] + ports: + # for ssh, starting from 2223-2243 on the host port all map to 22 in the container. Assumes no more than 21 robots + - "2223-2243:22" + volumes: + # display stuff + - $HOME/.Xauthority:/root/.Xauthority + - /tmp/.X11-unix:/tmp/.X11-unix + # developer stuff + - .dev:/root/.dev:rw # developer config + - .bashrc:/root/.bashrc:rw # bash config + - /var/run/docker.sock:/var/run/docker.sock # access docker API for container name + # autonomy stack stuff + - ../../common/ros_packages:/root/ros_ws/src/common:rw # common ROS packages + - ../../common/ros_packages/fastdds.xml:/root/ros_ws/fastdds.xml:rw # fastdds.xml + - ../ros_ws:/root/ros_ws:rw # robot-specific ROS packages +# ============================================================================== +networks: + airstack_network: + driver: bridge + ipam: + driver: default + config: + - subnet: 172.31.0.0/24 diff --git a/robot/installation/index.html b/robot/installation/index.html new file mode 100644 index 00000000..cd485286 --- /dev/null +++ b/robot/installation/index.html @@ -0,0 +1,2602 @@ + + + + + + + + + + + + + + + + + + + Index - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

Index

+ +

Scripts to install on machine, todo. +Maybe something with ansible? +Bash scripts could work too.

+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ +
+ + + +
+
+
+ +
+ Copyright © 2024 - 2030 AirLab CMU +
+ + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + +
+ +
+
+
+ +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/robot/ros_ws/fastdds.xml b/robot/ros_ws/fastdds.xml new file mode 100644 index 00000000..e69de29b diff --git a/robot/ros_ws/src/autonomy/0_interface/interface_bringup/CMakeLists.txt b/robot/ros_ws/src/autonomy/0_interface/interface_bringup/CMakeLists.txt new file mode 100644 index 00000000..d93f3f0d --- /dev/null +++ b/robot/ros_ws/src/autonomy/0_interface/interface_bringup/CMakeLists.txt @@ -0,0 +1,32 @@ +cmake_minimum_required(VERSION 3.8) +project(interface_bringup) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +# Install files. +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +# install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME}) +# install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) +# install(DIRECTORY params DESTINATION share/${PROJECT_NAME}) + +ament_package() diff --git a/robot/ros_ws/src/autonomy/0_interface/interface_bringup/LICENSE b/robot/ros_ws/src/autonomy/0_interface/interface_bringup/LICENSE new file mode 100644 index 00000000..d6456956 --- /dev/null +++ b/robot/ros_ws/src/autonomy/0_interface/interface_bringup/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/robot/ros_ws/src/autonomy/0_interface/interface_bringup/launch/interface.launch.xml b/robot/ros_ws/src/autonomy/0_interface/interface_bringup/launch/interface.launch.xml new file mode 100644 index 00000000..d82ec42c --- /dev/null +++ b/robot/ros_ws/src/autonomy/0_interface/interface_bringup/launch/interface.launch.xml @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/robot/ros_ws/src/autonomy/0_interface/interface_bringup/package.xml b/robot/ros_ws/src/autonomy/0_interface/interface_bringup/package.xml new file mode 100644 index 00000000..415a2be5 --- /dev/null +++ b/robot/ros_ws/src/autonomy/0_interface/interface_bringup/package.xml @@ -0,0 +1,18 @@ + + + + interface_bringup + 0.0.0 + TODO: Package description + andrew + Apache-2.0 + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/robot/ros_ws/src/autonomy/0_interface/mavros_interface/CMakeLists.txt b/robot/ros_ws/src/autonomy/0_interface/mavros_interface/CMakeLists.txt new file mode 100644 index 00000000..91bf2a3a --- /dev/null +++ b/robot/ros_ws/src/autonomy/0_interface/mavros_interface/CMakeLists.txt @@ -0,0 +1,76 @@ +cmake_minimum_required(VERSION 3.8) +project(mavros_interface) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) +find_package(robot_interface REQUIRED) +find_package(mavros_msgs REQUIRED) +find_package(pluginlib REQUIRED) +find_package(tf2 REQUIRED) +find_package(airstack_common REQUIRED) + +add_library(mavros_interface src/mavros_interface.cpp) +target_compile_features(mavros_interface PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +target_include_directories(mavros_interface PUBLIC + $ + $) + +ament_target_dependencies( + mavros_interface + robot_interface + mavros_msgs + pluginlib + tf2 + airstack_common +) + +pluginlib_export_plugin_description_file(robot_interface plugins.xml) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(mavros_interface PRIVATE "mavros_INTERFACE_BUILDING_LIBRARY") + +install( + DIRECTORY include/ + DESTINATION include +) +install( + TARGETS mavros_interface + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(PROGRAMS scripts/position_setpoint_pub.py DESTINATION lib/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_include_directories( + include +) +ament_export_libraries( + mavros_interface +) +ament_export_targets( + export_${PROJECT_NAME} +) + +ament_package() diff --git a/robot/ros_ws/src/autonomy/0_interface/mavros_interface/package.xml b/robot/ros_ws/src/autonomy/0_interface/mavros_interface/package.xml new file mode 100644 index 00000000..e7975ce0 --- /dev/null +++ b/robot/ros_ws/src/autonomy/0_interface/mavros_interface/package.xml @@ -0,0 +1,27 @@ + + + + mavros_interface + 0.0.0 + TODO: Package description + uav + TODO: License declaration + + ament_cmake_ros + + robot_interface + mavros + mavros_msgs + vision_msgs + ackerman_msgs + pluginlib + tf2 + airstack_common + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/robot/ros_ws/src/autonomy/0_interface/mavros_interface/plugins.xml b/robot/ros_ws/src/autonomy/0_interface/mavros_interface/plugins.xml new file mode 100644 index 00000000..eb30be9b --- /dev/null +++ b/robot/ros_ws/src/autonomy/0_interface/mavros_interface/plugins.xml @@ -0,0 +1,5 @@ + + + mavros plugin for the robot interface. + + diff --git a/robot/ros_ws/src/autonomy/0_interface/mavros_interface/scripts/position_setpoint_pub.py b/robot/ros_ws/src/autonomy/0_interface/mavros_interface/scripts/position_setpoint_pub.py new file mode 100644 index 00000000..a47e2354 --- /dev/null +++ b/robot/ros_ws/src/autonomy/0_interface/mavros_interface/scripts/position_setpoint_pub.py @@ -0,0 +1,34 @@ +#!/usr/bin/python3 +import os + +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy +from airstack_msgs.msg import Odometry +from geometry_msgs.msg import PoseStamped + +class OdomModifier(Node): + def __init__(self): + super().__init__('odom_modifier') + ''' + qos = QoSProfile( + reliability=QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, + history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST, + depth=1) + ''' + self.odom_subscriber = self.create_subscription(Odometry, "/" + os.getenv('ROBOT_NAME', "") + '/trajectory_controller/tracking_point', self.odom_callback, 1) + self.odom_publisher = self.create_publisher(PoseStamped, 'cmd_pose', 1) + + def odom_callback(self, msg): + out = PoseStamped() + out.header = msg.header + out.header.frame_id = 'base_link' + out.pose = msg.pose + self.odom_publisher.publish(out) + +if __name__ == '__main__': + rclpy.init(args=None) + odom_modifier_node = OdomModifier() + rclpy.spin(odom_modifier_node) + odom_modifier_node.destroy_node() + rclpy.shutdown() diff --git a/robot/ros_ws/src/autonomy/0_interface/mavros_interface/src/mavros_interface.cpp b/robot/ros_ws/src/autonomy/0_interface/mavros_interface/src/mavros_interface.cpp new file mode 100644 index 00000000..5fb9f05d --- /dev/null +++ b/robot/ros_ws/src/autonomy/0_interface/mavros_interface/src/mavros_interface.cpp @@ -0,0 +1,219 @@ +/** + * @file mavros_interface.cpp + * @author John Keller (jkeller2@andrew.cmu.edu), Andrew Jong + * (ajong@andrew.cmu.edu) + * @brief overrides the RobotInterface class to implement the PX4 flight control + * interface. + * @version 0.1 + * @date 2024-07-01 + * + * @copyright Copyright (c) 2024. This file is developed as part of software + * from the AirLab at the Robotics Institute at Carnegie Mellon University + * (https://theairlab.org). + * + */ +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace mavros_interface { + +class MAVROSInterface : public robot_interface::RobotInterface { + private: + // parameters + bool is_ardupilot; // TODO make this a launch file parameter + + bool is_state_received_ = false; + mavros_msgs::msg::State current_state_; + bool in_air = false; + rclcpp::Time in_air_start_time; + + // data from the flight control unit (FCU) + bool is_yaw_received_ = false; + float yaw_ = 0.0; + + rclcpp::CallbackGroup::SharedPtr service_callback_group; + rclcpp::Client::SharedPtr set_mode_client_; + rclcpp::Client::SharedPtr arming_client_; + rclcpp::Client::SharedPtr takeoff_client_; + + rclcpp::Publisher::SharedPtr attitude_target_pub_; + rclcpp::Publisher::SharedPtr local_position_target_pub_; + + rclcpp::Subscription::SharedPtr state_sub_; + rclcpp::Subscription::SharedPtr mavros_odometry_sub_; + + public: + MAVROSInterface() : RobotInterface("mavros_interface") { + // params + is_ardupilot = airstack::get_param(this, "is_ardupilot", false); + + // services + service_callback_group = + this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + set_mode_client_ = this->create_client( + "mavros/set_mode", rmw_qos_profile_services_default, service_callback_group); + arming_client_ = this->create_client( + "mavros/cmd/arming", rmw_qos_profile_services_default, service_callback_group); + takeoff_client_ = this->create_client( + "mavros/cmd/takeoff", rmw_qos_profile_services_default, service_callback_group); + + // publishers + attitude_target_pub_ = this->create_publisher( + "mavros/setpoint_raw/attitude", 1); + local_position_target_pub_ = this->create_publisher( + "mavros/setpoint_position/local", 1); + + // subscribers + state_sub_ = this->create_subscription( + "mavros/state", 1, + std::bind(&MAVROSInterface::state_callback, this, std::placeholders::_1)); + } + + virtual ~MAVROSInterface() {} + + // Control Callbacks. Translates commands to fit the MAVROS API. + // The MAVROS API only has two types of control: Attitude Control and + // Position Control. + + void attitude_thrust_callback(const mav_msgs::msg::AttitudeThrust::SharedPtr cmd) override { + mavros_msgs::msg::AttitudeTarget mavros_cmd; + mavros_cmd.header.stamp = this->get_clock()->now(); //.to_msg(); + mavros_cmd.type_mask = mavros_msgs::msg::AttitudeTarget::IGNORE_ROLL_RATE | + mavros_msgs::msg::AttitudeTarget::IGNORE_PITCH_RATE | + mavros_msgs::msg::AttitudeTarget::IGNORE_YAW_RATE; + + mavros_cmd.thrust = cmd->thrust.z; + mavros_cmd.orientation = cmd->attitude; + + attitude_target_pub_->publish(mavros_cmd); + } + + void roll_pitch_yawrate_thrust_callback( + const mav_msgs::msg::RollPitchYawrateThrust::SharedPtr cmd) override { + if (!is_yaw_received_) { + RCLCPP_ERROR(this->get_logger(), + "roll_pitch_yawrate_thrust command called but haven't yet " + "received drone current yaw"); + return; + } + + mavros_msgs::msg::AttitudeTarget mavros_cmd; + mavros_cmd.header.stamp = this->get_clock()->now(); //.to_msg(); + mavros_cmd.type_mask = mavros_msgs::msg::AttitudeTarget::IGNORE_ROLL_RATE | + mavros_msgs::msg::AttitudeTarget::IGNORE_PITCH_RATE; + tf2::Matrix3x3 m; + m.setRPY(cmd->roll, cmd->pitch, yaw_); + tf2::Quaternion q; + m.getRotation(q); + mavros_cmd.body_rate.z = cmd->yaw_rate; + mavros_cmd.thrust = cmd->thrust.z; + + mavros_cmd.orientation.x = q.x(); + mavros_cmd.orientation.y = q.y(); + mavros_cmd.orientation.z = q.z(); + mavros_cmd.orientation.w = q.w(); + + attitude_target_pub_->publish(mavros_cmd); + } + + void pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr cmd) override { + if (!is_ardupilot || + (in_air && ((this->get_clock()->now() - in_air_start_time).seconds() > 5.))) { + geometry_msgs::msg::PoseStamped cmd_copy = *cmd; + local_position_target_pub_->publish(cmd_copy); + } + } + + // Command Functions + + bool request_control() override { + auto request = std::make_shared(); + if (is_ardupilot) + request->custom_mode = "GUIDED"; //"OFFBOARD"; + else + request->custom_mode = "OFFBOARD"; + + auto result = set_mode_client_->async_send_request(request); + std::cout << "waiting rc" << std::endl; + result.wait(); + std::cout << "done rc" << std::endl; + + return result.get()->mode_sent; + } + + bool arm() override { + auto request = std::make_shared(); + request->value = true; + + auto result = arming_client_->async_send_request(request); + std::cout << "waiting arm" << std::endl; + result.wait(); + std::cout << "done arm" << std::endl; + + return result.get()->success; + } + + bool disarm() override { + bool success = false; + + auto request = std::make_shared(); + request->value = false; + + auto result = arming_client_->async_send_request(request); + std::cout << "waiting disarm" << std::endl; + result.wait(); + std::cout << "done disarm" << std::endl; + + return result.get()->success; + } + + bool is_armed() override { return is_state_received_ && current_state_.armed; } + + bool has_control() override { + return is_state_received_ && + (is_ardupilot ? current_state_.mode == "GUIDED" : current_state_.mode == "OFFBOARD"); + } + + bool takeoff() override { + if (is_ardupilot) { + mavros_msgs::srv::CommandTOL::Request::SharedPtr takeoff_request = + std::make_shared(); + takeoff_request->altitude = 0.1; + + std::cout << "ardupilot takeoff 1" << std::endl; + auto takeoff_result = takeoff_client_->async_send_request(takeoff_request); + takeoff_result.wait(); + std::cout << "ardupilot takeoff 2" << std::endl; + if (takeoff_result.get()->success) { + in_air = true; + in_air_start_time = this->get_clock()->now(); + return true; + } else + return false; + } + + return true; + } + + bool land() override {} + + void state_callback(const mavros_msgs::msg::State::SharedPtr msg) { + is_state_received_ = true; + current_state_ = *msg; + } +}; +} // namespace mavros_interface +#include + +PLUGINLIB_EXPORT_CLASS(mavros_interface::MAVROSInterface, robot_interface::RobotInterface) diff --git a/robot/ros_ws/src/autonomy/0_interface/robot_interface/CMakeLists.txt b/robot/ros_ws/src/autonomy/0_interface/robot_interface/CMakeLists.txt new file mode 100644 index 00000000..505aa239 --- /dev/null +++ b/robot/ros_ws/src/autonomy/0_interface/robot_interface/CMakeLists.txt @@ -0,0 +1,112 @@ +cmake_minimum_required(VERSION 3.8) +project(robot_interface) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(pluginlib REQUIRED) +find_package(mav_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(airstack_msgs REQUIRED) +find_package(airstack_common REQUIRED) + +add_library(robot_interface src/robot_interface.cpp) +target_include_directories(robot_interface PUBLIC + $ + $) +target_compile_features(robot_interface PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +ament_target_dependencies( + robot_interface + + # Required dependencies + geometry_msgs + mav_msgs + pluginlib + rclcpp + tf2 + tf2_ros +) +ament_export_targets(robot_interface HAS_LIBRARY_TARGET) +ament_export_dependencies(rclcpp mav_msgs geometry_msgs tf2 tf2_ros) + +install(TARGETS robot_interface + DESTINATION lib/${PROJECT_NAME}) + +add_executable(robot_interface_node src/robot_interface_node.cpp) +target_include_directories(robot_interface_node PUBLIC + $ + $) +ament_target_dependencies(robot_interface_node rclcpp pluginlib mav_msgs tf2 tf2_ros airstack_msgs airstack_common) +install(TARGETS robot_interface_node + DESTINATION lib/${PROJECT_NAME}) + +install( + DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS robot_interface + EXPORT robot_interface + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + +# odometry conversion +add_executable(odometry_conversion src/odometry_conversion.cpp) + +target_include_directories(odometry_conversion PUBLIC + $ + $) + +ament_target_dependencies(odometry_conversion + rclcpp + geometry_msgs + visualization_msgs + nav_msgs + tf2 + tf2_ros + tf2_geometry_msgs + airstack_msgs + airstack_common + ) +install(TARGETS odometry_conversion + DESTINATION lib/${PROJECT_NAME}) + +ament_export_include_directories( + include + ${mav_msgs_INCLUDE_DIRS} + ${geometry_msgs_INCLUDE_DIRS} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +# Install files. +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) + +# install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME}) +# install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) +# install(DIRECTORY params DESTINATION share/${PROJECT_NAME}) +ament_package() diff --git a/robot/ros_ws/src/autonomy/0_interface/robot_interface/include/robot_interface/robot_interface.hpp b/robot/ros_ws/src/autonomy/0_interface/robot_interface/include/robot_interface/robot_interface.hpp new file mode 100644 index 00000000..21f10a63 --- /dev/null +++ b/robot/ros_ws/src/autonomy/0_interface/robot_interface/include/robot_interface/robot_interface.hpp @@ -0,0 +1,145 @@ +/** + * + * @file robot_interface.hpp + * @author John Keller (jkeller2@andrew.cmu.edu), Andrew Jong + * (ajong@andrew.cmu.edu) + * @brief the Robot Interface specifies common MAV flight control functions for + * individual implementations to define. + * @version 0.1 + * @date 2024-07-01 + * + * @copyright Copyright (c) 2024. This file is developed as part of software + * from the AirLab at the Robotics Institute at Carnegie Mellon University + * (https://theairlab.org). + */ + +#pragma once + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace robot_interface { + /** + * @brief The Robot Interface class specifies common MAV flight control + * functions for individual implementations to define. + * + * Sets up subscribers for different types of control commands that call their respective + * callback implementations. It's up to the individual implementations to define the + * control logic. + */ + class RobotInterface : public rclcpp::Node { + private: + // Command subscribers + rclcpp::Subscription::SharedPtr attitude_thrust_sub_; + rclcpp::Subscription::SharedPtr rate_thrust_sub_; + rclcpp::Subscription::SharedPtr roll_pitch_yawrate_thrust_sub_; + rclcpp::Subscription::SharedPtr torque_thrust_sub_; + rclcpp::Subscription::SharedPtr velocity_sub_; + rclcpp::Subscription::SharedPtr pose_sub_; + + public: + RobotInterface(std::string interface_name) + : rclcpp::Node("robot_interface"){ + attitude_thrust_sub_ = + this->create_subscription("cmd_attitude_thrust", 1, + std::bind(&RobotInterface::attitude_thrust_callback, this, + std::placeholders::_1)); + rate_thrust_sub_ = + this->create_subscription("cmd_rate_thrust", 1, + std::bind(&RobotInterface::rate_thrust_callback, + this, std::placeholders::_1)); + roll_pitch_yawrate_thrust_sub_ = + this->create_subscription("cmd_roll_pitch_yawrate_thrust", 1, + std::bind(&RobotInterface::roll_pitch_yawrate_thrust_callback, + this, std::placeholders::_1)); + torque_thrust_sub_ = + this->create_subscription("cmd_torque_thrust", 1, + std::bind(&RobotInterface::torque_thrust_callback, + this, std::placeholders::_1)); + + velocity_sub_ = + this->create_subscription("cmd_velocity", 1, + std::bind(&RobotInterface::velocity_callback, + this, std::placeholders::_1)); + + pose_sub_ = + this->create_subscription("cmd_pose", 1, + std::bind(&RobotInterface::pose_callback, + this, std::placeholders::_1)); + } + + // Control callbacks. + virtual void attitude_thrust_callback(const mav_msgs::msg::AttitudeThrust::SharedPtr cmd) { + RCLCPP_ERROR(this->get_logger(), "Attitude thrust callback not implemented."); + } + virtual void rate_thrust_callback(const mav_msgs::msg::RateThrust::SharedPtr cmd) { + RCLCPP_ERROR(this->get_logger(), "Rate thrust callback not implemented."); + } + virtual void roll_pitch_yawrate_thrust_callback(const mav_msgs::msg::RollPitchYawrateThrust::SharedPtr cmd) { + RCLCPP_ERROR(this->get_logger(), "Roll pitch yawrate thrust callback not implemented."); + } + virtual void torque_thrust_callback(const mav_msgs::msg::TorqueThrust::SharedPtr cmd) { + RCLCPP_ERROR(this->get_logger(), "Torque thrust callback not implemented."); + } + virtual void velocity_callback(const geometry_msgs::msg::TwistStamped::SharedPtr cmd) { + RCLCPP_ERROR(this->get_logger(), "Velocity callback not implemented."); + } + virtual void pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr cmd) { + RCLCPP_ERROR(this->get_logger(), "Pose callback not implemented."); + } + + // Command functions + /** + * @brief Call the service to request that the flight controller relinquish + * control to the autonomy stack. + * + * @return true + * @return false + */ + virtual bool request_control() = 0; + /** + * @brief Set the flight controller to "armed" + * + * @return true + * @return false + */ + virtual bool arm() = 0; + /** + * @brief Set the flight controller to "disarmed" + * + * @return true + * @return false + */ + virtual bool disarm() = 0; + /** + * @brief Check if the flight controller is armed. + * + * @return true + * @return false + */ + virtual bool is_armed() = 0; + /** + * @brief Check if the autonomy stack has control of the flight controller. + * + * @return true + * @return false + */ + virtual bool has_control() = 0; + + virtual bool takeoff() = 0; + virtual bool land() = 0; + + public: + virtual ~RobotInterface() {} + }; +} // namespace robot_interface diff --git a/robot/ros_ws/src/autonomy/0_interface/robot_interface/launch/odometry_conversion.xml b/robot/ros_ws/src/autonomy/0_interface/robot_interface/launch/odometry_conversion.xml new file mode 100644 index 00000000..384160a4 --- /dev/null +++ b/robot/ros_ws/src/autonomy/0_interface/robot_interface/launch/odometry_conversion.xml @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/0_interface/robot_interface/package.xml b/robot/ros_ws/src/autonomy/0_interface/robot_interface/package.xml new file mode 100644 index 00000000..41909138 --- /dev/null +++ b/robot/ros_ws/src/autonomy/0_interface/robot_interface/package.xml @@ -0,0 +1,28 @@ + + + + robot_interface + 0.0.0 + TODO: Package description + uav + TODO: License declaration + + ament_cmake + + tf2 + tf2_ros + tf2_tools + rclcpp + pluginlib + mav_msgs + geometry_msgs + airstack_msgs + airstack_common + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/robot/ros_ws/src/autonomy/0_interface/robot_interface/src/odometry_conversion.cpp b/robot/ros_ws/src/autonomy/0_interface/robot_interface/src/odometry_conversion.cpp new file mode 100644 index 00000000..7c422ea8 --- /dev/null +++ b/robot/ros_ws/src/autonomy/0_interface/robot_interface/src/odometry_conversion.cpp @@ -0,0 +1,142 @@ +#include +#include +#include +#include + +/** + * @brief + * Does several things + * - if there's an odometry, republishes it with a new frame_id + * - if there's an odometry, republishes it as a transform + * - converts MAVROS odometry BEST_EFFORT to RELIABLE + * + */ + +class OdometryConversion : public rclcpp::Node { + private: + enum OdometryOutputType { NONE, TRANSFORM, OVERWRITE }; + + rclcpp::Subscription::SharedPtr odom_sub; + rclcpp::Publisher::SharedPtr odom_pub; + tf2_ros::Buffer* tf_buffer; + tf2_ros::TransformListener* tf_listener; + tf2_ros::TransformBroadcaster* tf_broadcaster; + + bool odom_input_qos_is_best_effort; + std::string new_frame_id; + std::string new_child_frame_id; + OdometryOutputType odometry_output_type; + bool convert_odometry_to_transform; + bool convert_odometry_to_stabilized_transform; + bool restamp_now_pre, restamp_now_post; + tf2::Quaternion odom_orientation_rotation_pre, odom_orientation_rotation_post; + tf2::Vector3 odom_position_translation_pre, odom_position_translation_post; + + public: + OdometryConversion() : Node("odometry_conversion") { + ; + odom_input_qos_is_best_effort = + airstack::get_param(this, "odom_input_qos_is_best_effort", false); + new_frame_id = airstack::get_param(this, "new_frame_id", std::string("")); + new_child_frame_id = airstack::get_param(this, "new_child_frame_id", std::string("")); + odometry_output_type = + (OdometryOutputType)airstack::get_param(this, "odometry_output_type", (int)NONE); + convert_odometry_to_transform = + airstack::get_param(this, "convert_odometry_to_transform", false); + convert_odometry_to_stabilized_transform = + airstack::get_param(this, "convert_odometry_to_stabilized_transform", false); + restamp_now_pre = airstack::get_param(this, "restamp_now_pre", false); + restamp_now_post = airstack::get_param(this, "restamp_now_post", false); + odom_orientation_rotation_pre = + tf2::Quaternion(airstack::get_param(this, "odometry_orientation_rotation_pre_x", 0.), + airstack::get_param(this, "odometry_orientation_rotation_pre_y", 0.), + airstack::get_param(this, "odometry_orientation_rotation_pre_z", 0.), + airstack::get_param(this, "odometry_orientation_rotation_pre_w", 1.)); + odom_position_translation_pre = + tf2::Vector3(airstack::get_param(this, "odometry_position_translation_pre_x", 0.), + airstack::get_param(this, "odometry_position_translation_pre_y", 0.), + airstack::get_param(this, "odometry_position_translation_pre_z", 0.)); + odom_orientation_rotation_post = + tf2::Quaternion(airstack::get_param(this, "odometry_orientation_rotation_post_x", 0.), + airstack::get_param(this, "odometry_orientation_rotation_post_y", 0.), + airstack::get_param(this, "odometry_orientation_rotation_post_z", 0.), + airstack::get_param(this, "odometry_orientation_rotation_post_w", 1.)); + odom_position_translation_post = + tf2::Vector3(airstack::get_param(this, "odometry_position_translation_post_x", 0.), + airstack::get_param(this, "odometry_position_translation_post_y", 0.), + airstack::get_param(this, "odometry_position_translation_post_z", 0.)); + + rmw_qos_profile_t qos = rmw_qos_profile_sensor_data; + qos.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST; + qos.depth = 1; + if (odom_input_qos_is_best_effort) qos.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + odom_sub = this->create_subscription( + "odometry_in", rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(qos), qos), + std::bind(&OdometryConversion::odom_callback, this, std::placeholders::_1)); + odom_pub = this->create_publisher("odometry_out", 1); + tf_buffer = new tf2_ros::Buffer(this->get_clock()); + tf_listener = new tf2_ros::TransformListener(*tf_buffer); + tf_broadcaster = new tf2_ros::TransformBroadcaster(*this); + } + + void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) { + nav_msgs::msg::Odometry out_odom = *msg; + + if (restamp_now_pre) out_odom.header.stamp = get_clock()->now(); + + out_odom.pose.pose.orientation = tflib::from_tf( + tflib::to_tf(out_odom.pose.pose.orientation) * odom_orientation_rotation_pre); + + if (odometry_output_type == NONE) { + // do nothing + } + // transform mode transforms the odometry to a new frame, expects the new frame to exist in + // the same tf tree + else if (odometry_output_type == TRANSFORM) { + try { + out_odom = tflib::transform_odometry(tf_buffer, out_odom, new_frame_id, + new_child_frame_id); + } catch (tf2::TransformException& ex) { + RCLCPP_ERROR_STREAM( + get_logger(), "TransformException while transforming odometry: " << ex.what()); + return; + } + } + // overwrite mode simply overwrites the header with the desired frame id and child frame id + else if (odometry_output_type == OVERWRITE) { + out_odom.header.frame_id = new_frame_id; + out_odom.child_frame_id = new_child_frame_id; + } else { + RCLCPP_ERROR_STREAM(get_logger(), + "Unsupported odometry output type: " << odometry_output_type); + } + + if (restamp_now_post) out_odom.header.stamp = get_clock()->now(); + + out_odom.pose.pose.orientation = tflib::from_tf( + tflib::to_tf(out_odom.pose.pose.orientation) * odom_orientation_rotation_post); + + if (convert_odometry_to_transform) { + geometry_msgs::msg::TransformStamped t = tf2::toMsg(tflib::to_tf(out_odom)); + t.child_frame_id = out_odom.child_frame_id; + tf_broadcaster->sendTransform(t); + } + + if (convert_odometry_to_stabilized_transform) { + geometry_msgs::msg::TransformStamped t = tf2::toMsg(tflib::to_tf(out_odom)); + t.child_frame_id = out_odom.child_frame_id + "_stabilized"; + t.transform.rotation = + tflib::from_tf(tflib::get_stabilized(tflib::to_tf(out_odom.pose.pose.orientation))); + tf_broadcaster->sendTransform(t); + } + + odom_pub->publish(out_odom); + } +}; + +int main(int argc, char* argv[]) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/robot/ros_ws/src/autonomy/0_interface/robot_interface/src/robot_interface.cpp b/robot/ros_ws/src/autonomy/0_interface/robot_interface/src/robot_interface.cpp new file mode 100644 index 00000000..6db53fce --- /dev/null +++ b/robot/ros_ws/src/autonomy/0_interface/robot_interface/src/robot_interface.cpp @@ -0,0 +1,16 @@ +/** + * @file robot_interface.cpp + * @author your name (you@domain.com) + * @brief this file is a blank file so that we can define it as a library in the CMakeLists.txt + * file. theoretically we should use the add_library(robot_interface INTERFACE) command in the + * CMakeLists.txt file, but haven't figured out how to make it work with ament_target_dependencies() + * yet + * @version 0.1 + * @date 2024-07-03 + * + * @copyright Copyright (c) 2024 + * + */ +#include + +int blank() { return 0; } diff --git a/robot/ros_ws/src/autonomy/0_interface/robot_interface/src/robot_interface_node.cpp b/robot/ros_ws/src/autonomy/0_interface/robot_interface/src/robot_interface_node.cpp new file mode 100644 index 00000000..5d20ba33 --- /dev/null +++ b/robot/ros_ws/src/autonomy/0_interface/robot_interface/src/robot_interface_node.cpp @@ -0,0 +1,151 @@ +/** + * @file robot_interface_node.cpp + * @author John Keller (jkeller2@andrew.cmu.edu), Andrew Jong + * (ajong@andrew.cmu.edu) + * @brief main function for the robot interface node + * @version 0.1 + * @date 2024-07-01 + * + * @copyright Copyright (c) 2024. This file is developed as part of software + * from the AirLab at the Robotics Institute at Carnegie Mellon University + * (https://theairlab.org). + * + */ +#include +#include +#include +#include +#include + +std::shared_ptr ri; + +rclcpp::Publisher::SharedPtr is_armed_pub; +rclcpp::Publisher::SharedPtr has_control_pub; + +void robot_command_callback( + const std::shared_ptr request, + std::shared_ptr response) { + // response->sum = request->a + request->b; + switch (request->command) { + case airstack_msgs::srv::RobotCommand::Request::REQUEST_CONTROL: + response->success = ri->request_control(); + break; + case airstack_msgs::srv::RobotCommand::Request::ARM: + response->success = ri->arm(); + break; + case airstack_msgs::srv::RobotCommand::Request::DISARM: + response->success = ri->disarm(); + break; + case airstack_msgs::srv::RobotCommand::Request::TAKEOFF: + response->success = ri->takeoff(); + break; + case airstack_msgs::srv::RobotCommand::Request::LAND: + response->success = ri->land(); + break; + /* + case airstack_msgs::srv::RobotCommand::Request::SET_LOW_THRUST_MODE: + drone_interface->set_low_thrust_mode(true); + response->success = true; + break; + case airstack_msgs::srv::RobotCommand::Request::UNSET_LOW_THRUST_MODE: + drone_interface->set_low_thrust_mode(false); + response->success = true; + break; + */ + } +} + +void attitude_thrust_callback(mav_msgs::msg::AttitudeThrust::SharedPtr msg) { + ri->attitude_thrust_callback(msg); +} + +void rate_thrust_callback(mav_msgs::msg::RateThrust::SharedPtr msg) { + ri->rate_thrust_callback(msg); +} + +void roll_pitch_yawrate_thrust_callback(mav_msgs::msg::RollPitchYawrateThrust::SharedPtr msg) { + ri->roll_pitch_yawrate_thrust_callback(msg); +} + +void torque_thrust_callback(mav_msgs::msg::TorqueThrust::SharedPtr msg) { + ri->torque_thrust_callback(msg); +} + +void velocity_callback(geometry_msgs::msg::TwistStamped::SharedPtr msg) { + ri->velocity_callback(msg); +} + +void pose_callback(geometry_msgs::msg::PoseStamped::SharedPtr msg) { ri->pose_callback(msg); } + +void timer_callback() { + std_msgs::msg::Bool is_armed; + is_armed.data = ri->is_armed(); + is_armed_pub->publish(is_armed); + + std_msgs::msg::Bool has_control; + has_control.data = ri->has_control(); + has_control_pub->publish(has_control); +} + +rclcpp::Subscription::SharedPtr attitude_thrust_sub; +rclcpp::Subscription::SharedPtr rate_thrust_sub; +rclcpp::Subscription::SharedPtr + roll_pitch_yawrate_thrust_sub; +rclcpp::Subscription::SharedPtr torque_thrust_sub; +rclcpp::Subscription::SharedPtr velocity_sub; +rclcpp::Subscription::SharedPtr pose_sub; + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + + // load the interface parameter + std::shared_ptr node = rclcpp::Node::make_shared("robot_interface_node"); + std::string interface = + airstack::get_param(node, "interface", std::string("mavros_interface::MAVROSInterface")); + node.reset(); + + pluginlib::ClassLoader loader( + "robot_interface", "robot_interface::RobotInterface"); + + try { + ri = loader.createSharedInstance("mavros_interface::MAVROSInterface"); + + // subscribers + attitude_thrust_sub = ri->create_subscription( + "attitude_thrust_command", 1, attitude_thrust_callback); + rate_thrust_sub = ri->create_subscription( + "rate_thrust_command", 1, rate_thrust_callback); + roll_pitch_yawrate_thrust_sub = + ri->create_subscription( + "roll_pitch_yawrate_thrust_command", 1, roll_pitch_yawrate_thrust_callback); + torque_thrust_sub = ri->create_subscription( + "torque_thrust_command", 1, torque_thrust_callback); + velocity_sub = ri->create_subscription( + "velocity_command", 1, velocity_callback); + pose_sub = ri->create_subscription("pose_command", 1, + pose_callback); + + // publishers + is_armed_pub = ri->create_publisher("is_armed", 1); + has_control_pub = ri->create_publisher("has_control", 1); + + // services + rclcpp::Service::SharedPtr service = + ri->create_service("robot_command", + &robot_command_callback); + + // timers + rclcpp::TimerBase::SharedPtr timer = rclcpp::create_timer( + ri, ri->get_clock(), rclcpp::Duration::from_seconds(1. / 20.), &timer_callback); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(ri); + executor.spin(); + // rclcpp::spin(ri); + rclcpp::shutdown(); + } catch (pluginlib::PluginlibException& ex) { + std::cout << "The plugin failed to load. Error: " << ex.what() << std::endl; + } + + return 0; +} diff --git a/robot/ros_ws/src/autonomy/1_sensors/sensors_bringup/CMakeLists.txt b/robot/ros_ws/src/autonomy/1_sensors/sensors_bringup/CMakeLists.txt new file mode 100644 index 00000000..785bb640 --- /dev/null +++ b/robot/ros_ws/src/autonomy/1_sensors/sensors_bringup/CMakeLists.txt @@ -0,0 +1,32 @@ +cmake_minimum_required(VERSION 3.8) +project(sensors_bringup) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +# Install files. +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +# install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME}) +# install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) +# install(DIRECTORY params DESTINATION share/${PROJECT_NAME}) + +ament_package() diff --git a/robot/ros_ws/src/autonomy/1_sensors/sensors_bringup/LICENSE b/robot/ros_ws/src/autonomy/1_sensors/sensors_bringup/LICENSE new file mode 100644 index 00000000..d6456956 --- /dev/null +++ b/robot/ros_ws/src/autonomy/1_sensors/sensors_bringup/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/robot/ros_ws/src/autonomy/1_sensors/sensors_bringup/launch/sensors.launch.xml b/robot/ros_ws/src/autonomy/1_sensors/sensors_bringup/launch/sensors.launch.xml new file mode 100644 index 00000000..b5bebe8e --- /dev/null +++ b/robot/ros_ws/src/autonomy/1_sensors/sensors_bringup/launch/sensors.launch.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/1_sensors/sensors_bringup/package.xml b/robot/ros_ws/src/autonomy/1_sensors/sensors_bringup/package.xml new file mode 100644 index 00000000..0c14d91a --- /dev/null +++ b/robot/ros_ws/src/autonomy/1_sensors/sensors_bringup/package.xml @@ -0,0 +1,18 @@ + + + + sensors_bringup + 0.0.0 + TODO: Package description + andrew + Apache-2.0 + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/robot/ros_ws/src/autonomy/2_perception/perception_bringup/CMakeLists.txt b/robot/ros_ws/src/autonomy/2_perception/perception_bringup/CMakeLists.txt new file mode 100644 index 00000000..2384d3aa --- /dev/null +++ b/robot/ros_ws/src/autonomy/2_perception/perception_bringup/CMakeLists.txt @@ -0,0 +1,32 @@ +cmake_minimum_required(VERSION 3.8) +project(perception_bringup) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +# Install files. +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +# install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME}) +# install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) +# install(DIRECTORY params DESTINATION share/${PROJECT_NAME}) + +ament_package() diff --git a/robot/ros_ws/src/autonomy/2_perception/perception_bringup/LICENSE b/robot/ros_ws/src/autonomy/2_perception/perception_bringup/LICENSE new file mode 100644 index 00000000..d6456956 --- /dev/null +++ b/robot/ros_ws/src/autonomy/2_perception/perception_bringup/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/robot/ros_ws/src/autonomy/2_perception/perception_bringup/launch/perception.launch.xml b/robot/ros_ws/src/autonomy/2_perception/perception_bringup/launch/perception.launch.xml new file mode 100644 index 00000000..0363a73b --- /dev/null +++ b/robot/ros_ws/src/autonomy/2_perception/perception_bringup/launch/perception.launch.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/2_perception/perception_bringup/package.xml b/robot/ros_ws/src/autonomy/2_perception/perception_bringup/package.xml new file mode 100644 index 00000000..90354365 --- /dev/null +++ b/robot/ros_ws/src/autonomy/2_perception/perception_bringup/package.xml @@ -0,0 +1,18 @@ + + + + perception_bringup + 0.0.0 + TODO: Package description + andrew + Apache-2.0 + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/CMakeLists.txt b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/CMakeLists.txt new file mode 100644 index 00000000..717649f7 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/CMakeLists.txt @@ -0,0 +1,272 @@ +cmake_minimum_required(VERSION 3.5) +project(disparity_expansion) + +# # Find catkin macros and libraries +# # if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +# # is used, also find other catkin packages +# find_package(catkin REQUIRED COMPONENTS +# cv_bridge +# image_geometry +# image_transport +# roscpp +# std_msgs +# tf +# ) +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(image_geometry REQUIRED) +find_package(image_transport REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(tf2 REQUIRED) + +find_package(stereo_msgs REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) + +find_package(message_filters REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(pcl_conversions REQUIRED) + +# CONFIGURE OPENCV +find_package(OpenCV REQUIRED) + +# CONFIGURE PCL +find_package(PCL 1.12 REQUIRED) + +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +# # System dependencies are found with CMake's conventions +message(STATUS "PCL_INCLUDE_DIRS: ${PCL_INCLUDE_DIRS}") + +# # Uncomment this if the package has a setup.py. This macro ensures +# # modules and global scripts declared therein get installed +# # See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +# ############################################### +# # Declare ROS messages, services and actions ## +# ############################################### + +# # To declare and build messages, services or actions from within this +# # package, follow these steps: +# # * Let MSG_DEP_SET be the set of packages whose message types you use in +# # your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +# # * In the file package.xml: +# # * add a build_depend and a run_depend tag for each package in MSG_DEP_SET +# # * If MSG_DEP_SET isn't empty the following dependencies might have been +# # pulled in transitively but can be declared for certainty nonetheless: +# # * add a build_depend tag for "message_generation" +# # * add a run_depend tag for "message_runtime" +# # * In this file (CMakeLists.txt): +# # * add "message_generation" and every package in MSG_DEP_SET to +# # find_package(catkin REQUIRED COMPONENTS ...) +# # * add "message_runtime" and every package in MSG_DEP_SET to +# # catkin_package(CATKIN_DEPENDS ...) +# # * uncomment the add_*_files sections below as needed +# # and list every .msg/.srv/.action file to be processed +# # * uncomment the generate_messages entry below +# # * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +# # Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +# # Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +# # Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +# # Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs +# ) + +# ################################## +# # catkin specific configuration ## +# ################################## +# # The catkin_package macro generates cmake config files for your package +# # Declare things to be passed to dependent projects +# # INCLUDE_DIRS: uncomment this if you package contains header files +# # LIBRARIES: libraries you create in this project that dependent projects also need +# # CATKIN_DEPENDS: catkin_packages dependent projects also need +# # DEPENDS: system dependencies of this project that dependent projects also need +# catkin_package( +# INCLUDE_DIRS +# include +# ${catkin_INCLUDE_DIRS} +# ${OpenCV_INCLUDE_DIRS} +# ${PCL_INCLUDE_DIRS} +include_directories( + ${rclcpp_INCLUDE_DIRS} + ${rcl_interfaces_INCLUDE_DIRS} + ${cv_bridge_INCLUDE_DIRS} + ${image_geometry_INCLUDE_DIRS} + ${image_transport_INCLUDE_DIRS} + ${sensor_msgs_INCLUDE_DIRS} + ${std_msgs_INCLUDE_DIRS} + ${tf2_INCLUDE_DIRS} + ${pcl_ros_INCLUDE_DIRS} + ${OpenCV_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} + ${pcl_conversions_INCLUDE_DIRS} + ${tf2_geometry_msgs_INCLUDE_DIRS} +) + +# LIBRARIES disparity_expansion +# CATKIN_DEPENDS cv_bridge image_geometry image_transport roscpp std_msgs tf +# DEPENDS system_lib +# ) + +# ########## +# # Build ## +# ########## +if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) + message("${PROJECT_NAME}: You did not request a specific build type: selecting 'Release'.") + set(CMAKE_BUILD_TYPE Release) +endif() + +# # Specify additional locations of header files +# # Your package locations should be listed before other locations +# include_directories(include) +include_directories( + + # ${catkin_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} +) + +# # Declare a cpp library +# add_library(disparity_expansion +# src/${PROJECT_NAME}/disparity_expansion.cpp +# ) + +# # Declare a cpp executable +add_executable(disparity_expansion src/disparity_expansion.cpp) +target_include_directories(disparity_expansion PUBLIC + $ + $) + +target_link_libraries(disparity_expansion ${OpenCV_LIBS} ${catkin_LIBRARIES} +) + +ament_target_dependencies(disparity_expansion + rclcpp + rcl_interfaces + cv_bridge + image_geometry + image_transport + sensor_msgs + std_msgs + tf2 + pcl_conversions + message_filters + visualization_msgs + stereo_msgs + tf2_geometry_msgs +) + +add_executable(disparity_pcd src/disparity_pcd.cpp) +target_link_libraries(disparity_pcd ${OpenCV_LIBS} ${catkin_LIBRARIES} +) + +ament_target_dependencies(disparity_pcd + rclcpp + rcl_interfaces + cv_bridge + image_geometry + image_transport + sensor_msgs + std_msgs + tf2 + pcl_conversions + message_filters + tf2_geometry_msgs +) + +# install(TARGETS disparity_expansion +# RUNTIME DESTINATION bin) +install(TARGETS disparity_expansion + DESTINATION lib/${PROJECT_NAME} +) +install(TARGETS disparity_pcd + DESTINATION lib/${PROJECT_NAME} +) +install(FILES package.xml + DESTINATION share/${PROJECT_NAME} +) + +# install(DIRECTORY params DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) + +# # Add cmake target dependencies of the executable/library +# # as an example, message headers may need to be generated before nodes +# add_dependencies(disparity_expansion_node disparity_expansion_generate_messages_cpp) + +# # Specify libraries to link a library or executable target against +# target_link_libraries(disparity_expansion_node +# ${catkin_LIBRARIES} +# ) +ament_package() + +# ############ +# # Install ## +# ############ + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +# # Mark executable scripts (Python etc.) for installation +# # in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +# # Mark executables and/or libraries for installation +# install(TARGETS disparity_expansion disparity_expansion_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +# # Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +# # Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +# ############ +# # Testing ## +# ############ + +# # Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_disparity_expansion.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +# # Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/LICENSE b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/LICENSE new file mode 100644 index 00000000..c0b5f17b --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/LICENSE @@ -0,0 +1,15 @@ +### License ### +[This software is BSD licensed.](http://opensource.org/licenses/BSD-3-Clause) + +Copyright (c) 2019, Carnegie Mellon University +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/config/disparity_expansion_params.yaml b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/config/disparity_expansion_params.yaml new file mode 100644 index 00000000..4c7c138e --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/config/disparity_expansion_params.yaml @@ -0,0 +1,13 @@ +/**: + ros__parameters: + # parameters + scale: 1.0 # units size of a meter. depth value corresponds to this many meters + downsample_scale: 1.0 # the ratio to scale down the disparity image before processing + baseline_fallback: 0.5 # if the baseline is 0 from the camera_info, use this value instead + lut_max_disparity: 180 # maximum disparity value in the disparity image + robot_radius: 1.0 + bg_multiplier: 1.0 + # sensor_pixel_error: 0.5 # what is this? and why was it 0.5? + sensor_pixel_error: 0.0 + + \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/include/disparity_expansion/disparity_expansion.hpp b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/include/disparity_expansion/disparity_expansion.hpp new file mode 100644 index 00000000..65459a3a --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/include/disparity_expansion/disparity_expansion.hpp @@ -0,0 +1,101 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "opencv2/core/core.hpp" +#include "opencv2/opencv.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/camera_info.hpp" +#include "sensor_msgs/msg/image.hpp" +#include "std_msgs/msg/header.hpp" + +typedef pcl::PointCloud PointCloud; + +class DisparityExpansionNode : public rclcpp::Node { + protected: + rclcpp::Subscription::SharedPtr cam_info_sub_; + rclcpp::Subscription::SharedPtr disparity_sub_; + rclcpp::Subscription::SharedPtr depth_sub_; + + rclcpp::Publisher::SharedPtr expanded_disparity_fg_pub; + rclcpp::Publisher::SharedPtr expanded_disparity_bg_pub; + rclcpp::Publisher::SharedPtr expansion_poly_pub; + rclcpp::Publisher::SharedPtr expansion_cloud_pub; + rclcpp::Publisher::SharedPtr frustum_pub; + + bool LUT_ready = false; + bool got_cam_info = false; + + double downsample_scale; + image_geometry::PinholeCameraModel model_; + double baseline; + + struct LUTCell { + unsigned int idx1; + unsigned int idx2; + }; + int lut_max_disparity; + double robot_radius; + double padding; + double bg_multiplier; + double pixel_error; + double scale; + std::vector> table_u; + std::vector> table_v; + double table_d[200]; + double fx, fy, cx, cy; + unsigned int width, height; + + cv::Mat convert_depth_to_disparity(const cv::Mat& depth_image); + + void generate_expansion_lookup_table(); + + public: + DisparityExpansionNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + + void set_cam_info(const sensor_msgs::msg::CameraInfo::ConstSharedPtr& msg_info); + + /** + * @brief Use depth image input instead of disparity. Converts depth to disparity, then call + * process_disparity_image + * + * @param msg + */ + void process_depth_image(const sensor_msgs::msg::Image::ConstSharedPtr& msg); + + void process_disparity_image(const stereo_msgs::msg::DisparityImage::ConstSharedPtr& msg_disp); + + void publish_expansion_poly(const stereo_msgs::msg::DisparityImage::ConstSharedPtr& msg_disp, + const cv_bridge::CvImagePtr& fg_msg, + const cv_bridge::CvImagePtr& bg_msg); + + void publish_expansion_cloud(const stereo_msgs::msg::DisparityImage::ConstSharedPtr& msg_disp, + const cv_bridge::CvImageConstPtr& cv_ptrdisparity, + const cv_bridge::CvImagePtr& fg_msg, + const cv_bridge::CvImagePtr& bg_msg); + + void publish_frustum(const stereo_msgs::msg::DisparityImage::ConstSharedPtr& msg); +}; diff --git a/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/index.html b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/index.html new file mode 100644 index 00000000..bd993ecd --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/index.html @@ -0,0 +1,2620 @@ + + + + + + + + + + + + + + + + + + + README - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

README

+

This package generates a world representation using disparity images. This enables planning in image space by applying C-space expansion in 2.5D disparity images.

+

It generates a pair expansion images: one for foreground expansion and one for background expansion.

+

Currently this package also has the following nodes

+
    +
  • +

    disparity_expansion: Generate expanded disparity

    +
    • +
  • +

    disparity_pcd: Disparity image to point cloud.

    +
    • +
+

Who do I talk to?

+ +

License

+

BSD, see LICENSE

+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ +
+ + + +
+
+
+ +
+ Copyright © 2024 - 2030 AirLab CMU +
+ + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + +
+ +
+
+
+ +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/launch/disparity_expansion.launch.xml b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/launch/disparity_expansion.launch.xml new file mode 100644 index 00000000..59c718d9 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/launch/disparity_expansion.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + + \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/launch/disparity_pcd.launch.xml b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/launch/disparity_pcd.launch.xml new file mode 100644 index 00000000..52ab5061 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/launch/disparity_pcd.launch.xml @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/package.xml b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/package.xml new file mode 100644 index 00000000..2da6f7f1 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/package.xml @@ -0,0 +1,78 @@ + + + + disparity_expansion + 0.0.0 + The disparity_expansion package + + + + + aeroscout + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + ament_cmake + rclcpp + cv_bridge + image_geometry + image_transport + sensor_msgs + std_msgs + tf2 + pcl_conversions + visualization_msgs + message_filters + stereo_msgs + OpenCV + PCL + + rclcpp + cv_bridge + image_geometry + image_transport + sensor_msgs + std_msgs + tf2 + pcl_conversions + visualization_msgs + message_filters + stereo_msgs + OpenCV + PCL + + + + + + ament_cmake + + diff --git a/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/src/disparity_expansion.cpp b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/src/disparity_expansion.cpp new file mode 100644 index 00000000..253b53c8 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/src/disparity_expansion.cpp @@ -0,0 +1,659 @@ +/* + * Copyright (c) 2016 Carnegie Mellon University, Author + * + * For License information please see the LICENSE file in the root directory. + * + */ + +// Applies C-Space expansion on disparity images. +#include + +DisparityExpansionNode::DisparityExpansionNode(const rclcpp::NodeOptions& options) + : Node("DisparityExpansionNode", options) { + this->declare_parameter("scale", 2.0); + this->get_parameter("scale", this->scale); + this->declare_parameter("robot_radius", 2.0); + this->get_parameter("robot_radius", this->robot_radius); + this->declare_parameter("lut_max_disparity", 164); + this->get_parameter("lut_max_disparity", this->lut_max_disparity); + this->declare_parameter("padding", 2.0); + this->get_parameter("padding", this->padding); + this->declare_parameter("baseline_fallback", 0.5); + this->declare_parameter("bg_multiplier", 5.0); + this->get_parameter("bg_multiplier", this->bg_multiplier); + this->declare_parameter("sensor_pixel_error", 0.5); + this->get_parameter("sensor_pixel_error", this->pixel_error); + this->declare_parameter("downsample_scale", 2.0); + this->get_parameter("downsample_scale", this->downsample_scale); + + // subscribers + this->cam_info_sub_ = this->create_subscription( + "camera_info", 1, + std::bind(&DisparityExpansionNode::set_cam_info, this, std::placeholders::_1)); + + this->disparity_sub_ = this->create_subscription( + "disparity", 1, + std::bind(&DisparityExpansionNode::process_disparity_image, this, std::placeholders::_1)); + + this->depth_sub_ = this->create_subscription( + "depth", 1, + std::bind(&DisparityExpansionNode::process_depth_image, this, std::placeholders::_1)); + + // publishers + this->expanded_disparity_fg_pub = + this->create_publisher("expanded_disparity_fg", 10); + this->expanded_disparity_bg_pub = + this->create_publisher("expanded_disparity_bg", 10); + this->expansion_poly_pub = + this->create_publisher("expansion_poly", 10); + this->expansion_cloud_pub = + this->create_publisher("expansion_cloud", 10); + this->frustum_pub = this->create_publisher("frustum", 10); + + RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << " waiting"); +} + +void DisparityExpansionNode::set_cam_info( + const sensor_msgs::msg::CameraInfo::ConstSharedPtr& msg_info) { + if (this->got_cam_info) return; + this->model_.fromCameraInfo(msg_info); + RCLCPP_INFO_ONCE(this->get_logger(), "Cam Info Recvd Fx Fy Cx Cy: %f %f , %f %f", + this->model_.fx(), this->model_.fy(), this->model_.cx(), this->model_.cy()); + this->cx = this->model_.cx() / this->downsample_scale; + this->cy = this->model_.cy() / this->downsample_scale; + this->fx = this->fy = this->model_.fx() / this->downsample_scale; + this->width = msg_info->width / this->downsample_scale; + this->height = msg_info->height / this->downsample_scale; + this->baseline = -msg_info->p[3] / msg_info->p[0]; + if (this->baseline == 0.0) { + this->get_parameter("baseline_fallback", this->baseline); + RCLCPP_ERROR_STREAM_ONCE( + this->get_logger(), + "Baseline from camera_info was 0, setting to this->baseline_fallback:" + << this->baseline); + } + this->baseline *= this->downsample_scale; + this->generate_expansion_lookup_table(); + this->got_cam_info = true; + RCLCPP_INFO(this->get_logger(), "Baseline: %.4f meters", this->baseline); + RCLCPP_INFO(this->get_logger(), "Focal length (fx): %.4f pixels", this->fx); +} + +void DisparityExpansionNode::generate_expansion_lookup_table() { + if (this->LUT_ready) { + RCLCPP_ERROR(this->get_logger(), "LUT all ready"); + + return; + } + RCLCPP_INFO(this->get_logger(), "Fx Fy Cx Cy: %f %f , %f %f \nW H this->baseline: %d %d %f", + this->fx, this->fy, this->cx, this->cy, this->width, this->height, this->baseline); + double r = this->robot_radius; // expansion radius in cm + this->table_u = std::vector>(this->lut_max_disparity, + std::vector(this->width)); + this->table_v = std::vector>(this->lut_max_disparity, + std::vector(this->height)); + int u1, u2, v1, v2; + double x, y, z; + double disparity; + + for (unsigned int disp_idx = 1; disp_idx < lut_max_disparity; disp_idx++) { + disparity = disp_idx / this->scale; // 1 cell = 0.5m, z is in meters + r = this->robot_radius; // * exp(DEPTH_ERROR_COEFF*z); + z = this->baseline * this->fx / disparity; + + double disp_new = this->baseline * this->fx / (z - this->robot_radius) + 0.5; + table_d[disp_idx] = disp_new; + + for (int v = (int)height - 1; v >= 0; --v) { + y = (v - this->cy) * z / this->fy; + + double beta = atan2(z, y); + double beta1 = asin(r / sqrt(z * z + y * y)); + + double r1 = z / tan(beta + beta1); + double r2 = z / tan(beta - beta1); + v1 = this->fy * r1 / z + this->cy; + v2 = this->fy * r2 / z + this->cy; + + if ((v2 - v1) < 0) + RCLCPP_ERROR(this->get_logger(), "Something MESSED %d %d %d", v1, v2, disp_idx); + + if (v1 < 0) v1 = 0; + if (v1 > (height - 1)) v1 = height - 1; + + if (v2 < 0) v2 = 0; + if (v2 > (height - 1)) v2 = height - 1; + + this->table_v[disp_idx][v].idx1 = v1; + this->table_v[disp_idx][v].idx2 = v2; + } + + for (int u = (int)this->width - 1; u >= 0; --u) { + x = (u - this->cx) * z / this->fx; + + double alpha = atan2(z, x); + double alpha1 = asin(r / sqrt(z * z + x * x)); + + double r1 = z / tan(alpha + alpha1); + double r2 = z / tan(alpha - alpha1); + u1 = this->fx * r1 / z + this->cx; + u2 = this->fx * r2 / z + this->cx; + + if ((u2 - u1) < 0) + RCLCPP_ERROR(this->get_logger(), "Something MESSED %d %d %d", u1, u2, disp_idx); + + if (u1 < 0) u1 = 0; + if (u1 > (this->width - 1)) u1 = this->width - 1; + + if (u2 < 0) u2 = 0; + if (u2 > (this->width - 1)) u2 = this->width - 1; + + this->table_u[disp_idx][u].idx1 = u1; + this->table_u[disp_idx][u].idx2 = u2; + } + } + + RCLCPP_WARN(this->get_logger(), "Expansion LUT created: LUT MAX: %d , ROBOT SIZE: %f", + lut_max_disparity / 2, this->robot_radius); + this->LUT_ready = true; +} + +void DisparityExpansionNode::process_depth_image( + const sensor_msgs::msg::Image::ConstSharedPtr& msg) { + cv_bridge::CvImagePtr cv_ptr; + try { + cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_32FC1); + } catch (cv_bridge::Exception& e) { + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); + return; + } + + if (cv_ptr->image.empty()) { + RCLCPP_ERROR(this->get_logger(), "Received empty depth image."); + return; + } + cv::Mat disparity_image = this->convert_depth_to_disparity(cv_ptr->image); + + auto disparity_msg = std::make_shared(); + disparity_msg->header = msg->header; + disparity_msg->image = + *cv_bridge::CvImage(msg->header, sensor_msgs::image_encodings::TYPE_32FC1, disparity_image) + .toImageMsg(); + disparity_msg->f = this->fx; + + // Call the existing disparity processing function + this->process_disparity_image(disparity_msg); +} + +cv::Mat DisparityExpansionNode::convert_depth_to_disparity(const cv::Mat& depth_image) { + if (depth_image.empty()) { + RCLCPP_ERROR(this->get_logger(), + "Depth image is empty in depthToDisparity. Raising an error."); + throw std::runtime_error("Depth image is empty in depthToDisparity."); + } + + cv::Mat disparity_image(depth_image.size(), CV_32F); + + // DISPARITY FORMULA + + disparity_image = (this->baseline * this->fx) / (depth_image); // MULT BY 100 + + RCLCPP_INFO_ONCE(this->get_logger(), "Baseline: %.4f", this->baseline); + RCLCPP_INFO_ONCE(this->get_logger(), "Focal length (fx): %.4f", this->fx); + cv::patchNaNs(disparity_image, 0.0f); + + disparity_image.setTo(0.0f, disparity_image == std::numeric_limits::infinity()); + + return disparity_image; +} + +void DisparityExpansionNode::process_disparity_image( + const stereo_msgs::msg::DisparityImage::ConstSharedPtr& msg_disp) { + if (!this->LUT_ready) { + auto& clock = *this->get_clock(); + RCLCPP_INFO_THROTTLE(this->get_logger(), clock, 1, + "LUT not ready yet, not processing disparity"); + return; + } + + auto img_msg = std::make_shared(); + img_msg->header = msg_disp->header; + img_msg->height = msg_disp->image.height; + img_msg->width = msg_disp->image.width; + img_msg->encoding = msg_disp->image.encoding; + img_msg->is_bigendian = msg_disp->image.is_bigendian; + img_msg->step = msg_disp->image.step; + img_msg->data = msg_disp->image.data; + + cv_bridge::CvImagePtr fg_msg(new cv_bridge::CvImage()); + cv_bridge::CvImagePtr bg_msg(new cv_bridge::CvImage()); + + cv_bridge::CvImageConstPtr cv_ptrdisparity; + try { + cv_ptrdisparity = cv_bridge::toCvShare(img_msg, sensor_msgs::image_encodings::TYPE_32FC1); + } catch (cv_bridge::Exception& e) { + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); + return; + } + cv::Mat disparity32F = cv_ptrdisparity->image; + if (disparity32F.empty()) { + RCLCPP_ERROR(this->get_logger(), "Received empty disparity image"); + return; + } + cv::resize(disparity32F, disparity32F, cv::Size(), 1.0 / this->downsample_scale, + 1.0 / this->downsample_scale, cv::INTER_AREA); + if (disparity32F.empty()) { + RCLCPP_ERROR(this->get_logger(), "Resized disparity image is empty"); + return; + } + + cv::Mat disparity_fg; + cv::Mat disparity_bg; + cv::Mat disparity32F_bg; + + try { + disparity32F.copyTo(fg_msg->image); + disparity32F.copyTo(bg_msg->image); + + disparity32F.copyTo(disparity_fg); + disparity32F.copyTo(disparity_bg); + disparity32F.copyTo(disparity32F_bg); + } catch (cv_bridge::Exception& e) { + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); + return; + } + + fg_msg->header = msg_disp->header; + fg_msg->encoding = sensor_msgs::image_encodings::TYPE_32FC1; + bg_msg->header = msg_disp->header; + bg_msg->encoding = sensor_msgs::image_encodings::TYPE_32FC1; + + RCLCPP_INFO_ONCE(this->get_logger(), "IMG TYPE 32FC, GOOD"); + + rclcpp::Time start = this->get_clock()->now(); + + // In a first step, an intermediate disparity image is generated by horizontally expanding all + // disparity values from the stereo disparity map using the u1/u2 look-up table. + + // The first step expands disparities along the image XY axis Figure 3 (right) + for (int v = (int)this->height - 2; (v >= 0); v -= 1) { + for (int u = (int)this->width - 1; u >= 0; u -= 1) { + float disparity_value = disparity32F.at(v, u); + + if (std::isnan(double(disparity_value * this->scale)) || + ((int(disparity_value * this->scale) + 1) >= this->lut_max_disparity) || + ((int(disparity_value * this->scale) + 1) <= 0)) { + RCLCPP_INFO_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), 5000, + "Step 1 Expand u: Disparity out of range: " + << disparity_value << ", lut_max_disparity: " << this->lut_max_disparity + << " Scale: " << this->scale); + continue; + } + + unsigned int u1 = this->table_u[int(disparity_value * this->scale) + 1][u].idx1; + unsigned int u2 = this->table_u[int(disparity_value * this->scale) + 1][u].idx2; + + if (disparity32F.empty()) { + RCLCPP_ERROR(this->get_logger(), "disparity32F matrix is empty."); + return; + } + + // top left corner, then width and height + cv::Rect roi = cv::Rect(u1, v, (u2 - u1), 1); + + cv::Mat submat_t = disparity32F(roi).clone(); + + double min, max; + cv::Point p1, p2; + int min_idx, max_idx; + + cv::minMaxLoc(disparity32F(roi), &min, &max, &p1, &p2); + min_idx = p1.x; + max_idx = p2.x; + float disp_new_fg = max; + float disp_new_bg = min; + float disp_to_depth = this->baseline * this->fx / max; + + cv::Mat submat; + cv::divide(baseline * this->fx, submat_t, submat); + submat = (submat - disp_to_depth); /// this->robot_radius; + + if (this->padding < 0.0) { + float range = this->bg_multiplier * this->robot_radius; + float max_depth = 0.0; + bool found = true; + int ctr = 1; + while (found) { + found = false; + for (int j = 0; j < submat.cols; j++) { + float val = submat.at(0, j); + if (std::isfinite(val)) { + if (val < ctr * range && val > max_depth) { + found = true; + max_depth = val; + } + } + } + ctr++; + } + disp_to_depth += max_depth; + } else + disp_to_depth += this->padding; + + disp_new_bg = this->baseline * this->fx / (disp_to_depth); + disparity_fg(roi).setTo(disp_new_fg); + disparity_bg(roi).setTo(disp_new_bg); + + int u_temp = u1 + max_idx; + if (u_temp >= u) + u = u1; + else + u = u_temp + 1; + } + } + + disparity_fg.copyTo(disparity32F); + disparity_bg.copyTo(disparity32F_bg); + + // In a second step, each pixel in the intermediate disparity image is expanded vertically using + // the v1/v2 look-up table and the expansion column is stored with a new disparity value from + // the dnew look-up table in the final C-space disparity map + for (int u = (int)width - 2; (u >= 0) && true; u -= 1) { + for (int v = (int)height - 1; v >= 0; v -= 1) { + float disparity_value = disparity32F.at(v, u) + + this->pixel_error; // disparity_row[v * row_step + u];// + 0.5; + + if (std::isnan(double(disparity_value * this->scale)) || + ((int(disparity_value * this->scale) + 1) >= this->lut_max_disparity) || + ((int(disparity_value * this->scale) + 1) <= 0)) { + RCLCPP_INFO_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), 5000, + "Step 2 Expand v: Disparity out of range: " + << disparity_value << ", lut_max_disparity: " << this->lut_max_disparity + << " Scale: " << this->scale); + continue; + } + + unsigned int v1 = this->table_v[int(disparity_value * this->scale) + 1][v].idx1; + unsigned int v2 = this->table_v[int(disparity_value * this->scale) + 1][v].idx2; + + cv::Rect roi = cv::Rect(u, v1, 1, (v2 - v1)); + + cv::Mat submat_t = disparity32F_bg(roi).clone(); + + double min, max; + cv::Point p1, p2; + int min_idx, max_idx; + + cv::minMaxLoc(disparity32F(roi), &min, &max, &p1, &p2); + min_idx = p1.y; + max_idx = p2.y; + float disp_new_fg; + float disp_new_bg; + float disp_to_depth = this->baseline * this->fx / max; + + disp_new_fg = this->baseline * this->fx / (disp_to_depth - this->robot_radius) + + this->pixel_error; + + cv::Mat submat; + cv::divide(baseline * this->fx, submat_t, submat); + cv::minMaxLoc(disparity32F_bg(roi), &min, &max, &p1, &p2); + disp_to_depth = this->baseline * this->fx / max; + submat = (submat - disp_to_depth); /// this->robot_radius; + + if (this->padding < 0.0) { + float range = this->bg_multiplier * this->robot_radius; + float max_depth = 0.0; + bool found = true; + int ctr = 1; + while (found) { + found = false; + for (int j = 0; j < submat.rows; j++) { + float val = submat.at(j, 0); + if (std::isfinite(val)) { + if (val < ctr * range && val > max_depth) { + found = true; + max_depth = val; + } + } + } + ctr++; + } + disp_to_depth += max_depth; + } else + disp_to_depth += this->padding; // this->baseline * this->fx/min; + + disp_new_bg = this->baseline * this->fx / (disp_to_depth + this->robot_radius) - + this->pixel_error; + disp_new_bg = disp_new_bg < 0.0 ? 0.0001 : disp_new_bg; + + disparity_fg(roi).setTo(disp_new_fg); + disparity_bg(roi).setTo(disp_new_bg); + + int v_temp = v1 + max_idx; + if (v_temp >= v) + v = v1; + else + v = v_temp + 1; + } + } + + fg_msg->image = disparity_fg; + bg_msg->image = disparity_bg; + + expanded_disparity_fg_pub->publish(*(fg_msg->toImageMsg())); + expanded_disparity_bg_pub->publish(*(bg_msg->toImageMsg())); + + this->publish_frustum(msg_disp); + // only visualize expansion polygon if there's a subscriber + if (expansion_poly_pub->get_subscription_count() > 0) + this->publish_expansion_poly(msg_disp, fg_msg, bg_msg); + // only visualize expansion cloud if there's a subscriber + if (expansion_cloud_pub->get_subscription_count() > 0) + this->publish_expansion_cloud(msg_disp, cv_ptrdisparity, fg_msg, bg_msg); + + return; +} + +void DisparityExpansionNode::publish_frustum( + const stereo_msgs::msg::DisparityImage::ConstSharedPtr& msg) { + visualization_msgs::msg::Marker marker; + auto sensor_frame = msg->header.frame_id; + marker.header.frame_id = sensor_frame; + marker.ns = "frustum"; + marker.id = 0; + marker.type = visualization_msgs::msg::Marker::TRIANGLE_LIST; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose.position.x = marker.pose.position.y = marker.pose.position.z = 0; + tf2::Quaternion q; + q.setRPY(0.0, 0.0, 0.0); + marker.pose.orientation = tf2::toMsg(q); + + marker.scale.x = marker.scale.y = marker.scale.z = 1.; + marker.color.a = 0.3; + marker.color.r = 0.5; + marker.color.g = 1.0; + marker.color.b = 0.9; + + double max_depth = this->lut_max_disparity * this->baseline * this->fx; + // v, u + std::tuple top_left = {0.0, 0.0}; + std::tuple top_right = {0.0, width}; + std::tuple bottom_left = {height, 0.0}; + std::tuple bottom_right = {height, width}; + std::vector> fov = {top_left, top_right, bottom_right, bottom_left}; + + std::vector points_3d; + for (auto& [v, u] : fov) { + geometry_msgs::msg::Point point; + point.x = (u - this->cx) * max_depth / this->fx; + point.y = (v - this->cy) * max_depth / this->fy; + point.z = max_depth; + points_3d.push_back(point); + } + geometry_msgs::msg::Point top_left_p = points_3d.at(0); + geometry_msgs::msg::Point top_right_p = points_3d.at(1); + geometry_msgs::msg::Point bottom_right_p = points_3d.at(2); + geometry_msgs::msg::Point bottom_left_p = points_3d.at(3); + geometry_msgs::msg::Point center_p; // 0,0,0 + + // push back the triangles that make up the frustum. 4 sides, and 2 triangles to make the box in + // left face + marker.points.push_back(top_left_p); + marker.points.push_back(bottom_left_p); + marker.points.push_back(center_p); + + // top face + marker.points.push_back(top_left_p); + marker.points.push_back(top_right_p); + marker.points.push_back(center_p); + + // right face + marker.points.push_back(top_right_p); + marker.points.push_back(bottom_right_p); + marker.points.push_back(center_p); + + // bottom face + marker.points.push_back(bottom_left_p); + marker.points.push_back(bottom_right_p); + marker.points.push_back(center_p); + + // frustum base + // upper right triangle + marker.points.push_back(top_left_p); + marker.points.push_back(top_right_p); + marker.points.push_back(bottom_right_p); + // lower left triangle + marker.points.push_back(top_left_p); + marker.points.push_back(bottom_left_p); + marker.points.push_back(bottom_right_p); + + this->frustum_pub->publish(marker); +} + +void DisparityExpansionNode::publish_expansion_poly( + const stereo_msgs::msg::DisparityImage::ConstSharedPtr& msg_disp, + const cv_bridge::CvImagePtr& fg_msg, const cv_bridge::CvImagePtr& bg_msg) { + visualization_msgs::msg::MarkerArray marker_arr; + visualization_msgs::msg::Marker marker; + marker.header = msg_disp->header; + marker.ns = "occ_space"; + marker.id = 0; + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + + marker.lifetime = rclcpp::Duration::from_seconds(0.5); + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose.position.x = 0; // xyz_centroid[0]; + marker.pose.position.y = 0; // xyz_centroid[1]; + marker.pose.position.z = 0; // xyz_centroid[2]; + + tf2::Quaternion q; + q.setRPY(0.0, 0.0, 0.0); + marker.pose.orientation = tf2::toMsg(q); + + marker.scale.x = marker.scale.y = marker.scale.z = 0.5; + marker.color.a = 0.3; // Don't forget to set the alpha! + + geometry_msgs::msg::PolygonStamped poly; + poly.header = msg_disp->header; + int v = 120; + float prev_depth = 0.0; + // TODO: what is this magic number 239? wtf + for (int v = (int)0; v <= 239; v += 10) { + for (int u = (int)width - 1; u >= 0; u--) { + float depth_value = + this->baseline * this->fx / + fg_msg->image.at(v, u); // free_msg->image.at(v,u)[0]; + float depth_diff = fabs(depth_value - prev_depth); + prev_depth = depth_value; + if (!std::isnan(depth_value) && !std::isinf(depth_value) && depth_diff < 0.5) { + marker.color.r = 1.0 * (fg_msg->image.at(v, u) - this->pixel_error) / + fg_msg->image.at(v, u); + marker.color.g = 1.0 * (this->pixel_error) / fg_msg->image.at(v, u); + geometry_msgs::msg::Point gm_p; + gm_p.x = (u - this->cx) * depth_value / this->fx; + gm_p.y = (v - this->cy) * depth_value / this->fy; + gm_p.z = depth_value; + marker.points.push_back(gm_p); + + depth_value = this->baseline * this->fx / bg_msg->image.at(v, u); + gm_p.x = (u - this->cx) * depth_value / this->fx; + gm_p.y = (v - this->cy) * depth_value / this->fy; + gm_p.z = depth_value; + marker.points.push_back(gm_p); + + } else { + marker_arr.markers.push_back(marker); + marker.points.clear(); + marker.id++; + } + } + } + marker_arr.markers.push_back(marker); + this->expansion_poly_pub->publish(marker_arr); +} + +void DisparityExpansionNode::publish_expansion_cloud( + const stereo_msgs::msg::DisparityImage::ConstSharedPtr& msg_disp, + const cv_bridge::CvImageConstPtr& cv_ptrdisparity, const cv_bridge::CvImagePtr& fg_msg, + const cv_bridge::CvImagePtr& bg_msg) { + PointCloud::Ptr cloud(new PointCloud); + cloud->header.frame_id = msg_disp->header.frame_id; + cloud->height = 1; + cloud->width = 1; + cloud->is_dense = false; + int point_counter = 0; + pcl::PointXYZI pt_fg, pt_bg, pt_free1, pt_free2, pt_real; + + for (int v = (int)this->height - 1; v >= 0; v -= 4) { + for (int u = (int)this->width - 1; u >= 0; u -= 4) { + // foreground + // free_msg->image.at(v,u)[0]; + float depth_value = this->baseline * this->fx / fg_msg->image.at(v, u); + pt_fg.x = (u - this->cx) * depth_value / this->fx; + pt_fg.y = (v - this->cy) * depth_value / this->fy; + pt_fg.z = depth_value; + pt_fg.intensity = 220; + + // background + // free_msg->image.at(v,u)[1]; + depth_value = this->baseline * this->fx / bg_msg->image.at(v, u); + pt_bg.x = (u - this->cx) * depth_value / this->fx; + pt_bg.y = (v - this->cy) * depth_value / this->fy; + pt_bg.z = depth_value; + pt_bg.intensity = 120; + + // actual depth + // new_disparity_bridgePtr->image.at(v,u); + depth_value = this->baseline * this->fx / cv_ptrdisparity->image.at(v, u); + pt_real.x = (u - this->cx) * depth_value / this->fx; + pt_real.y = (v - this->cy) * depth_value / this->fy; + pt_real.z = depth_value; + pt_real.intensity = 170; //*disparity32F.at(v,u)/200; + + point_counter++; + cloud->points.push_back(pt_fg); + point_counter++; + cloud->points.push_back(pt_bg); + point_counter++; + cloud->points.push_back(pt_real); + } + } + cloud->width = point_counter; + + cloud->header.stamp = rclcpp::Time(msg_disp->header.stamp).nanoseconds(); + cloud->header.stamp = pcl_conversions::toPCL(rclcpp::Time(msg_disp->header.stamp)); + + sensor_msgs::msg::PointCloud2 cloud_PC2; + pcl::toROSMsg(*cloud, cloud_PC2); + this->expansion_cloud_pub->publish(cloud_PC2); +} + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/src/disparity_pcd.cpp b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/src/disparity_pcd.cpp new file mode 100644 index 00000000..29d6dbe7 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/src/disparity_pcd.cpp @@ -0,0 +1,309 @@ +/* +* Copyright (c) 2016 Carnegie Mellon University, Author +* +* For License information please see the LICENSE file in the root directory. +* +*/ + +//Outputs a point cloud using disparity images. +/* +#include "ros/ros.h" +#include "sensor_msgs/Image.h" +#include "sensor_msgs/CameraInfo.h" +#include "std_msgs/Header.h" +#include +#include "opencv2/core/core.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +*/ + +#include +#include +#include +#include +//#include +#include +#include + +#include "opencv2/core/core.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/camera_info.hpp" +#include "sensor_msgs/msg/image.hpp" +#include "std_msgs/msg/header.hpp" +#include +#include +#include +#include + +//#include +//#include +//#include +//#include +#include +#include +#include +#include +#include +#include +#include +#include + +typedef pcl::PointCloud PointCloud; + +class disparity_pcd : public rclcpp::Node +{ +public: + disparity_pcd(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + //ros::Subscriber cam_info_sub_; + //ros::Subscriber disparity_sub_; + //ros::Publisher disparity_pcd_pub_; + rclcpp::Subscription::SharedPtr cam_info_sub_; + rclcpp::Subscription::SharedPtr disparity_sub_; + rclcpp::Publisher::SharedPtr disparity_pcd_pub_; + + double baseline; + double downsample_scale; + bool got_cam_info; + + void stereoDisparityCb(const sensor_msgs::msg::Image::ConstSharedPtr msg_disp); + void callback(const sensor_msgs::msg::Image::ConstSharedPtr msg_disp, const sensor_msgs::msg::Image::ConstSharedPtr msg_image); + void getCamInfo(const sensor_msgs::msg::CameraInfo::ConstSharedPtr msg_info); + + double fx_,fy_,cx_,cy_; + unsigned int width,height; +}; + +void disparity_pcd::getCamInfo(const sensor_msgs::msg::CameraInfo::ConstSharedPtr msg_info) +{ + if(got_cam_info) + return; + image_geometry::PinholeCameraModel model_;model_.fromCameraInfo ( msg_info ); + RCLCPP_INFO_ONCE(this->get_logger(), "Cam Info Recvd Fx Fy Cx Cy: %f %f , %f %f",model_.fx(),model_.fy(),model_.cx(),model_.cy()); + cx_ = model_.cx()/downsample_scale; + cy_ = model_.cy()/downsample_scale; + fx_ = fy_ = model_.fx()/downsample_scale; + width = msg_info->width/downsample_scale; + height = msg_info->height/downsample_scale; + baseline = -msg_info->p[3]/msg_info->p[0]; + baseline *=downsample_scale; + got_cam_info = true; +} + +void disparity_pcd::stereoDisparityCb(const sensor_msgs::msg::Image::ConstSharedPtr msg_disp){ + RCLCPP_INFO_ONCE(this->get_logger(),"Disparity CB"); + + rclcpp::Time start = this->get_clock()->now(); + cv_bridge::CvImageConstPtr cv_ptrdisparity; + cv_bridge::CvImagePtr di_msg(new cv_bridge::CvImage()); + + cv::Mat disparity32F; + disparity32F = cv::Mat::zeros(msg_disp->height,msg_disp->width,CV_32FC1); + try + { + cv_ptrdisparity = cv_bridge::toCvShare(msg_disp); + disparity32F = cv_ptrdisparity->image; + } + catch (cv_bridge::Exception& e) + { + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); + return; + } + + if(disparity_pcd_pub_->get_subscription_count() > 0 )//create expansion PCD + { + PointCloud::Ptr cloud (new PointCloud); + cloud->header.frame_id = msg_disp->header.frame_id; + cloud->height = 1; + cloud->width =1; + cloud->is_dense = false; + int point_counter=0; + pcl::PointXYZI pt_; + for ( int v = ( int ) height - 1; v >= 0; v-=4 ) + { + for ( int u = ( int ) width - 1; u >= 0; u-=4 ) + { + + // Fill in XYZ + float depth_value = depth_value = baseline * fx_ / disparity32F.at(v,u);//new_disparity_bridgePtr->image.at(v,u); + pt_.x = ( u - cx_ ) * depth_value / fx_; + pt_.y = ( v - cy_ ) * depth_value / fy_; + pt_.z = depth_value; + pt_.intensity = 170; +// if(fg_msg->image.at(v,u) >= 0.1 && bg_msg->image.at(v,u) >= 0.1 ) +// if(new_disparity_bridgePtr->image.at(v,u) > 0.1) + { +// ROS_INFO("depth: %f",new_disparity_bridgePtr->image.at(v,u)); + point_counter++; + cloud->points.push_back ( pt_ ); + } + } + } + cloud->width = point_counter; + //cloud->header.seq = msg_disp->header.seq; + cloud->header.stamp = rclcpp::Time(msg_disp->header.stamp).nanoseconds(); + cloud->header.stamp = pcl_conversions::toPCL(rclcpp::Time(msg_disp->header.stamp)); + + + sensor_msgs::msg::PointCloud2 cloud_PC2; + pcl::toROSMsg(*cloud,cloud_PC2); + disparity_pcd_pub_->publish(cloud_PC2); + } + +// ROS_INFO("Time: %f \t%f",(ros::Time::now()-start).toSec(),1/(ros::Time::now()-start).toSec()); + return; +} + +void disparity_pcd::callback(const sensor_msgs::msg::Image::ConstSharedPtr msg_disp, const sensor_msgs::msg::Image::ConstSharedPtr msg_image) +{ + if(!got_cam_info) + { + RCLCPP_INFO_THROTTLE(this->get_logger(),*this->get_clock(),1,"Cam Info not received, not processing"); + return; + } + RCLCPP_INFO_ONCE(this->get_logger(),"Disparity CB"); + + rclcpp::Time start = this->get_clock()->now(); + cv_bridge::CvImageConstPtr cv_ptrdisparity; + cv_bridge::CvImageConstPtr cv_ptrImage; + cv_bridge::CvImagePtr di_msg(new cv_bridge::CvImage()); + + cv::Mat disparity32F,image32F; + disparity32F = cv::Mat::zeros(msg_disp->height,msg_disp->width,CV_32FC1); + image32F = cv::Mat::zeros(msg_image->height,msg_image->width,CV_32FC1); + try + { + cv_ptrdisparity = cv_bridge::toCvShare(msg_disp); + disparity32F = cv_ptrdisparity->image; + + cv_ptrImage= cv_bridge::toCvShare(msg_image); + cv_ptrImage->image.convertTo(image32F,CV_32F); + } + catch (cv_bridge::Exception& e) + { + RCLCPP_ERROR(this->get_logger(),"cv_bridge exception: %s", e.what()); + return; + } + + if(disparity_pcd_pub_->get_subscription_count() > 0 )//create expansion PCD + { + PointCloud::Ptr cloud (new PointCloud); + cloud->header.frame_id = msg_disp->header.frame_id; + cloud->height = 1; + cloud->width =1; + cloud->is_dense = false; + int point_counter=0; + pcl::PointXYZI pt_; + for ( int v = ( int ) height - 1; v >= 0; v-=4 ) + { + for ( int u = ( int ) width - 1; u >= 0; u-=4 ) + { + + // Fill in XYZ + float depth_value = depth_value = baseline * fx_ / disparity32F.at(v,u);//new_disparity_bridgePtr->image.at(v,u); + pt_.x = ( u - cx_ ) * depth_value / fx_; + pt_.y = ( v - cy_ ) * depth_value / fy_; + pt_.z = depth_value; + pt_.intensity = image32F.at(v,u); +// if(fg_msg->image.at(v,u) >= 0.1 && bg_msg->image.at(v,u) >= 0.1 ) +// if(new_disparity_bridgePtr->image.at(v,u) > 0.1) + { +// ROS_INFO("depth: %f",new_disparity_bridgePtr->image.at(v,u)); + point_counter++; + cloud->points.push_back ( pt_ ); + } + } + } + cloud->width = point_counter; + //cloud->header.seq = msg_disp->header.seq; + //cloud->header.stamp = pcl_conversions::toPCL(msg_disp->header.stamp); + cloud->header.stamp = rclcpp::Time(msg_disp->header.stamp).nanoseconds(); + cloud->header.stamp = pcl_conversions::toPCL(rclcpp::Time(msg_disp->header.stamp)); + + sensor_msgs::msg::PointCloud2 cloud_PC2; + pcl::toROSMsg(*cloud,cloud_PC2); + disparity_pcd_pub_->publish(cloud_PC2); + } + +// ROS_INFO("Time: %f \t%f",(ros::Time::now()-start).toSec(),1/(ros::Time::now()-start).toSec()); + return; +} + +disparity_pcd::disparity_pcd(const rclcpp::NodeOptions & options) +: Node("disparity_pcd", options) +{ + disparity_pcd_pub_ = this->create_publisher("/pcd", 10); + //cam_info_sub_ = nh.subscribe("/nerian_sp1/right/camera_info", 1,&disparity_pcd::getCamInfo,this); + cam_info_sub_ = this->create_subscription("/nerian_sp1/right/camera_info", 10, std::bind(&disparity_pcd::getCamInfo, this, std::placeholders::_1)); + + //nh.param("downsample_scale", downsample_scale, 1.0); + //ROS_INFO("into constr"); + this->declare_parameter("downsample_scale", 1.0); + this->get_parameter("downsample_scale", downsample_scale); + + RCLCPP_INFO(this->get_logger(), "into constr"); +// disparity_sub_ = nh.subscribe("/disparity", 1,&disparity_pcd::stereoDisparityCb,this); + + // Q-Matrix + // [1.0 0.0 0.0 -317.20617294311523, + // 0.0 1.0 0.0 -233.2914752960205, + // 0.0 0.0 0.0 307.4838344732113, + // 0.0 0.0 1.7930881143612616 -0.0] + +// cx_ = 317.20617294311523; +// cy_ = 233.2914752960205; +// fx_ = fy_ = 307.4838344732113; +// baseline = 0.5576007548439765; +// downsample_scale =1.0; + got_cam_info = false; +// width = 640; +// height = 480; + +// auto disp_sub_ = std::make_shared>(node.get(), "/disparity", 10); +// auto image_sub_ = std::make_shared>(shared_from_this(), "/nerian_sp1/left/image_raw", 10); + +// auto sync = std::make_shared>(*disp_sub_, *image_sub_, 10); +// sync->registerCallback(std::bind(&disparity_pcd::callback, this, std::placeholders::_1, std::placeholders::_2)); +} + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + // cv::initModule_nonfree();//THIS LINE IS IMPORTANT for using surf and sift features of opencv + //ros::NodeHandle nh("~"); + auto node = std::make_shared(rclcpp::NodeOptions()); + + using SyncPolicy = message_filters::sync_policies::ApproximateTime; + auto disp_sub = std::make_shared>( + node.get(), "/nerian_sp1/disparity_map", rmw_qos_profile_default); + auto image_sub = std::make_shared>( + node.get(), "/nerian_sp1/left/image_raw", rmw_qos_profile_default); + + auto sync = std::make_shared>(SyncPolicy(10), + *disp_sub, *image_sub); + + sync->registerCallback( + std::bind(&disparity_pcd::callback, node, std::placeholders::_1, std::placeholders::_2)); + + + //disparity_pcd d(nh); + //message_filters::Subscriber disp_sub(nh, "/disparity", 1); + //message_filters::Subscriber image_sub(nh, "/nerian_sp1/left/image_raw", 1); + //message_filters::TimeSynchronizer sync(disp_sub, image_sub, 10); + //sync.registerCallback(boost::bind(&disparity_pcd::callback,&d, _1, _2)); + + rclcpp::spin(node); + rclcpp::shutdown(); + + return 0; +} diff --git a/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph/CMakeLists.txt b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph/CMakeLists.txt new file mode 100644 index 00000000..8478015a --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph/CMakeLists.txt @@ -0,0 +1,92 @@ +cmake_minimum_required(VERSION 3.8) +project(disparity_graph) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) +find_package(rclcpp REQUIRED) +find_package(pcl_ros REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(image_geometry REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(image_transport REQUIRED) +find_package(visualization_msgs REQUIRED) + +find_package(OpenCV REQUIRED) +find_package(PCL REQUIRED) + +add_library(disparity_graph src/disparity_graph.cpp) +target_compile_features(disparity_graph PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 + +target_include_directories(disparity_graph PUBLIC + $ + $) + +ament_target_dependencies( + disparity_graph + "rclcpp" + "sensor_msgs" + "std_msgs" + "tf2" + "tf2_ros" + "tf2_geometry_msgs" + "geometry_msgs" + "cv_bridge" + "image_geometry" + "nav_msgs" + "image_transport" + "OpenCV" + "visualization_msgs" +) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(disparity_graph PRIVATE "DISPARITY_GRAPH_BUILDING_LIBRARY") + +install( + DIRECTORY include/ + DESTINATION include +) +install( + TARGETS disparity_graph + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_include_directories( + include +) +ament_export_libraries( + disparity_graph +) +ament_export_targets( + export_${PROJECT_NAME} +) + +ament_package() diff --git a/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph/include/disparity_graph/disparity_graph.hpp b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph/include/disparity_graph/disparity_graph.hpp new file mode 100644 index 00000000..f97a6a0d --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph/include/disparity_graph/disparity_graph.hpp @@ -0,0 +1,424 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace disparity_graph { + +class DisparityGraph { + struct DisparityGraphNode { + cv_bridge::CvImagePtr Im_fg; + cv_bridge::CvImagePtr Im_bg; + std_msgs::msg::Header header; + tf2::Transform w2s_tf; + tf2::Transform s2w_tf; + }; + std::deque disp_graph_; + + rclcpp::Node::SharedPtr node_ptr; + + size_t graph_size_; + double thresh_; + std::shared_ptr tf_buffer_ptr; + std::string sensor_frame, fixed_frame, stabilized_frame; + visualization_msgs::msg::Marker marker_; + rclcpp::Publisher::SharedPtr disparity_graph_marker_pub_; + double angle_tol, displacement_tol, assume_seen_within_radius; + bool first; + bool got_cam_info; + double fx_, fy_, cx_, cy_, baseline_, downsample_scale; + unsigned int width_, height_; + + std::mutex io_mutex; + + rclcpp::Subscription::SharedPtr cam_info_sub_; + + message_filters::Subscriber disp_fg_sub_, disp_bg_sub_; + typedef message_filters::sync_policies::ExactTime + ExactPolicy; + typedef message_filters::Synchronizer ExactSync; + + std::shared_ptr exact_sync; + + public: + DisparityGraph() + : graph_size_(10), + angle_tol(30.0), + displacement_tol(1.5), + first(true), + got_cam_info(false) { + } + + // we need this separate initialization function because we're taking a node_ptr. shared_from_this() only works after constructor exits. + // ideally we switch to use nodelets so there's a proper setup/activate phase + void initialize(const rclcpp::Node::SharedPtr &node_ptr, + const std::shared_ptr tf_buffer_ptr) { + RCLCPP_INFO(node_ptr->get_logger(), "DisparityGraph initialize called"); + this->node_ptr = node_ptr; + this->tf_buffer_ptr = tf_buffer_ptr; + disparity_graph_marker_pub_ = + node_ptr->create_publisher("disparity_graph", 10); + + node_ptr->declare_parameter("baseline_fallback", 0.5); + + node_ptr->declare_parameter("fixed_frame", "map"); + node_ptr->get_parameter("fixed_frame", this->fixed_frame); + node_ptr->declare_parameter("stabilized_frame", "base_link_stabilized"); + node_ptr->get_parameter("stabilized_frame", this->stabilized_frame); + node_ptr->declare_parameter("assume_seen_within_radius", 1.0); + node_ptr->get_parameter("assume_seen_within_radius", assume_seen_within_radius); + node_ptr->declare_parameter("downsample_scale", 1.0); + node_ptr->get_parameter("downsample_scale", downsample_scale); + node_ptr->declare_parameter("low_occ_thresh", 0.9); + node_ptr->get_parameter("low_occ_thresh", thresh_); + node_ptr->declare_parameter("graph_size", 10); + node_ptr->get_parameter("graph_size", graph_size_); + node_ptr->declare_parameter("displacement_tolerance", 1.5); + node_ptr->get_parameter("displacement_tolerance", displacement_tol); + node_ptr->declare_parameter("angle_tolerance", 30.0); + angle_tol = node_ptr->get_parameter("angle_tolerance").as_double() * M_PI / 180.0; + + node_ptr->declare_parameter("expanded_disparity_fg_topic", "expanded_disparity_fg"); + disp_fg_sub_.subscribe(node_ptr, + node_ptr->get_parameter("expanded_disparity_fg_topic").as_string()); + node_ptr->declare_parameter("expanded_disparity_bg_topic", "expanded_disparity_bg"); + disp_bg_sub_.subscribe(node_ptr, + node_ptr->get_parameter("expanded_disparity_bg_topic").as_string()); + + exact_sync.reset(new ExactSync(ExactPolicy(50), disp_fg_sub_, disp_bg_sub_)); + exact_sync->registerCallback(std::bind(&DisparityGraph::disp_cb, this, + std::placeholders::_1, std::placeholders::_2)); + + node_ptr->declare_parameter("camera_info_topic", "camera_info"); + cam_info_sub_ = node_ptr->create_subscription( + node_ptr->get_parameter("camera_info_topic").as_string(), 1, + std::bind(&DisparityGraph::get_cam_info, this, std::placeholders::_1)); + + marker_.header.frame_id = this->fixed_frame; + marker_.ns = "disparityGraph"; + marker_.id = 0; + marker_.type = visualization_msgs::msg::Marker::ARROW; + marker_.action = visualization_msgs::msg::Marker::ADD; + marker_.pose.position.x = 1; + marker_.pose.position.y = 1; + marker_.pose.position.z = 1; + marker_.pose.orientation.x = 0.0; + marker_.pose.orientation.y = 0.0; + marker_.pose.orientation.z = 0.0; + marker_.pose.orientation.w = 1.0; + marker_.scale.x = 1; + marker_.scale.y = 0.2; + marker_.scale.z = 0.2; + marker_.color.a = 1.0; // Don't forget to set the alpha! + marker_.color.r = 0.0; + marker_.color.g = 1.0; + marker_.color.b = 0.0; + RCLCPP_INFO(node_ptr->get_logger(), "Disparity Graph Node Started"); + } + + void disp_cb(const sensor_msgs::msg::Image::ConstSharedPtr &disp_fg, + const sensor_msgs::msg::Image::ConstSharedPtr &disp_bg) { + // RCLCPP_INFO(node_ptr->get_logger(), "Received disp stamp: %lf", + // (node_ptr->get_clock()->now() - disp_fg->header.stamp).seconds()); + this->sensor_frame = disp_fg->header.frame_id; + + tf2::Stamped transform; + try { + auto tf_msg = + tf_buffer_ptr->lookupTransform(this->sensor_frame, this->fixed_frame, disp_fg->header.stamp); + tf2::fromMsg(tf_msg, transform); + } catch (tf2::TransformException &ex) { + RCLCPP_ERROR(node_ptr->get_logger(), "DG disp_cb %s", ex.what()); + return; + } + + std::scoped_lock lock(io_mutex); + + DisparityGraphNode new_graph_node; + + new_graph_node.header = disp_bg->header; + new_graph_node.w2s_tf = transform; + new_graph_node.s2w_tf = transform.inverse(); + try { + new_graph_node.Im_fg = + cv_bridge::toCvCopy(disp_fg, sensor_msgs::image_encodings::TYPE_32FC1); + new_graph_node.Im_bg = + cv_bridge::toCvCopy(disp_bg, sensor_msgs::image_encodings::TYPE_32FC1); + } catch (const std::exception &ex) { + RCLCPP_ERROR(node_ptr->get_logger(), "\n\n\n\n\n\t\t\tDEQUE ERR\n\n\n\n: %s", + ex.what()); + return; + } + + if (first) { + // if first, initialize the graph by adding the node to the front and back of the queue + first = false; + disp_graph_.push_front(new_graph_node); + disp_graph_.push_back(new_graph_node); + + } else { + tf2::Vector3 new_position = new_graph_node.s2w_tf.getOrigin(); + std::deque::iterator it, end; + it = disp_graph_.begin(); + end = disp_graph_.end() - 1; + tf2::Vector3 graph_position = it->s2w_tf.getOrigin(); + tf2Scalar angle_difference = tf2::angleShortestPath(new_graph_node.s2w_tf.getRotation(), + it->s2w_tf.getRotation()); + tf2Scalar position_difference = new_position.distance(graph_position); + + if (fabs(angle_difference) >= angle_tol || + fabs(position_difference) > displacement_tol) { + RCLCPP_INFO(node_ptr->get_logger(), "Adding new node, graph size now %d", + (int)disp_graph_.size()); + + if (disp_graph_.size() >= graph_size_) { + disp_graph_.erase(end); + } + + if (disp_graph_.size() <= graph_size_) { + disp_graph_.push_front(new_graph_node); + } + } + disp_graph_.pop_back(); + disp_graph_.push_back(new_graph_node); + } + + publish_disparity_graph_marker(); + } + + void publish_disparity_graph_marker() { + // create marker + visualization_msgs::msg::MarkerArray marker_arr; + + for (uint i = 0; i < disp_graph_.size(); i++) { + auto position = disp_graph_.at(i).s2w_tf.getOrigin(); + tf2::toMsg(position, marker_.pose.position); + auto rotation = disp_graph_.at(i).s2w_tf.getRotation(); + marker_.pose.orientation = tf2::toMsg(rotation); + marker_.header.stamp = node_ptr->now(); + marker_.ns = "pose"; + marker_.id = i; + marker_arr.markers.push_back(marker_); + } + + disparity_graph_marker_pub_->publish(marker_arr); + } + + void get_cam_info(const sensor_msgs::msg::CameraInfo::ConstSharedPtr &cam_info_msg) { + if (got_cam_info) { + return; + } + image_geometry::PinholeCameraModel model; + model.fromCameraInfo(cam_info_msg); + // print + RCLCPP_INFO(node_ptr->get_logger(), + "Cam Info Recvd Fx Fy Cx Cy: %f %f , %f %f Baseline: %f", model.fx(), + model.fy(), model.cx(), model.cy(), baseline_); + cx_ = model.cx() / downsample_scale; + cy_ = model.cy() / downsample_scale; + fx_ = fy_ = model.fx() / downsample_scale; + width_ = cam_info_msg->width / downsample_scale; + height_ = cam_info_msg->height / downsample_scale; + double baseline = -cam_info_msg->p[3] / cam_info_msg->p[0]; + baseline_ = baseline; + if (this->baseline_ == 0.0) { + node_ptr->get_parameter("baseline_fallback", this->baseline_); + RCLCPP_ERROR_STREAM_ONCE( + node_ptr->get_logger(), + "Baseline from camera_info was 0, setting to baseline_fallback:" + << this->baseline_); + } + baseline_ *= downsample_scale; + RCLCPP_INFO(node_ptr->get_logger(), + "Transformed Cam Info Recvd Fx Fy Cx Cy: %f %f, %f %f Baseline: %f with " + "downsamplescale: %f", + model.fx(), model.fy(), model.cx(), model.cy(), baseline_, downsample_scale); + + RCLCPP_WARN(node_ptr->get_logger(), + "Cam Info Constants - Fx Fy Cx Cy: %f %f, %f %f / width=%d - height=%d", fx_, + fy_, cx_, cy_, width_, height_); + + got_cam_info = true; + } + + /** + * @brief checks if the pose is seen by any of the disparity images in the graph and if it is, + * check if it is free + * + * @param pose_to_check the pose to check + * @param thresh threshold for occupancy + * @return std::tuple is_seen, is_free, occupancy value [0, inf] + */ + std::tuple is_pose_seen_and_free( + geometry_msgs::msg::PoseStamped pose_to_check, double thresh) { + double occupancy = 0.0; // init + geometry_msgs::msg::PointStamped checked_point_stamped; + checked_point_stamped.point = pose_to_check.pose.position; + checked_point_stamped.header = pose_to_check.header; + + geometry_msgs::msg::PointStamped world_point_stamped; + + try { + tf_buffer_ptr->transform(checked_point_stamped, world_point_stamped, this->fixed_frame); + } catch (tf2::TransformException &ex) { + RCLCPP_ERROR(node_ptr->get_logger(), "Could not transform %s to %s: %s", + this->fixed_frame.c_str(), pose_to_check.header.frame_id.c_str(), ex.what()); + return {false, false, 0.0}; + } + + tf2::Vector3 optical_point; + tf2::fromMsg(world_point_stamped.point, optical_point); + + std::scoped_lock lock(io_mutex); + + bool is_free = true; // whether the point is unoccupied. we will go through all disparity + // graph nodes to check + bool is_seen = false; // whether the point is is_seen by any of the disparity images + + if (disp_graph_.size() == 0) { + RCLCPP_WARN_STREAM_ONCE( + node_ptr->get_logger(), + "No disparity images in graph, can't see anything, everything is invalid"); + } + + for (const auto &graph_node : disp_graph_) { + tf2::Vector3 local_point = graph_node.w2s_tf * optical_point; + + float x = local_point.getX(); + float y = local_point.getY(); + float z = local_point.getZ(); + + // RCLCPP_INFO_STREAM(node_ptr->get_logger(), "Local Point: " << x << " " << y << " " << + // z); + + if (local_point.length() < this->assume_seen_within_radius) { + is_seen = true; // assume the point is seen if it is close to the camera + continue; + } + + // project the 3D point to the image plane + int u = x / z * fx_ + cx_; + int v = y / z * fy_ + cy_; + + // check that the point is within the image bounds, and that it is in front of the + // camera + if (u >= 0 && u < width_ && v >= 0 && v < height_ && z > 0.0) { + is_seen = true; + + double state_disparity = baseline_ * fx_ / z; + // RCLCPP_INFO_STREAM(node_ptr->get_logger(), "State Disparity: " << + // state_disparity); + + // see Table 1 of Dubey et al. 2017 DROAN. it seems a little different + bool is_state_between_fg_and_bg = + (state_disparity < graph_node.Im_fg->image.at(v, u)) && + (state_disparity > graph_node.Im_bg->image.at(v, u)); + if (is_state_between_fg_and_bg) { + // if the disparity point lies between the disparity images, it is occupied and + // we add to the occupancy value. + // TODO: what does this value mean? is it the sensor model? It seems to be a + // heuristic based on distance. also shouldn't we clamp this at 1.0? or is it + // not really a probability? + occupancy += (state_disparity - 0.5) / state_disparity; + } else { + // otherwise it's outside, we subtdroan.rvizract from the occupancy value + occupancy -= 0.5 * (state_disparity - 0.5) / state_disparity; + occupancy = std::clamp(occupancy, 0.0, 1.0); + } + } + if (occupancy >= thresh) { + is_free = false; + break; + } + } + // RCLCPP_INFO_STREAM(node_ptr->get_logger(),"occupancy:" << occupancy << " is_free: " << + // is_free << " is_seen: " << is_seen); + + return {is_seen, is_free, occupancy}; + } + + bool get_state_cost(geometry_msgs::msg::Pose checked_state, double &cost) { + geometry_msgs::msg::PointStamped checked_point_stamped; + checked_point_stamped.header.frame_id = "world"; + checked_point_stamped.header.stamp = node_ptr->now(); // - rclcpp::Duration(0.1); + checked_point_stamped.point = checked_state.position; + + geometry_msgs::msg::PointStamped world_point_stamped; + + try { + tf_buffer_ptr->transform(checked_point_stamped, world_point_stamped, this->fixed_frame); + } catch (tf2::TransformException &ex) { + RCLCPP_ERROR(node_ptr->get_logger(), "TF to fixed frame failed: %s", ex.what()); + return false; + } + + tf2::Vector3 optical_point; + tf2::fromMsg(world_point_stamped.point, optical_point); + + std::lock_guard lock(io_mutex); + for (uint i = 0; i < disp_graph_.size(); i++) { + tf2::Vector3 local_point = disp_graph_.at(i).w2s_tf * optical_point; + + float x, y, z; + + x = local_point.getX(); + y = local_point.getY(); + z = local_point.getZ(); + + if (z < 0.0) { + return false; + } + + int u = x / z * fx_ + cx_; + int v = y / z * fy_ + cy_; + + if (u >= 0 && u < width_ && v >= 0 && v < height_ && z > 0.0) { + double state_disparity = baseline_ * fx_ / z; + if ((disp_graph_.at(i).Im_fg->image.at(v, u) > state_disparity) && + (disp_graph_.at(i).Im_bg->image.at(v, u) < state_disparity)) { + cost += (state_disparity - 0.5) / state_disparity; + } + cost = cost < 0.0 ? 0.0 : cost; + + if (cost > 100.0) { + cost = 100.0; + return false; + } + } + } + return true; + } + + void clear_graph(void) { + std::lock_guard lock(io_mutex); + disp_graph_.clear(); + first = true; + RCLCPP_INFO_STREAM(node_ptr->get_logger(), "<<< DISP GRAPH CLEARED >>>"); + }; + + rclcpp::Publisher::SharedPtr pcdPub, occPub_; + + rclcpp::Publisher::SharedPtr expansion_poly_pub; + float orig_z; +}; + +} // namespace disparity_graph diff --git a/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph/package.xml b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph/package.xml new file mode 100644 index 00000000..481d5a5c --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph/package.xml @@ -0,0 +1,30 @@ + + + + disparity_graph + 0.0.0 + TODO: Package description + andrew + TODO: License declaration + + ament_cmake_ros + + rclcpp + sensor_msgs + std_msgs + tf2 + tf2_ros + tf2_geometry_msgs + geometry_msgs + cv_bridge + image_geometry + nav_msgs + image_transport + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph/src/disparity_graph.cpp b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph/src/disparity_graph.cpp new file mode 100644 index 00000000..2e1006d0 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph/src/disparity_graph.cpp @@ -0,0 +1,6 @@ +#include "disparity_graph/disparity_graph.hpp" + +namespace disparity_graph +{ + +} // namespace disparity_graph diff --git a/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_map_representation/CMakeLists.txt b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_map_representation/CMakeLists.txt new file mode 100644 index 00000000..7b6cfa2d --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_map_representation/CMakeLists.txt @@ -0,0 +1,109 @@ +cmake_minimum_required(VERSION 3.5) +project(disparity_map_representation) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + + +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) +find_package(airstack_common REQUIRED) +find_package(airstack_msgs REQUIRED) +find_package(map_representation_interface REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(disparity_graph REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(image_geometry REQUIRED) +find_package(image_transport REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(pcl_msgs REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclpy REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(stereo_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(trajectory_library REQUIRED) + + +add_library(disparity_map_representation src/disparity_map_representation.cpp) +target_compile_features(disparity_map_representation PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +target_include_directories(disparity_map_representation PUBLIC + $ + $) + +ament_target_dependencies( + disparity_map_representation + airstack_common + airstack_msgs + map_representation_interface + cv_bridge + disparity_graph + geometry_msgs + image_geometry + image_transport + nav_msgs + pcl_msgs + pluginlib + rclcpp + rclpy + sensor_msgs + std_msgs + stereo_msgs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + visualization_msgs + trajectory_library + ) + +pluginlib_export_plugin_description_file(map_representation_interface disparity_map_representation_plugin.xml) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +#target_compile_definitions(disparity_map_representation PRIVATE "disparity_map_representation_INTERFACE_BUILDING_LIBRARY") + +install( + DIRECTORY include/ + DESTINATION include +) +install( + TARGETS disparity_map_representation + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_include_directories( + include +) +ament_export_libraries( + disparity_map_representation +) +ament_export_targets( + export_${PROJECT_NAME} +) + +ament_package() diff --git a/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_map_representation/disparity_map_representation_plugin.xml b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_map_representation/disparity_map_representation_plugin.xml new file mode 100644 index 00000000..1513218b --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_map_representation/disparity_map_representation_plugin.xml @@ -0,0 +1,6 @@ + + + Disparity map representation plugin. + + \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_map_representation/include/disparity_map_representation/disparity_map_representation.hpp b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_map_representation/include/disparity_map_representation/disparity_map_representation.hpp new file mode 100644 index 00000000..bc59ef45 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_map_representation/include/disparity_map_representation/disparity_map_representation.hpp @@ -0,0 +1,82 @@ +#ifndef _DISPARITY_MAP_REPRESENTATION_ +#define _DISPARITY_MAP_REPRESENTATION_ +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace disparity_map_representation { +class DisparityMapRepresentation : public map_representation_interface::MapRepresentation { + private: + disparity_graph::DisparityGraph disp_graph; + + visualization_msgs::msg::MarkerArray marker_array; + visualization_msgs::msg::Marker points_marker; + std_msgs::msg::ColorRGBA green; + std_msgs::msg::ColorRGBA gray; + std_msgs::msg::ColorRGBA red; + + int obstacle_check_num_points; + double obstacle_check_radius; + tf2::Quaternion Q_UP, Q_DOWN, Q_LEFT, Q_RIGHT; + std::set Q_DIRECTIONS; + + tf2::Vector3 quaternion_to_unit_vector(tf2::Quaternion q) { + tf2::Vector3 unit(1, 0, 0); + return tf2::Transform(q) * unit; + } + + auto create_side_and_up_vectors(tf2::Vector3 direction) { + // Choose an arbitrary up vector + tf2::Vector3 forward = direction.normalized(); + tf2::Vector3 arbitrary_up(0.0, 0.0, 1.0); + + // If forward is parallel to arbitrary_up, choose a different up vector + if (fabs(forward.dot(arbitrary_up)) > 0.999) { + arbitrary_up = tf2::Vector3(0.0, 1.0, 0.0); + } + + // Side vector (perpendicular to both forward and arbitrary up) + tf2::Vector3 side = forward.cross(arbitrary_up).normalized(); + + // True up vector (perpendicular to both forward and side) + tf2::Vector3 up = side.cross(forward).normalized(); + + return std::make_pair(side, up); + } + void check_pose_and_add_marker(const Trajectory& trajectory, const Waypoint& waypoint, + double dist, const tf2::Vector3 direction, + double& closest_obstacle_distance); +tf2::Vector3 determine_waypoint_direction(const Trajectory& trajectory, + const Waypoint& waypoint, + size_t waypoint_index) ; + + public: + DisparityMapRepresentation(); + + virtual const visualization_msgs::msg::MarkerArray& get_debug_markerarray() const override; + + virtual std::vector > get_values( + const std::vector& trajectories) override; + + virtual void initialize(const rclcpp::Node::SharedPtr& node_ptr, + const std::shared_ptr tf_buffer_ptr) override; +}; +} // namespace disparity_map_representation + +#endif diff --git a/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_map_representation/package.xml b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_map_representation/package.xml new file mode 100644 index 00000000..3a7c5fe5 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_map_representation/package.xml @@ -0,0 +1,38 @@ + + + + disparity_map_representation + 0.0.0 + The disparity_map_representation package + + john + TODO + + ament_cmake + + airstack_common + airstack_msgs + map_representation_interface + cv_bridge + disparity_graph + geometry_msgs + nav_msgs + pluginlib + rclcpp + rclpy + sensor_msgs + std_msgs + stereo_msgs + tf2 + tf2_ros + tf2_geometry_msgs + visualization_msgs + trajectory_library + + ament_lint_auto + ament_lint_common + + + ament_cmake + + \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_map_representation/src/disparity_map_representation.cpp b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_map_representation/src/disparity_map_representation.cpp new file mode 100644 index 00000000..c78209d4 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_map_representation/src/disparity_map_representation.cpp @@ -0,0 +1,164 @@ +#include +namespace disparity_map_representation { +DisparityMapRepresentation::DisparityMapRepresentation() : MapRepresentation(), disp_graph() { + points_marker.ns = "obstacles"; + points_marker.id = 0; + points_marker.type = visualization_msgs::msg::Marker::SPHERE_LIST; + points_marker.action = visualization_msgs::msg::Marker::ADD; + points_marker.scale.x = 0.03; + points_marker.scale.y = 0.03; + points_marker.scale.z = 0.03; + + green.r = 0; + green.b = 0; + green.g = 1; + green.a = 0.8; + gray.r = 0.7; + gray.b = 0.7; + gray.g = 0.7; + gray.a = 0.6; + red.r = 1; + red.b = 0; + red.g = 0; + red.a = 0.6; + + Q_UP.setRPY(0, -M_PI / 2, 0); + Q_DOWN.setRPY(0, M_PI / 2, 0); + Q_LEFT.setRPY(0, 0, M_PI / 2); + Q_RIGHT.setRPY(0, 0, -M_PI / 2); + Q_DIRECTIONS = {Q_UP, Q_DOWN, Q_LEFT, Q_RIGHT}; +} + +void DisparityMapRepresentation::initialize(const rclcpp::Node::SharedPtr& node_ptr, + const std::shared_ptr tf_buffer_ptr) { + RCLCPP_INFO(node_ptr->get_logger(), "DisparityMapRepresentation initialize called"); + MapRepresentation::initialize(node_ptr, tf_buffer_ptr); + node_ptr->declare_parameter("obstacle_check_num_points", 69); + node_ptr->get_parameter("obstacle_check_num_points", this->obstacle_check_num_points); + RCLCPP_INFO_STREAM(node_ptr->get_logger(), + "obstacle_check_num_points: " << this->obstacle_check_num_points); + node_ptr->get_parameter("obstacle_check_radius", this->obstacle_check_radius); + RCLCPP_INFO_STREAM(node_ptr->get_logger(), + "obstacle_check_radius: " << this->obstacle_check_radius); + disp_graph.initialize(node_ptr, tf_buffer_ptr); +} + +tf2::Vector3 DisparityMapRepresentation::determine_waypoint_direction(const Trajectory& trajectory, + const Waypoint& waypoint, + size_t waypoint_index) { + tf2::Vector3 waypoint_direction; + if (trajectory.get_num_waypoints() == 1) { + // edge case: only 1 waypoint, make direction match the waypoint's heading + waypoint_direction = quaternion_to_unit_vector(waypoint.quaternion()); + } else { + if (&waypoint == &(trajectory.get_waypoint(0))) { + // edge case: first waypoint, direction is from waypoint 0 to waypoint 1 + waypoint_direction = trajectory.get_waypoint(1).position() - waypoint.position(); + } else { + // otherwise direction is from previous waypoint to current waypoint + waypoint_direction = + waypoint.position() - trajectory.get_waypoint(waypoint_index - 1).position(); + } + } + return waypoint_direction; +} + +std::vector > DisparityMapRepresentation::get_values( + const std::vector &trajectories) { + marker_array.markers.clear(); + points_marker.points.clear(); + points_marker.colors.clear(); + + // initialize values to 0 + std::vector > values(trajectories.size()); + for (size_t i = 0; i < trajectories.size(); i++) { + values[i] = std::vector(trajectories[i].get_num_waypoints(), 0.0); + } + + size_t trajectory_index = -1; + for (const Trajectory& trajectory : trajectories) { + ++trajectory_index; + points_marker.header.frame_id = trajectory.get_frame_id(); + + size_t waypoint_index = -1; + for (const Waypoint& waypoint : trajectory.get_waypoints()) { + ++waypoint_index; + // find the direction of the current trajectory segment between waypoints + // this direction will be used to check for obstacles in a perpendicular direction. + // i.e. left, right, up, down + tf2::Vector3 waypoint_direction = + determine_waypoint_direction(trajectory, waypoint, waypoint_index); + + auto [side, up] = create_side_and_up_vectors(waypoint_direction); + + std::set direction_vectors = {up, side, -up, -side}; + + double closest_obstacle_distance = std::numeric_limits::infinity(); + for (auto direction : direction_vectors) { + for (int k = 1; k < obstacle_check_num_points + 1; k++) { + double dist = k * obstacle_check_radius / (obstacle_check_num_points + 1); + + check_pose_and_add_marker(trajectory, waypoint, dist, direction, + closest_obstacle_distance); + } + } + + // check the waypoint itself + check_pose_and_add_marker(trajectory, waypoint, 0, waypoint_direction, + closest_obstacle_distance); + + values[trajectory_index][waypoint_index] = closest_obstacle_distance; + } + } + + points_marker.header.stamp = node_ptr->now(); + marker_array.markers.push_back(points_marker); + + return values; +} + +void DisparityMapRepresentation::check_pose_and_add_marker(const Trajectory& trajectory, + const Waypoint& waypoint, double dist, + const tf2::Vector3 direction, + double& closest_obstacle_distance) { + tf2::Vector3 point_to_check = waypoint.position() + dist * direction; + geometry_msgs::msg::PoseStamped pose_to_check; + pose_to_check.header.frame_id = trajectory.get_frame_id(); + pose_to_check.header.stamp = trajectory.get_stamp(); + pose_to_check.pose.position.x = point_to_check.x(); + pose_to_check.pose.position.y = point_to_check.y(); + pose_to_check.pose.position.z = point_to_check.z(); + pose_to_check.pose.orientation.w = 1.0; + // RCLCPP_INFO_STREAM(node_ptr->get_logger(), + // "check_pose: " << check_pose.pose.position.x << " " + // << check_pose.pose.position.y << " " + // << check_pose.pose.position.z); + + std_msgs::msg::ColorRGBA color; + + auto [is_seen, is_free, occupancy] = disp_graph.is_pose_seen_and_free(pose_to_check, 0.9); + if (is_seen && is_free) { + color = green; + } else { + closest_obstacle_distance = std::min(dist, closest_obstacle_distance); + if (!is_seen) { + color = gray; + } else if (!is_free) { + color = red; + } + } + + points_marker.points.push_back(pose_to_check.pose.position); + points_marker.colors.push_back(color); +} + +const visualization_msgs::msg::MarkerArray& DisparityMapRepresentation::get_debug_markerarray() + const { + return marker_array; +} + +} // namespace disparity_map_representation +#include + +PLUGINLIB_EXPORT_CLASS(disparity_map_representation::DisparityMapRepresentation, + map_representation_interface::MapRepresentation) diff --git a/robot/ros_ws/src/autonomy/3_local/a_world_models/map_representation_interface/CMakeLists.txt b/robot/ros_ws/src/autonomy/3_local/a_world_models/map_representation_interface/CMakeLists.txt new file mode 100644 index 00000000..8c176d6b --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/a_world_models/map_representation_interface/CMakeLists.txt @@ -0,0 +1,80 @@ +cmake_minimum_required(VERSION 3.5) +project(map_representation_interface) + +# Find dependencies +find_package(ament_cmake REQUIRED) +find_package(airstack_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclpy REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(trajectory_library REQUIRED) + + +add_library(map_representation_interface src/map_representation_interface.cpp) +target_include_directories(map_representation_interface PUBLIC + $ + $) +target_compile_features(map_representation_interface PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +ament_target_dependencies( + map_representation_interface + # Required dependencies + airstack_msgs + geometry_msgs + nav_msgs + pluginlib + rclcpp + rclpy + sensor_msgs + std_msgs + tf2 + tf2_ros + visualization_msgs + trajectory_library +) +ament_export_targets(map_representation_interface HAS_LIBRARY_TARGET) +ament_export_dependencies(rclcpp geometry_msgs tf2 tf2_ros) + +install(TARGETS map_representation_interface + DESTINATION lib/${PROJECT_NAME}) + +install( + DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS map_representation_interface + EXPORT map_representation_interface + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + +ament_export_include_directories( + include + ${geometry_msgs_INCLUDE_DIRS} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/robot/ros_ws/src/autonomy/3_local/a_world_models/map_representation_interface/include/map_representation_interface/map_representation.hpp b/robot/ros_ws/src/autonomy/3_local/a_world_models/map_representation_interface/include/map_representation_interface/map_representation.hpp new file mode 100644 index 00000000..b2bd4a8b --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/a_world_models/map_representation_interface/include/map_representation_interface/map_representation.hpp @@ -0,0 +1,53 @@ +#ifndef _CORE_MAP_REPRESENTATION_H_ +#define _CORE_MAP_REPRESENTATION_H_ + +#include +#include + +#include +#include +#include +#include +#include + +namespace map_representation_interface { + +class MapRepresentation { + protected: + rclcpp::Node::SharedPtr node_ptr; + std::shared_ptr tf_buffer_ptr; + MapRepresentation() {} + + public: + /** + Takes in a list of trajectories and outputs a value for each waypoint in each trajectory. + @return A vector of vectors containing values for each waypoint. There is a vector of vectors + for each trajectory. There is a vector of doubles for each waypoint within a trajectory. + */ + + virtual std::vector > get_values(const std::vector &trajectories) = 0; + /** + Clears the map. + */ + virtual void clear() { + RCLCPP_ERROR(node_ptr->get_logger(), "clear CALLED BUT NOT IMPLEMENTED"); + } + + /** + Use this function to publish visualizations of the map that might be helpful for debugging. + */ + virtual const visualization_msgs::msg::MarkerArray& get_debug_markerarray() const {} + + virtual ~MapRepresentation() {} + + virtual void initialize(const rclcpp::Node::SharedPtr& node_ptr, + const std::shared_ptr tf_buffer_ptr) { + RCLCPP_INFO(node_ptr->get_logger(), "MapRepresentation initialize called"); + this->node_ptr = node_ptr; + this->tf_buffer_ptr = tf_buffer_ptr; + } +}; + +} // namespace map_representation_interface + +#endif diff --git a/robot/ros_ws/src/autonomy/3_local/a_world_models/map_representation_interface/package.xml b/robot/ros_ws/src/autonomy/3_local/a_world_models/map_representation_interface/package.xml new file mode 100644 index 00000000..d2da32b6 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/a_world_models/map_representation_interface/package.xml @@ -0,0 +1,32 @@ + + + + map_representation_interface + 0.0.0 + The map_representation_interface package + + john + TODO + + ament_cmake + + geometry_msgs + nav_msgs + rclcpp + rclpy + sensor_msgs + std_msgs + tf2 + tf2_ros + visualization_msgs + pluginlib + airstack_msgs + trajectory_library + + ament_lint_auto + ament_lint_common + + + ament_cmake + + \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/3_local/a_world_models/map_representation_interface/src/map_representation_interface.cpp b/robot/ros_ws/src/autonomy/3_local/a_world_models/map_representation_interface/src/map_representation_interface.cpp new file mode 100644 index 00000000..9ace4abe --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/a_world_models/map_representation_interface/src/map_representation_interface.cpp @@ -0,0 +1,3 @@ +#include + +int blank() { return 0; } diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/droan_local_planner/CMakeLists.txt b/robot/ros_ws/src/autonomy/3_local/b_planners/droan_local_planner/CMakeLists.txt new file mode 100644 index 00000000..5e8c571c --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/droan_local_planner/CMakeLists.txt @@ -0,0 +1,72 @@ +cmake_minimum_required(VERSION 3.8) +project(droan_local_planner) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) + +find_package(airstack_common REQUIRED) +find_package(airstack_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(map_representation_interface REQUIRED) +# find_package(disparity_map_representation REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(trajectory_controller REQUIRED) +find_package(trajectory_library REQUIRED) + +add_executable(droan_local_planner src/droan_local_planner.cpp) +target_include_directories(droan_local_planner PUBLIC + $ + $) +target_compile_features(droan_local_planner PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 + +ament_target_dependencies( + droan_local_planner + "airstack_msgs" + "airstack_common" + "nav_msgs" + "map_representation_interface" + # "disparity_map_representation" + "trajectory_controller" + "trajectory_library" + "pluginlib" + "rclcpp" + "std_msgs" + "tf2" + "tf2_ros" +) + +install(TARGETS droan_local_planner + DESTINATION lib/${PROJECT_NAME}) + +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}) + +install(DIRECTORY + config + DESTINATION share/${PROJECT_NAME}) + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/droan_local_planner/config/droan.yaml b/robot/ros_ws/src/autonomy/3_local/b_planners/droan_local_planner/config/droan.yaml new file mode 100644 index 00000000..ca4e7fd8 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/droan_local_planner/config/droan.yaml @@ -0,0 +1,43 @@ +/**: + ros__parameters: + + # execution rate in hz + execute_rate: 5.0 + + + # Trajectory parameters + trajectory_library_config: "$(find-pkg-share trajectory_library)/config/acceleration_magnitudes.yaml" + dt: 0.2 + ht: 2.4 + ht_long: 3.0 + max_velocity: 0.7 + magnitude: 0.5 + + # DROAN parameters + robot_radius: 1.0 + obstacle_distance_reward: 1.0 + forward_progress_reward_weight: 0.5 + + # map parameters. see disparity_map_representation.cpp + map_representation: disparity_map_representation::DisparityMapRepresentation + obstacle_check_radius: 1.0 # this value MUST be strictly greater than the robot radius + obstacle_check_num_points: 5 + # disparity graph parameters. see disparity_graph.hpp + fixed_frame: "map" + stabilized_frame: "base_link_stabilized" # the frame of the stabilized robot (roll and pitch removed) + baseline_fallback: 0.5 # if the baseline is 0 from the camera_info, use this value instead + assume_seen_within_radius: 2.0 # anything within this radius, assume it is seen and don't check for obstacles + + # GLOBAL_PLAN mode parameters + # 0: use the yaw of the subscribed traj, 1: smoothly vary the yaw in the direction of the subscribed trajectory + # either SMOOTH_YAW or TRAJECTORY_YAW + yaw_mode: SMOOTH_YAW + + # AUTO_WAYPOINT mode parameters + auto_waypoint_buffer_duration: 30.0 + auto_waypoint_spacing_threshold: 0.5 + auto_waypoint_angle_threshold: 30.0 + + # CUSTOM_WAYPOINT mode parameters + custom_waypoint_timeout_factor: 0.3 + custom_waypoint_distance_threshold: 0.5 \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/droan_local_planner/include/droan_local_planner/droan_local_planner.hpp b/robot/ros_ws/src/autonomy/3_local/b_planners/droan_local_planner/include/droan_local_planner/droan_local_planner.hpp new file mode 100644 index 00000000..953d1623 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/droan_local_planner/include/droan_local_planner/droan_local_planner.hpp @@ -0,0 +1,665 @@ +#pragma once + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using std::placeholders::_1; + +class DroanLocalPlanner : public rclcpp::Node { + private: + std::unique_ptr traj_lib; + + std::string map_representation_class_string; + bool is_global_plan_received; + nav_msgs::msg::Path global_plan_msg; + double global_plan_trajectory_distance; + bool is_look_ahead_received, is_tracking_point_received; + + // look ahead is + airstack_msgs::msg::Odometry look_ahead_odom, tracking_point_odom; + + double execute_rate, obstacle_check_radius, obstacle_distance_reward, + forward_progress_reward_weight; + double robot_radius; + + float auto_waypoint_buffer_duration, auto_waypoint_spacing_threshold, + auto_waypoint_angle_threshold; + std::list waypoint_buffer; + + enum YawMode { TRAJECTORY_YAW, SMOOTH_YAW }; + YawMode yaw_mode; + + // whether to follow the global plan, a custom waypoint, or automatically interpolated waypoints + enum GoalMode { GLOBAL_PLAN, CUSTOM_WAYPOINT, AUTO_WAYPOINT }; + GoalMode goal_mode; + double custom_waypoint_timeout_factor, custom_waypoint_distance_threshold; + + std::shared_ptr map_representation; + + std::shared_ptr tf_buffer_ptr; + tf2_ros::TransformListener tf_listener; + + rclcpp::Subscription::SharedPtr global_plan_sub; + rclcpp::Subscription::SharedPtr waypoint_sub; + rclcpp::Subscription::SharedPtr look_ahead_sub; + rclcpp::Subscription::SharedPtr tracking_point_sub; + rclcpp::Subscription::SharedPtr custom_waypoint_sub; + + rclcpp::Publisher::SharedPtr traj_lib_vis_pub; + rclcpp::Publisher::SharedPtr traj_pub; + rclcpp::Publisher::SharedPtr traj_track_pub; + rclcpp::Publisher::SharedPtr obst_vis_pub; + rclcpp::Publisher::SharedPtr global_plan_vis_pub; + + rclcpp::Publisher::SharedPtr map_debug_pub; + + rclcpp::TimerBase::SharedPtr execute_timer; + + public: + DroanLocalPlanner() + : Node("droan_local_planner"), + goal_mode(GLOBAL_PLAN), + tf_buffer_ptr(new tf2_ros::Buffer(this->get_clock())), + tf_listener(*tf_buffer_ptr) { + // follow the global plan + global_plan_sub = this->create_subscription( + "global_plan", 10, std::bind(&DroanLocalPlanner::global_plan_callback, this, _1)); + waypoint_sub = this->create_subscription( + "way_point", 10, std::bind(&DroanLocalPlanner::waypoint_callback, this, _1)); + // from the trajectory controller, the expected look ahead point to start the next + // trajectory + look_ahead_sub = this->create_subscription( + "look_ahead", 10, std::bind(&DroanLocalPlanner::look_ahead_callback, this, _1)); + // from the tracking controller, the current position of the drone + tracking_point_sub = this->create_subscription( + "tracking_point", 10, std::bind(&DroanLocalPlanner::tracking_point_callback, this, _1)); + custom_waypoint_sub = this->create_subscription( + "custom_waypoint", 1, + std::bind(&DroanLocalPlanner::custom_waypoint_callback, this, _1)); + + // publishers + + traj_lib_vis_pub = this->create_publisher( + "trajectory_library_vis", 10); + obst_vis_pub = this->create_publisher("obstacle_vis", 10); + global_plan_vis_pub = this->create_publisher( + "local_planner_global_plan_vis", 10); + traj_pub = this->create_publisher( + "trajectory_segment_to_add", 10); + traj_track_pub = this->create_publisher( + "trajectory_override", 10); + + map_debug_pub = + this->create_publisher("disparity_map_debug", 1); + + // init parameters + this->declare_parameter("execute_rate", 5.); + this->get_parameter("execute_rate", this->execute_rate); + this->declare_parameter("obstacle_distance_reward", 1.); + this->get_parameter("obstacle_distance_reward", obstacle_distance_reward); + this->declare_parameter("obstacle_check_radius", 1.); + this->get_parameter("obstacle_check_radius", obstacle_check_radius); + this->declare_parameter("forward_progress_reward_weight", 0.5); + this->get_parameter("forward_progress_reward_weight", forward_progress_reward_weight); + this->declare_parameter("robot_radius", 0.75); + this->get_parameter("robot_radius", robot_radius); + this->declare_parameter("yaw_mode", "SMOOTH_YAW"); + auto yaw_mode_str = this->get_parameter("yaw_mode").as_string(); + if (yaw_mode_str == "TRAJECTORY_YAW") { + this->yaw_mode = TRAJECTORY_YAW; + } else if (yaw_mode_str == "SMOOTH_YAW") { + this->yaw_mode = SMOOTH_YAW; + } else { + RCLCPP_ERROR(this->get_logger(), "Invalid yaw_mode parameter"); + } + this->declare_parameter("map_representation", std::string("PointCloudMapRepresentation")); + this->get_parameter("map_representation", map_representation_class_string); + this->declare_parameter("auto_waypoint_buffer_duration", 30.); + this->get_parameter("auto_waypoint_buffer_duration", auto_waypoint_buffer_duration); + this->declare_parameter("auto_waypoint_spacing_threshold", 0.5); + this->get_parameter("auto_waypoint_spacing_threshold", auto_waypoint_spacing_threshold); + this->declare_parameter("auto_waypoint_angle_threshold", 30. * M_PI / 180.); + this->get_parameter("auto_waypoint_angle_threshold", auto_waypoint_angle_threshold); + + this->declare_parameter("custom_waypoint_timeout_factor", 0.3); + this->get_parameter("custom_waypoint_timeout_factor", custom_waypoint_timeout_factor); + this->declare_parameter("custom_waypoint_distance_threshold", 0.5); + this->get_parameter("custom_waypoint_distance_threshold", + custom_waypoint_distance_threshold); + + RCLCPP_INFO_STREAM(this->get_logger(), "DROAN node name is: " << this->get_name()); + this->declare_parameter("trajectory_library_config", std::string("")); + } + + void initialize() { + pluginlib::ClassLoader + map_representation_loader("map_representation_interface", + "map_representation_interface::MapRepresentation"); + auto node_ptr = this->shared_from_this(); + this->map_representation = + map_representation_loader.createSharedInstance(map_representation_class_string); + this->map_representation->initialize(node_ptr, tf_buffer_ptr); + this->traj_lib = std::make_unique( + this->get_parameter("trajectory_library_config").as_string(), node_ptr); + double interval = 1. / this->execute_rate; + this->execute_timer = + rclcpp::create_timer(this, this->get_clock(), rclcpp::Duration::from_seconds(interval), + std::bind(&DroanLocalPlanner::execute, this)); + } + + virtual ~DroanLocalPlanner() {} + + virtual bool execute() { + this->update_waypoint_mode(); + + if (!this->is_global_plan_received) { + RCLCPP_INFO_ONCE(this->get_logger(), "Waiting for global plan"); + return true; + } + if (!this->is_look_ahead_received) { + RCLCPP_INFO_ONCE(this->get_logger(), "Waiting for look ahead point"); + return true; + } + + Trajectory global_plan(this, global_plan_msg); + + // transform the look ahead point to the global plan frame + tf2::Vector3 look_ahead_position = tflib::to_tf(look_ahead_odom.pose.position); + bool success = tflib::to_frame(tf_buffer_ptr.get(), look_ahead_position, + look_ahead_odom.header.frame_id, global_plan.get_frame_id(), + look_ahead_odom.header.stamp, &look_ahead_position); + if (!success) { + RCLCPP_ERROR_STREAM(this->get_logger(), + "Couldn't transform from lookahead frame to global plan frame"); + return true; + } + + // increment how far along the global plan we are + double trajectory_distance; + bool is_valid = global_plan.get_trajectory_distance_at_closest_point(look_ahead_position, + &trajectory_distance); + // trim the global plan to only the next 10 meters + if (is_valid) { + this->global_plan_trajectory_distance += trajectory_distance; + global_plan = global_plan.trim_trajectory_between_distances( + this->global_plan_trajectory_distance, + this->global_plan_trajectory_distance + 10.0); + } else { + RCLCPP_INFO(this->get_logger(), "invalid"); + } + + // publish the trimmed segment of the global plan currently being used, for visualization + visualization_msgs::msg::MarkerArray global_markers = + global_plan.get_markers(this->now(), "global_plan", 0, 0, 1); + global_plan_vis_pub->publish(global_markers); + + // get the dynamic trajectories + std::vector dynamic_trajectories = + traj_lib->get_dynamic_trajectories(look_ahead_odom); + + // pick the best trajectory + auto [is_success, best_traj] = this->get_best_trajectory(dynamic_trajectories, global_plan); + + // publish the trajectory + if (is_success) { + airstack_msgs::msg::TrajectoryXYZVYaw best_traj_msg = + best_traj.get_TrajectoryXYZVYaw_msg(); + + // set yaw + if (yaw_mode == SMOOTH_YAW && best_traj.get_num_waypoints() > 0) { + apply_smooth_yaw(best_traj_msg); + } + best_traj_msg.header.stamp = this->now(); + traj_pub->publish(best_traj_msg); + RCLCPP_INFO_STREAM(this->get_logger(), "Published local trajectory"); + } else { + RCLCPP_INFO_STREAM(this->get_logger(), + "No valid trajectories, all trajectories either collide or are unseen"); + } + return true; + } + + /** + * @brief Get the best trajectory based on the cost function. Minimize the cost + * + * @param trajectory_candidates + * @param global_plan + * @return std::tuple is_success: whether there exists a valid collision-free + * trajectory, best_traj: the best trajectory + */ + std::tuple get_best_trajectory( + const std::vector& trajectory_candidates, Trajectory global_plan) { + Trajectory best_traj_ret; + double min_cost = std::numeric_limits::max(); + bool are_all_traj_in_collision = true; + + auto now = this->now(); + + auto trajectory_distances_to_closest_obstacle = + this->get_trajectory_distances_to_closest_obstacle(trajectory_candidates); + + visualization_msgs::msg::MarkerArray traj_lib_marker_arr; + + // RCLCPP_INFO_STREAM(this->get_logger(), + // "Number of trajectories: " << trajectory_candidates.size()); + + for (size_t i = 0; i < trajectory_candidates.size(); ++i) { + Trajectory traj = trajectory_candidates[i]; + double avg_distance_from_global_plan = std::numeric_limits::infinity(); + double closest_obstacle_distance = std::numeric_limits::infinity(); + + Trajectory global_plan_in_traj_frame; + try { + global_plan_in_traj_frame = global_plan.to_frame(traj.get_frame_id(), now); + } catch (tf2::TransformException& ex) { + RCLCPP_ERROR(this->get_logger(), + "Failed to transform global plan to trajectory frame: %s", ex.what()); + return {false, best_traj_ret}; + } + + // for each waypoint in the trajectory, fetch its distance to its closest obstacle + for (size_t j = 0; j < traj.get_num_waypoints(); j++) { + Waypoint wp = traj.get_waypoint(j); + + airstack_msgs::msg::Odometry odom = wp.as_odometry_msg(now, traj.get_frame_id()); + geometry_msgs::msg::PoseStamped pose; + pose.header = odom.header; + pose.pose = odom.pose; + double waypoint_obst_dist = trajectory_distances_to_closest_obstacle.at(i).at(j); + std::cout << "trajectory " << i << " waypoint " << j << " obstacle distance " + << waypoint_obst_dist << std::endl; + closest_obstacle_distance = + std::min(closest_obstacle_distance, + trajectory_distances_to_closest_obstacle.at(i).at(j)); + + // get the closest global plan point to the current trajectory waypoint + Waypoint closest_point_from_global_plan(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); + int wp_index; + double path_distance; + bool is_valid = global_plan_in_traj_frame.get_closest_point( + wp.position(), &closest_point_from_global_plan, &wp_index, &path_distance); + + // reward making progress along the global plan + double forward_progress_reward = -forward_progress_reward_weight * path_distance; + + if (is_valid) { + if (!std::isfinite(avg_distance_from_global_plan)) { + avg_distance_from_global_plan = 0; + } + avg_distance_from_global_plan += + closest_point_from_global_plan.position().distance(wp.position()) + + forward_progress_reward; + } + } + avg_distance_from_global_plan /= traj.get_num_waypoints(); + + bool is_collision = closest_obstacle_distance <= robot_radius; + // RCLCPP_INFO_STREAM(this->get_logger(), + // "Trajectory " << i << " is_collision: " << is_collision + // << " closest_obstacle_distance: " + // << closest_obstacle_distance); + if (!is_collision) { + are_all_traj_in_collision = false; + } + + std::string marker_ns = "trajectory_" + std::to_string(i); + + visualization_msgs::msg::MarkerArray traj_markers; + if (is_collision) { + // red for collision + traj_markers = traj.get_markers(this->now(), marker_ns, 1, 0, 0, .3); + } else { + // green for no collision + traj_markers = traj.get_markers(this->now(), marker_ns, 0, 1, 0, .5); + } + traj_lib_marker_arr.markers.insert(traj_lib_marker_arr.markers.end(), + traj_markers.markers.begin(), + traj_markers.markers.end()); + + // bigger distance from obstacles makes the cost smaller (more negative). cap by the + // obstacle check radius + double cost = avg_distance_from_global_plan - + obstacle_distance_reward * + std::min(closest_obstacle_distance, obstacle_check_radius); + if (!is_collision && cost < min_cost) { + min_cost = cost; + best_traj_ret = traj; + } + } + + traj_lib_vis_pub->publish(traj_lib_marker_arr); + map_debug_pub->publish(this->map_representation->get_debug_markerarray()); + + bool is_success = !are_all_traj_in_collision; + return {is_success, best_traj_ret}; + } + + /** + * @brief Get the distance to the closest obstacle for each waypoint in the trajectory + */ + std::vector> get_trajectory_distances_to_closest_obstacle( + const std::vector& trajectory_candidates) { + // TODO: clearly we should refactor map_representation to accept Trajectory objects + std::vector> trajectory_distances_to_closest_obstacle = + this->map_representation->get_values(trajectory_candidates); + return trajectory_distances_to_closest_obstacle; + } + + /** + * @brief Applies a smoothing filter to the yaw of the trajectory + */ + void apply_smooth_yaw(airstack_msgs::msg::TrajectoryXYZVYaw& best_traj_msg) { + bool found_initial_heading = false; + double initial_heading = 0; + try { + tf2::Stamped transform; + tf_buffer_ptr->canTransform( + best_traj_msg.header.frame_id, look_ahead_odom.header.frame_id, + look_ahead_odom.header.stamp, rclcpp::Duration::from_seconds(0.1)); + auto transform_msg = tf_buffer_ptr->lookupTransform(best_traj_msg.header.frame_id, + look_ahead_odom.header.frame_id, + look_ahead_odom.header.stamp); + tf2::fromMsg(transform_msg, transform); + + transform.setOrigin(tf2::Vector3(0, 0, 0)); // only care about rotation + initial_heading = + tf2::getYaw(transform * tflib::to_tf(look_ahead_odom.pose.orientation)); + + found_initial_heading = true; + + } catch (tf2::TransformException& ex) { + RCLCPP_ERROR(this->get_logger(), "Failed to get transform: %s", ex.what()); + } + + if (found_initial_heading) { + best_traj_msg.waypoints[0].yaw = initial_heading; + double alpha = 0.1; + double sin_yaw_prev = sin(best_traj_msg.waypoints[0].yaw); + double cos_yaw_prev = cos(best_traj_msg.waypoints[0].yaw); + + for (size_t i = 1; i < best_traj_msg.waypoints.size(); i++) { + airstack_msgs::msg::WaypointXYZVYaw wp_prev = best_traj_msg.waypoints[i - 1]; + airstack_msgs::msg::WaypointXYZVYaw& wp_curr = best_traj_msg.waypoints[i]; + + double yaw = atan2(wp_curr.position.y - wp_prev.position.y, + wp_curr.position.x - wp_prev.position.x); + double cos_yaw = alpha * cos(yaw) + (1 - alpha) * cos_yaw_prev; + double sin_yaw = alpha * sin(yaw) + (1 - alpha) * sin_yaw_prev; + yaw = atan2(sin_yaw, cos_yaw); + + sin_yaw_prev = sin_yaw; + cos_yaw_prev = cos_yaw; + + wp_curr.yaw = yaw; + } + } + } + + // subscriber callbacks + + /** + * @brief Saves the global plan if the goal mode is GLOBAL_PLAN + * + * @param global_plan_msg + */ + void global_plan_callback(const nav_msgs::msg::Path::SharedPtr global_plan_msg) { + RCLCPP_INFO_STREAM(this->get_logger(), "GOT GLOBAL PLAN, goal_mode: " << this->goal_mode); + if (this->goal_mode != GLOBAL_PLAN) return; + + this->global_plan_msg = *global_plan_msg; // copies + this->is_global_plan_received = true; + this->global_plan_trajectory_distance = 0; + } + + /** + * @brief If mode == AUTO_WAYPOINT, then the local planner will automatically interpolate + * waypoints to reach the desired waypoint. This callback happens when mode == AUTO_WAYPOINT and + * it receives a waypoint message. + * + * @param wp the desired waypoint + */ + void waypoint_callback(const geometry_msgs::msg::PointStamped::SharedPtr wp) { + if (this->goal_mode != AUTO_WAYPOINT) { + this->waypoint_buffer.clear(); + this->waypoint_buffer.push_back(*wp); + return; + } + + // remove old waypoints if necessary + this->waypoint_buffer.push_back(*wp); + if ((rclcpp::Time(wp->header.stamp) - rclcpp::Time(waypoint_buffer.front().header.stamp)) + .seconds() > auto_waypoint_buffer_duration) { + waypoint_buffer.pop_front(); + } + + // stitch together the history of waypoints + nav_msgs::msg::Path global_plan_msg; + global_plan_msg.header.frame_id = wp->header.frame_id; + global_plan_msg.header.stamp = wp->header.stamp; + + std::vector backwards_global_plan; + + std::vector prev_wps; + for (auto it = waypoint_buffer.rbegin(); it != waypoint_buffer.rend(); it++) { + geometry_msgs::msg::PointStamped curr_wp = *it; + geometry_msgs::msg::PoseStamped waypoint; + waypoint.pose.position.x = curr_wp.point.x; + waypoint.pose.position.y = curr_wp.point.y; + waypoint.pose.position.z = curr_wp.point.z; + + if (prev_wps.size() > 1) { + geometry_msgs::msg::PointStamped prev_wp = prev_wps[prev_wps.size() - 1]; + geometry_msgs::msg::PointStamped prev2_wp = prev_wps[prev_wps.size() - 2]; + float distance = sqrt(pow(curr_wp.point.x - prev_wp.point.x, 2) + + pow(curr_wp.point.y - prev_wp.point.y, 2)); + + float a = pow(prev_wp.point.x - curr_wp.point.x, 2) + + pow(prev_wp.point.y - curr_wp.point.y, 2); + float b = pow(prev_wp.point.x - prev2_wp.point.x, 2) + + pow(prev_wp.point.y - prev2_wp.point.y, 2); + float c = pow(prev2_wp.point.x - curr_wp.point.x, 2) + + pow(prev2_wp.point.y - curr_wp.point.y, 2); + float angle = acos((a + b - c) / sqrt(4 * a * b)); + + float angle_diff = fabs(atan2(sin(angle - M_PI), cos(angle - M_PI))); + + // ROS_INFO_STREAM("\tdistance: " << distance << " angle: " << angle*180./M_PI << " + // angle_diff: " << angle_diff*180./M_PI); + + if (distance >= auto_waypoint_spacing_threshold && + angle_diff < auto_waypoint_angle_threshold) { + // ROS_INFO_STREAM("\tADDING wp: " << curr_wp.point.x << " " << curr_wp.point.y + // << " " << curr_wp.point.z); + backwards_global_plan.push_back(waypoint); + prev_wps.push_back(curr_wp); + } + } else { + if (prev_wps.size() == 0) { + prev_wps.push_back(curr_wp); + backwards_global_plan.push_back(waypoint); + } else if (prev_wps.size() == 1) { + geometry_msgs::msg::PointStamped prev_wp = prev_wps[prev_wps.size() - 1]; + float distance = sqrt(pow(curr_wp.point.x - prev_wp.point.x, 2) + + pow(curr_wp.point.y - prev_wp.point.y, 2)); + if (distance >= auto_waypoint_spacing_threshold) { + prev_wps.push_back(curr_wp); + backwards_global_plan.push_back(waypoint); + } + } + } + } + for (size_t i = 0; i < backwards_global_plan.size(); i++) { + global_plan_msg.poses.push_back( + backwards_global_plan[backwards_global_plan.size() - 1 - i]); + } + if (is_tracking_point_received) { + try { + tf2::Stamped transform; + tf_buffer_ptr->canTransform(tracking_point_odom.header.frame_id, + wp->header.frame_id, wp->header.stamp, + rclcpp::Duration::from_seconds(0.1)); + auto transform_msg = tf_buffer_ptr->lookupTransform( + tracking_point_odom.header.frame_id, wp->header.frame_id, wp->header.stamp); + tf2::fromMsg(transform_msg, transform); + + tf2::Vector3 tp_position = tflib::to_tf(tracking_point_odom.pose.position); + tf2::Vector3 wp_position = transform * tflib::to_tf(wp->point); + + tf2::Vector3 direction = (wp_position - tp_position).normalized() * 3; + tf2::Vector3 wp2_position = wp_position + direction; + + geometry_msgs::msg::PoseStamped wp1, wp2; + wp1.pose.position.x = wp_position.x(); + wp1.pose.position.y = wp_position.y(); + wp1.pose.position.z = wp_position.z(); + wp2.pose.position.x = wp2_position.x(); + wp2.pose.position.y = wp2_position.y(); + wp2.pose.position.z = wp2_position.z(); + global_plan_msg.poses.push_back(wp1); + global_plan_msg.poses.push_back(wp2); + } catch (tf2::TransformException& ex) { + RCLCPP_ERROR(this->get_logger(), "Failed to get transform: %s", ex.what()); + } + } + + global_plan_trajectory_distance = 0; + this->global_plan_msg = global_plan_msg; + this->is_global_plan_received = true; + } + + /** + * @brief Clears the current global plan reference and go immediately to a custom waypoint + * (typically set by the user). Sets the global plan to be the line between the current look + * ahead point and the custom waypoint. + * + * @param wp + */ + void custom_waypoint_callback(const geometry_msgs::msg::PoseStamped::SharedPtr wp) { + if (!is_look_ahead_received) return; + + try { + tf2::Stamped transform; + tf_buffer_ptr->canTransform(look_ahead_odom.header.frame_id, wp->header.frame_id, + wp->header.stamp, rclcpp::Duration::from_seconds(0.1)); + auto transform_msg = tf_buffer_ptr->lookupTransform( + look_ahead_odom.header.frame_id, wp->header.frame_id, wp->header.stamp); + tf2::fromMsg(transform_msg, transform); + + tf2::Vector3 la_position = tflib::to_tf(look_ahead_odom.pose.position); + tf2::Vector3 wp_position = transform * tflib::to_tf(wp->pose.position); + + nav_msgs::msg::Path global_plan_msg; + global_plan_msg.header.frame_id = look_ahead_odom.header.frame_id; + global_plan_msg.header.stamp = this->now(); + + geometry_msgs::msg::PoseStamped wp1, wp2; + wp1.pose.position.x = la_position.x(); + wp1.pose.position.y = la_position.y(); + wp1.pose.position.z = la_position.z(); + wp2.pose.position.x = wp_position.x(); + wp2.pose.position.y = wp_position.y(); + wp2.pose.position.z = wp_position.z(); + global_plan_msg.poses.push_back(wp1); + global_plan_msg.poses.push_back(wp2); + + global_plan_trajectory_distance = 0; + this->global_plan_msg = global_plan_msg; + this->is_global_plan_received = true; + + goal_mode = CUSTOM_WAYPOINT; + } catch (tf2::TransformException& ex) { + RCLCPP_ERROR(this->get_logger(), "Failed to get transform: %s", ex.what()); + } + } + + /** + * Check if we should switch from CUSTOM_WAYPOINT to AUTO_WAYPOINT + */ + void update_waypoint_mode() { + if (goal_mode == CUSTOM_WAYPOINT) { + if (global_plan_msg.poses.size() < 2) goal_mode = AUTO_WAYPOINT; + + // check if the time limit for reaching the waypoint has elapsed + double elapsed_time = (this->now() - global_plan_msg.header.stamp).seconds(); + double distance = 0; + for (size_t i = 1; i < global_plan_msg.poses.size(); i++) { + geometry_msgs::msg::Pose prev_wp, curr_wp; + prev_wp = global_plan_msg.poses[i - 1].pose; + curr_wp = global_plan_msg.poses[i].pose; + + distance += sqrt(pow(prev_wp.position.x - curr_wp.position.x, 2) + + pow(prev_wp.position.y - curr_wp.position.y, 2)); + } + + // ROS_INFO_STREAM("elapsed: " << elapsed_time << " / " << + // distance/custom_waypoint_timeout_factor << " distance: " << distance); + if (elapsed_time >= distance / this->custom_waypoint_timeout_factor) { + // ROS_INFO_STREAM("CUSTOM WAYPOINT TIMEOUT REACHED"); + goal_mode = AUTO_WAYPOINT; + } + + // check if we are close enough to the waypoint + if (is_tracking_point_received) { + try { + tf2::Stamped transform; + tf_buffer_ptr->canTransform( + tracking_point_odom.header.frame_id, global_plan_msg.header.frame_id, + global_plan_msg.header.stamp, rclcpp::Duration::from_seconds(0.1)); + auto transform_msg = tf_buffer_ptr->lookupTransform( + tracking_point_odom.header.frame_id, global_plan_msg.header.frame_id, + global_plan_msg.header.stamp); + tf2::fromMsg(transform_msg, transform); + + tf2::Vector3 tp_position = tflib::to_tf(tracking_point_odom.pose.position); + tp_position.setZ(0); + tf2::Vector3 wp_position = + transform * tflib::to_tf(global_plan_msg.poses.back().pose.position); + wp_position.setZ(0); + + if (tp_position.distance(wp_position) < custom_waypoint_distance_threshold) { + // ROS_INFO_STREAM("CUSTOM WAYPOINT DISTANCE THRESHOLD MET"); + goal_mode = AUTO_WAYPOINT; + } + } catch (tf2::TransformException& ex) { + RCLCPP_ERROR_STREAM( + this->get_logger(), + "TransformException in update_waypoint_mode: " << ex.what()); + } + } + } + } + /** + * @brief Receive the look ahead point to plan the next local trajectory from + * + * @param odom + */ + void look_ahead_callback(const airstack_msgs::msg::Odometry::SharedPtr odom) { + is_look_ahead_received = true; + RCLCPP_INFO_STREAM_ONCE(this->get_logger(), + "look ahead received with frame id: " << odom->header.frame_id); + look_ahead_odom = *odom; + } + void tracking_point_callback(const airstack_msgs::msg::Odometry::SharedPtr odom) { + is_tracking_point_received = true; + RCLCPP_INFO_STREAM_ONCE(this->get_logger(), + "tracking point received with frame id: " << odom->header.frame_id); + tracking_point_odom = *odom; + } +}; diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/droan_local_planner/launch/droan_local_planner.launch.xml b/robot/ros_ws/src/autonomy/3_local/b_planners/droan_local_planner/launch/droan_local_planner.launch.xml new file mode 100644 index 00000000..96efd061 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/droan_local_planner/launch/droan_local_planner.launch.xml @@ -0,0 +1,5 @@ + + + + + \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/droan_local_planner/launch/droan_local_planner_launch.yaml b/robot/ros_ws/src/autonomy/3_local/b_planners/droan_local_planner/launch/droan_local_planner_launch.yaml new file mode 100644 index 00000000..b1a89a75 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/droan_local_planner/launch/droan_local_planner_launch.yaml @@ -0,0 +1,16 @@ +launch: +# - arg: +# name: "robot_name" +# default: "robot1" +# - arg: +# name: "tf_prefix" +# default: "$(var robot_name)" +- node: + pkg: "droan_local_planner" + exec: "droan_local_planner" + name: "droan_local_planner" + namespace: "droan_local_planner" + param: + - + from: $(find-pkg-share droan_local_planner)/config/droan.yaml + # allow_substs: true diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/droan_local_planner/package.xml b/robot/ros_ws/src/autonomy/3_local/b_planners/droan_local_planner/package.xml new file mode 100644 index 00000000..ef446e80 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/droan_local_planner/package.xml @@ -0,0 +1,28 @@ + + + + droan_local_planner + 0.0.0 + TODO: Package description + andrew + TODO: License declaration + + ament_cmake + + airstack_msgs + map_representation_interface + pluginlib + rclcpp + std_msgs + tf2 + tf2_ros + trajectory_controller + trajectory_library + + ament_lint_auto + ament_lint_common + + + ament_cmake + + \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/droan_local_planner/src/droan_local_planner.cpp b/robot/ros_ws/src/autonomy/3_local/b_planners/droan_local_planner/src/droan_local_planner.cpp new file mode 100644 index 00000000..b1571589 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/droan_local_planner/src/droan_local_planner.cpp @@ -0,0 +1,12 @@ +#include +#include +#include + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + node->initialize(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/CMakeLists.txt b/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/CMakeLists.txt new file mode 100644 index 00000000..4e068f99 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/CMakeLists.txt @@ -0,0 +1,53 @@ +cmake_minimum_required(VERSION 3.8) +project(takeoff_landing_planner) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(airstack_msgs REQUIRED) +find_package(airstack_common REQUIRED) +find_package(trajectory_library REQUIRED) + +add_executable(takeoff_landing_planner src/takeoff_landing_planner.cpp) +target_include_directories(takeoff_landing_planner PUBLIC + $ + $) +target_compile_features(takeoff_landing_planner PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +ament_target_dependencies( + takeoff_landing_planner + std_msgs + airstack_msgs + airstack_common + trajectory_library +) + +install(TARGETS takeoff_landing_planner + DESTINATION lib/${PROJECT_NAME}) + +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}/ + ) + +install(DIRECTORY + config + DESTINATION share/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/config/takeoff_landing_planner.yaml b/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/config/takeoff_landing_planner.yaml new file mode 100644 index 00000000..8b80240e --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/config/takeoff_landing_planner.yaml @@ -0,0 +1,14 @@ +/**: + ros__parameters: + takeoff_height: 1.0 + high_takeoff_height: 2.0 + takeoff_landing_velocity: 0.3 + takeoff_acceptance_distance: 0.3 + takeoff_acceptance_time: 10.0 + landing_stationary_distance: 0.02 + landing_acceptance_time: 5.0 + landing_tracking_point_ahead_time: 5.0 + + takeoff_path_roll: 0. # in degrees + takeoff_path_pitch: 20. # in degrees + takeoff_path_relative_to_orientation: false diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/include/takeoff_landing_planner/takeoff_landing_planner.hpp b/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/include/takeoff_landing_planner/takeoff_landing_planner.hpp new file mode 100644 index 00000000..8a6b1cc9 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/include/takeoff_landing_planner/takeoff_landing_planner.hpp @@ -0,0 +1,83 @@ +#ifndef _TAKEOFF_LANDING_PLANNER_H_ +#define _TAKEOFF_LANDING_PLANNER_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class TakeoffLandingPlanner : public rclcpp::Node { + private: + // parameters + float takeoff_height, high_takeoff_height, takeoff_landing_velocity; + float takeoff_acceptance_distance, takeoff_acceptance_time; + float landing_stationary_distance, landing_acceptance_time; + float landing_tracking_point_ahead_time; + float takeoff_path_roll, takeoff_path_pitch; + bool takeoff_path_relative_to_orientation; + + // variables + bool got_completion_percentage, is_tracking_point_received, got_robot_odom; + nav_msgs::msg::Odometry robot_odom; + airstack_msgs::msg::Odometry tracking_point_odom; + float completion_percentage; + // airstack_msgs::srv::TrajectoryMode track_mode_srv; + uint8_t current_command; + + // takeoff variables + bool takeoff_is_newly_active; + bool takeoff_distance_check; + bool ekf_active; + rclcpp::Time takeoff_acceptance_start; + bool high_takeoff; + TakeoffTrajectory* takeoff_traj_gen; + TakeoffTrajectory* high_takeoff_traj_gen; + + // land variables + bool land_is_newly_active; + std::list robot_odoms; + TakeoffTrajectory* landing_traj_gen; + + // subscribers + rclcpp::Subscription::SharedPtr completion_percentage_sub; + rclcpp::Subscription::SharedPtr tracking_point_sub; + rclcpp::Subscription::SharedPtr robot_odom_sub; + rclcpp::Subscription::SharedPtr ekf_active_sub; + rclcpp::Subscription::SharedPtr high_takeoff_sub; + tf2_ros::Buffer* tf_buffer; + tf2_ros::TransformListener* tf_listener; + + // publishers + rclcpp::Publisher::SharedPtr traj_override_pub; + rclcpp::Publisher::SharedPtr takeoff_state_pub; + rclcpp::Publisher::SharedPtr landing_state_pub; + + // services + rclcpp::Service::SharedPtr command_server; + + // timers + rclcpp::TimerBase::SharedPtr timer; + + // callbacks + void completion_percentage_callback(std_msgs::msg::Float32::SharedPtr msg); + void tracking_point_callback(airstack_msgs::msg::Odometry::SharedPtr msg); + void robot_odom_callback(nav_msgs::msg::Odometry::SharedPtr msg); + void ekf_active_callback(std_msgs::msg::Bool::SharedPtr msg); + void high_takeoff_callback(std_msgs::msg::Bool::SharedPtr msg); + + void set_takeoff_landing_command( + const airstack_msgs::srv::TakeoffLandingCommand::Request::SharedPtr request, + airstack_msgs::srv::TakeoffLandingCommand::Response::SharedPtr response); + + public: + TakeoffLandingPlanner(); + void timer_callback(); +}; + +#endif diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/index.html b/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/index.html new file mode 100644 index 00000000..9b965906 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/index.html @@ -0,0 +1,2606 @@ + + + + + + + + + + + + + + + + + + + Index - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

Index

+ +

TakeoffLandingPlanner

+

Author:

+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ +
+ + + +
+
+
+ +
+ Copyright © 2024 - 2030 AirLab CMU +
+ + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + +
+ +
+
+
+ +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/launch/takeoff_landing_planner.launch.xml b/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/launch/takeoff_landing_planner.launch.xml new file mode 100644 index 00000000..7b4a46a6 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/launch/takeoff_landing_planner.launch.xml @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/package.xml b/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/package.xml new file mode 100644 index 00000000..b3a2a699 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/package.xml @@ -0,0 +1,24 @@ + + + + takeoff_landing_planner + 0.0.0 + TODO: Package description + uav + TODO: License declaration + + ament_cmake + + rclcpp + std_msgs + airstack_msgs + airstack_common + trajectory_library + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/src/takeoff_landing_planner.cpp b/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/src/takeoff_landing_planner.cpp new file mode 100644 index 00000000..279485fc --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/src/takeoff_landing_planner.cpp @@ -0,0 +1,274 @@ +#include + +TakeoffLandingPlanner::TakeoffLandingPlanner() : rclcpp::Node("takeoff_landing_planner") { + // init parameters + takeoff_height = airstack::get_param(this, "takeoff_height", 0.5); + high_takeoff_height = airstack::get_param(this, "high_takeoff_height", 1.2); + takeoff_landing_velocity = airstack::get_param(this, "takeoff_landing_velocity", 0.3); + takeoff_acceptance_distance = airstack::get_param(this, "takeoff_acceptance_distance", 0.1); + takeoff_acceptance_time = airstack::get_param(this, "takeoff_acceptance_time", 2.0); + landing_stationary_distance = airstack::get_param(this, "landing_stationary_distance", 0.02); + landing_acceptance_time = airstack::get_param(this, "landing_acceptance_time", 5.0); + landing_tracking_point_ahead_time = + airstack::get_param(this, "landing_tracking_point_ahead_time", 5.0); + takeoff_path_roll = airstack::get_param(this, "takeoff_path_roll", 0.) * M_PI / 180.; + takeoff_path_pitch = airstack::get_param(this, "takeoff_path_pitch", 0.) * M_PI / 180.; + takeoff_path_relative_to_orientation = + airstack::get_param(this, "takeoff_path_relative_to_orientation", false); + + // init subscribers + tf_buffer = new tf2_ros::Buffer(this->get_clock()); + tf_listener = new tf2_ros::TransformListener(*tf_buffer); + completion_percentage_sub = this->create_subscription( + "trajectory_completion_percentage", 1, + std::bind(&TakeoffLandingPlanner::completion_percentage_callback, this, + std::placeholders::_1)); + tracking_point_sub = this->create_subscription( + "tracking_point", 1, + std::bind(&TakeoffLandingPlanner::tracking_point_callback, this, std::placeholders::_1)); + robot_odom_sub = this->create_subscription( + "odometry", 1, + std::bind(&TakeoffLandingPlanner::robot_odom_callback, this, std::placeholders::_1)); + ekf_active_sub = this->create_subscription( + "ekf_active", 1, + std::bind(&TakeoffLandingPlanner::ekf_active_callback, this, std::placeholders::_1)); + high_takeoff_sub = this->create_subscription( + "high_takeoff", 1, + std::bind(&TakeoffLandingPlanner::high_takeoff_callback, this, std::placeholders::_1)); + + // init publishers + traj_override_pub = + this->create_publisher("trajectory_override", 1); + takeoff_state_pub = this->create_publisher("takeoff_state", 1); + landing_state_pub = this->create_publisher("landing_state", 1); + + // init services + command_server = this->create_service( + "set_takeoff_landing_command", + std::bind(&TakeoffLandingPlanner::set_takeoff_landing_command, this, std::placeholders::_1, + std::placeholders::_2)); + + // init variables + got_completion_percentage = false; + is_tracking_point_received = false; + + // TODO set this back to false and remove robot_odom initialization + got_robot_odom = false; + + // track_mode_srv.request.mode = airstack_msgs::srv::TrajectoryMode::Request::TRACK; + high_takeoff = false; + takeoff_traj_gen = + new TakeoffTrajectory(takeoff_height, takeoff_landing_velocity, takeoff_path_roll, + takeoff_path_pitch, takeoff_path_relative_to_orientation); + high_takeoff_traj_gen = + new TakeoffTrajectory(high_takeoff_height, takeoff_landing_velocity, takeoff_path_roll, + takeoff_path_pitch, takeoff_path_relative_to_orientation); + // TODO: this landing point is hardcoded. it should be parameterized + landing_traj_gen = new TakeoffTrajectory(-10000., takeoff_landing_velocity); + current_command = airstack_msgs::srv::TakeoffLandingCommand::Request::NONE; + + completion_percentage = 0.f; + takeoff_is_newly_active = true; + land_is_newly_active = true; + takeoff_distance_check = false; + ekf_active = false; + + + // timers + timer = rclcpp::create_timer(this, this->get_clock(), rclcpp::Duration::from_seconds(1. / 20.), + std::bind(&TakeoffLandingPlanner::timer_callback, this)); +} + +void TakeoffLandingPlanner::timer_callback() { + RCLCPP_INFO_STREAM(this->get_logger(), + "conditions: " << got_completion_percentage << " " << is_tracking_point_received << " " << got_robot_odom); + if (!got_completion_percentage || !is_tracking_point_received || !got_robot_odom) return; + + std_msgs::msg::String takeoff_state, landing_state; + takeoff_state.data = landing_state.data = "NONE"; + + // takeoff + if (current_command == airstack_msgs::srv::TakeoffLandingCommand::Request::TAKEOFF) { + takeoff_state.data = "TAKING_OFF"; + + if (completion_percentage >= 100.f) { + // get distance between tracking point and robot odom + float distance = std::numeric_limits::max(); + try { + tf2::Stamped transform; + geometry_msgs::msg::TransformStamped t = tf_buffer->lookupTransform( + tracking_point_odom.header.frame_id, robot_odom.header.frame_id, + robot_odom.header.stamp, rclcpp::Duration::from_seconds(0.1)); + tf2::fromMsg(t, transform); + + tf2::Vector3 tracking_point_position = + tflib::to_tf(tracking_point_odom.pose.position); + tf2::Vector3 robot_odom_position = + transform * tflib::to_tf(robot_odom.pose.pose.position); + + distance = tracking_point_position.distance(robot_odom_position); + } catch (const tf2::TransformException& ex) { + RCLCPP_ERROR_STREAM(this->get_logger(), + "TransformException in TakeoffMonitor: " << ex.what()); + } + + // ROS_INFO_STREAM("distance: " << distance); + + // check if distance meets the takeoff acceptance threshold + if (distance <= takeoff_acceptance_distance) { + if (!takeoff_distance_check) { + takeoff_distance_check = true; + takeoff_acceptance_start = robot_odom.header.stamp; + } + + // check if the distance threshold has been met for the required amount of time + // ROS_INFO_STREAM("duration: " << (robot_odom.header.stamp - + // takeoff_acceptance_start).toSec() << " / " << takeoff_acceptance_time); + if ((rclcpp::Time(robot_odom.header.stamp) - takeoff_acceptance_start).seconds() >= + takeoff_acceptance_time || + ekf_active) { + takeoff_state.data = "COMPLETE"; + } + } else + takeoff_distance_check = false; + } + } + + // land + if (current_command == airstack_msgs::srv::TakeoffLandingCommand::Request::LAND) { + landing_state.data = "LANDING"; + + // check if the robot has been moving + if (robot_odoms.size() > 2) { + nav_msgs::msg::Odometry initial_odom = robot_odoms.front(); + float time_diff = + (rclcpp::Time(robot_odom.header.stamp) - rclcpp::Time(initial_odom.header.stamp)) + .seconds(); + + // landing_detected = time_diff > landing_acceptance_time; + bool time_check = time_diff > landing_acceptance_time; + while (!robot_odoms.empty() && (rclcpp::Time(robot_odom.header.stamp) - + rclcpp::Time(robot_odoms.front().header.stamp)) + .seconds() > landing_acceptance_time) { + initial_odom = robot_odoms.front(); + robot_odoms.pop_front(); + } + robot_odoms.push_front(initial_odom); + + bool distance_check = false; + for (auto it = robot_odoms.begin(); time_check && it != robot_odoms.end(); it++) { + float distance = tflib::to_tf(robot_odoms.begin()->pose.pose.position) + .distance(tflib::to_tf(it->pose.pose.position)); + distance_check = distance <= landing_stationary_distance; + if (!distance_check) { + // ROS_INFO_STREAM("distance: " << distance); + break; + } + } + + float z_distance = 100000.f; + try { + tf2::Stamped transform; + geometry_msgs::msg::TransformStamped t = tf_buffer->lookupTransform( + tracking_point_odom.header.frame_id, robot_odom.header.frame_id, + robot_odom.header.stamp, rclcpp::Duration::from_seconds(0.1)); + tf2::fromMsg(t, transform); + tf2::Vector3 tracking_point_position = + tflib::to_tf(tracking_point_odom.pose.position); + tf2::Vector3 robot_odom_position = + transform * tflib::to_tf(robot_odom.pose.pose.position); + + z_distance = robot_odom_position.z() - tracking_point_position.z(); + } catch (const tf2::TransformException& ex) { + RCLCPP_ERROR_STREAM( + this->get_logger(), + "TransformException in TakeoffMonitor landing tf lookup: " << ex.what()); + } + bool tracking_point_check = + (z_distance / takeoff_landing_velocity) > landing_tracking_point_ahead_time; + RCLCPP_INFO_STREAM( + this->get_logger(), + "landing: " << z_distance << " " << (z_distance / takeoff_landing_velocity) << " " + << landing_tracking_point_ahead_time << " " << tracking_point_check); + + // ROS_INFO_STREAM("LANDING CHECK: " << time_diff << " " << time_check << " " << + // distance_check); + + if (time_check && (distance_check || tracking_point_check)) + landing_state.data = "COMPLETE"; + } + } + + takeoff_state_pub->publish(takeoff_state); + landing_state_pub->publish(landing_state); +} + +// callbacks +void TakeoffLandingPlanner::completion_percentage_callback(std_msgs::msg::Float32::SharedPtr msg) { + got_completion_percentage = true; + completion_percentage = msg->data; +} + +void TakeoffLandingPlanner::tracking_point_callback(airstack_msgs::msg::Odometry::SharedPtr msg) { + is_tracking_point_received = true; + tracking_point_odom = *msg; +} + +void TakeoffLandingPlanner::robot_odom_callback(nav_msgs::msg::Odometry::SharedPtr msg) { + got_robot_odom = true; + robot_odom = *msg; + + if (current_command == airstack_msgs::srv::TakeoffLandingCommand::Request::LAND) + robot_odoms.push_back(*msg); +} + +void TakeoffLandingPlanner::ekf_active_callback(std_msgs::msg::Bool::SharedPtr msg) { + ekf_active = msg->data; +} + +void TakeoffLandingPlanner::high_takeoff_callback(std_msgs::msg::Bool::SharedPtr msg) { + high_takeoff = msg->data; +} + +void TakeoffLandingPlanner::set_takeoff_landing_command( + const airstack_msgs::srv::TakeoffLandingCommand::Request::SharedPtr request, + airstack_msgs::srv::TakeoffLandingCommand::Response::SharedPtr response) { + current_command = request->command; + + if (current_command == airstack_msgs::srv::TakeoffLandingCommand::Request::NONE) { + } else if (current_command == airstack_msgs::srv::TakeoffLandingCommand::Request::TAKEOFF) { + // put the trajectory controller into track mode + // traj_mode_client.call(track_mode_srv); + // publish a takeoff trajectory + RCLCPP_INFO_STREAM(get_logger(), "takeofflanding 1"); + + airstack_msgs::msg::Odometry takeoff_starting_point = tracking_point_odom; + if (got_robot_odom && takeoff_path_relative_to_orientation) + takeoff_starting_point.pose.orientation = robot_odom.pose.pose.orientation; + + if (high_takeoff) { + RCLCPP_INFO_STREAM(get_logger(), "takeofflanding hightakeoff"); + traj_override_pub->publish( + high_takeoff_traj_gen->get_trajectory(takeoff_starting_point)); + } else { + RCLCPP_INFO_STREAM(get_logger(), "takeofflanding lowtakeoff"); + traj_override_pub->publish(takeoff_traj_gen->get_trajectory(takeoff_starting_point)); + } + } else if (current_command == airstack_msgs::srv::TakeoffLandingCommand::Request::LAND) { + robot_odoms.clear(); + // put the trajectory controller into track mode + // traj_mode_client.call(track_mode_srv); + // publish a landing trajectory + traj_override_pub->publish(landing_traj_gen->get_trajectory(tracking_point_odom)); + } + RCLCPP_INFO_STREAM(get_logger(), "takeofflanding end"); + + response->accepted = true; +} + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/CMakeLists.txt b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/CMakeLists.txt new file mode 100644 index 00000000..b582b711 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/CMakeLists.txt @@ -0,0 +1,87 @@ +cmake_minimum_required(VERSION 3.8) +project(trajectory_library) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(airstack_msgs REQUIRED) +find_package(airstack_common REQUIRED) + +add_library(trajectory_library SHARED src/trajectory_library.cpp) +target_include_directories(trajectory_library PUBLIC + $ + $) +target_link_libraries(trajectory_library + yaml-cpp +) +target_compile_features(trajectory_library PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +ament_target_dependencies( + trajectory_library + # Required dependencies + nav_msgs + visualization_msgs + geometry_msgs + rclcpp + tf2 + tf2_ros + airstack_msgs + airstack_common +) +ament_export_targets(trajectory_library HAS_LIBRARY_TARGET) +ament_export_dependencies(rclcpp nav_msgs geometry_msgs visualization_msgs tf2 tf2_ros airstack_common) + +install(TARGETS trajectory_library + DESTINATION lib/${PROJECT_NAME}) + +install( + DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS trajectory_library + EXPORT trajectory_library + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + +# install config files +install(DIRECTORY + config + DESTINATION share/${PROJECT_NAME}) + + +install(PROGRAMS scripts/fixed_trajectory_generator.py DESTINATION lib/${PROJECT_NAME}) + +ament_export_include_directories( + include + ${mav_msgs_INCLUDE_DIRS} + ${geometry_msgs_INCLUDE_DIRS} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/config/acceleration_magnitudes.yaml b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/config/acceleration_magnitudes.yaml new file mode 100644 index 00000000..b63102ac --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/config/acceleration_magnitudes.yaml @@ -0,0 +1,1298 @@ +--- +trajectories: + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 0 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 180 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 0 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 180 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 0 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 180 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 0 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 180 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 0 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 180 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 0 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 180 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 0 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 180 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 0 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 180 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 0 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 180 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 0 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 180 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 90 + magnitude_yaw: 0 + max_velocity: 0.3 + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -90 + magnitude_yaw: 0 + max_velocity: 0.3 + type: acceleration \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/config/acceleration_trajectories.yaml b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/config/acceleration_trajectories.yaml new file mode 100644 index 00000000..f8e268ee --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/config/acceleration_trajectories.yaml @@ -0,0 +1,214 @@ +trajectories: + - type: acceleration + frame: look_ahead_point_stabilized + x: .3 + y: 0 + z: 0 + dt: 0.2 + ht: 3 + max_velocity: 0.7 + - type: acceleration + frame: look_ahead_point_stabilized + x: -.3 + y: 0 + z: 0 + dt: 0.2 + ht: 3 + max_velocity: 0.7 + - type: acceleration + frame: look_ahead_point_stabilized + x: 0 + y: .3 + z: 0 + dt: 0.2 + ht: 3 + max_velocity: 0.7 + - type: acceleration + frame: look_ahead_point_stabilized + x: 0 + y: -.3 + z: 0 + dt: 0.2 + ht: 3 + max_velocity: 0.7 + - type: acceleration + frame: look_ahead_point_stabilized + x: 0 + y: 0 + z: .3 + dt: 0.2 + ht: 3 + max_velocity: 0.7 + - type: acceleration + frame: look_ahead_point_stabilized + x: 0 + y: 0 + z: -.3 + dt: 0.2 + ht: 3 + max_velocity: 0.7 + + - type: acceleration + frame: look_ahead_point_stabilized + x: 0.173 + y: 0.173 + z: 0.173 + dt: 0.2 + ht: 3 + max_velocity: 0.7 + - type: acceleration + frame: look_ahead_point_stabilized + x: -0.173 + y: 0.173 + z: 0.173 + dt: 0.2 + ht: 3 + max_velocity: 0.7 + - type: acceleration + frame: look_ahead_point_stabilized + x: 0.173 + y: -0.173 + z: 0.173 + dt: 0.2 + ht: 3 + max_velocity: 0.7 + - type: acceleration + frame: look_ahead_point_stabilized + x: -0.173 + y: -0.173 + z: 0.173 + dt: 0.2 + ht: 3 + max_velocity: 0.7 + + - type: acceleration + frame: look_ahead_point_stabilized + x: 0.173 + y: 0.173 + z: -0.173 + dt: 0.2 + ht: 3 + max_velocity: 0.7 + - type: acceleration + frame: look_ahead_point_stabilized + x: -0.173 + y: 0.173 + z: -0.173 + dt: 0.2 + ht: 3 + max_velocity: 0.7 + - type: acceleration + frame: look_ahead_point_stabilized + x: 0.173 + y: -0.173 + z: -0.173 + dt: 0.2 + ht: 3 + max_velocity: 0.7 + - type: acceleration + frame: look_ahead_point_stabilized + x: -0.173 + y: -0.173 + z: -0.173 + dt: 0.2 + ht: 3 + max_velocity: 0.7 + + - type: acceleration + frame: look_ahead_point_stabilized + x: 0.212 + y: 0.212 + z: 0 + dt: 0.2 + ht: 3 + max_velocity: 0.7 + - type: acceleration + frame: look_ahead_point_stabilized + x: -0.212 + y: 0.212 + z: 0 + dt: 0.2 + ht: 3 + max_velocity: 0.7 + - type: acceleration + frame: look_ahead_point_stabilized + x: 0.212 + y: -0.212 + z: 0 + dt: 0.2 + ht: 3 + max_velocity: 0.7 + - type: acceleration + frame: look_ahead_point_stabilized + x: 0.212 + y: -0.212 + z: 0 + dt: 0.2 + ht: 3 + max_velocity: 0.7 + + - type: acceleration + frame: look_ahead_point_stabilized + x: 0.212 + y: 0 + z: 0.212 + dt: 0.2 + ht: 3 + max_velocity: 0.7 + - type: acceleration + frame: look_ahead_point_stabilized + x: -0.212 + y: 0 + z: 0.212 + dt: 0.2 + ht: 3 + max_velocity: 0.7 + - type: acceleration + frame: look_ahead_point_stabilized + x: 0.212 + y: 0 + z: -0.212 + dt: 0.2 + ht: 3 + max_velocity: 0.7 + - type: acceleration + frame: look_ahead_point_stabilized + x: -0.212 + y: 0 + z: -0.212 + dt: 0.2 + ht: 3 + max_velocity: 0.7 + + - type: acceleration + frame: look_ahead_point_stabilized + x: 0 + y: 0.212 + z: 0.212 + dt: 0.2 + ht: 3 + max_velocity: 0.7 + - type: acceleration + frame: look_ahead_point_stabilized + x: 0 + y: -0.212 + z: 0.212 + dt: 0.2 + ht: 3 + max_velocity: 0.7 + - type: acceleration + frame: look_ahead_point_stabilized + x: 0 + y: 0.212 + z: -0.212 + dt: 0.2 + ht: 3 + max_velocity: 0.7 + - type: acceleration + frame: look_ahead_point_stabilized + x: 0 + y: -0.212 + z: -0.212 + dt: 0.2 + ht: 3 + max_velocity: 0.7 diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/config/acceleration_trajectories_fast.yaml b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/config/acceleration_trajectories_fast.yaml new file mode 100644 index 00000000..4e14ac2f --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/config/acceleration_trajectories_fast.yaml @@ -0,0 +1,214 @@ +trajectories: + - type: acceleration + frame: look_ahead_point_stabilized + x: 1. + y: 0 + z: 0 + dt: 0.2 + ht: 1.2 + max_velocity: 1.5 + - type: acceleration + frame: look_ahead_point_stabilized + x: -1. + y: 0 + z: 0 + dt: 0.2 + ht: 1.2 + max_velocity: 1.5 + - type: acceleration + frame: look_ahead_point_stabilized + x: 0 + y: 1. + z: 0 + dt: 0.2 + ht: 1.2 + max_velocity: 1.5 + - type: acceleration + frame: look_ahead_point_stabilized + x: 0 + y: -1. + z: 0 + dt: 0.2 + ht: 1.2 + max_velocity: 1.5 + - type: acceleration + frame: look_ahead_point_stabilized + x: 0 + y: 0 + z: 1. + dt: 0.2 + ht: 1.2 + max_velocity: 1.5 + - type: acceleration + frame: look_ahead_point_stabilized + x: 0 + y: 0 + z: -1. + dt: 0.2 + ht: 1.2 + max_velocity: 1.5 + + - type: acceleration + frame: look_ahead_point_stabilized + x: 0.577 + y: 0.577 + z: 0.577 + dt: 0.2 + ht: 1.2 + max_velocity: 1.5 + - type: acceleration + frame: look_ahead_point_stabilized + x: -0.577 + y: 0.577 + z: 0.577 + dt: 0.2 + ht: 1.2 + max_velocity: 1.5 + - type: acceleration + frame: look_ahead_point_stabilized + x: 0.577 + y: -0.577 + z: 0.577 + dt: 0.2 + ht: 1.2 + max_velocity: 1.5 + - type: acceleration + frame: look_ahead_point_stabilized + x: -0.577 + y: -0.577 + z: 0.577 + dt: 0.2 + ht: 1.2 + max_velocity: 1.5 + + - type: acceleration + frame: look_ahead_point_stabilized + x: 0.577 + y: 0.577 + z: -0.577 + dt: 0.2 + ht: 1.2 + max_velocity: 1.5 + - type: acceleration + frame: look_ahead_point_stabilized + x: -0.577 + y: 0.577 + z: -0.577 + dt: 0.2 + ht: 1.2 + max_velocity: 1.5 + - type: acceleration + frame: look_ahead_point_stabilized + x: 0.577 + y: -0.577 + z: -0.577 + dt: 0.2 + ht: 1.2 + max_velocity: 1.5 + - type: acceleration + frame: look_ahead_point_stabilized + x: -0.577 + y: -0.577 + z: -0.577 + dt: 0.2 + ht: 1.2 + max_velocity: 1.5 + + - type: acceleration + frame: look_ahead_point_stabilized + x: 0.707 + y: 0.707 + z: 0 + dt: 0.2 + ht: 1.2 + max_velocity: 1.5 + - type: acceleration + frame: look_ahead_point_stabilized + x: -0.707 + y: 0.707 + z: 0 + dt: 0.2 + ht: 1.2 + max_velocity: 1.5 + - type: acceleration + frame: look_ahead_point_stabilized + x: 0.707 + y: -0.707 + z: 0 + dt: 0.2 + ht: 1.2 + max_velocity: 1.5 + - type: acceleration + frame: look_ahead_point_stabilized + x: 0.707 + y: -0.707 + z: 0 + dt: 0.2 + ht: 1.2 + max_velocity: 1.5 + + - type: acceleration + frame: look_ahead_point_stabilized + x: 0.707 + y: 0 + z: 0.707 + dt: 0.2 + ht: 1.2 + max_velocity: 1.5 + - type: acceleration + frame: look_ahead_point_stabilized + x: -0.707 + y: 0 + z: 0.707 + dt: 0.2 + ht: 1.2 + max_velocity: 1.5 + - type: acceleration + frame: look_ahead_point_stabilized + x: 0.707 + y: 0 + z: -0.707 + dt: 0.2 + ht: 1.2 + max_velocity: 1.5 + - type: acceleration + frame: look_ahead_point_stabilized + x: -0.707 + y: 0 + z: -0.707 + dt: 0.2 + ht: 1.2 + max_velocity: 1.5 + + - type: acceleration + frame: look_ahead_point_stabilized + x: 0 + y: 0.707 + z: 0.707 + dt: 0.2 + ht: 1.2 + max_velocity: 1.5 + - type: acceleration + frame: look_ahead_point_stabilized + x: 0 + y: -0.707 + z: 0.707 + dt: 0.2 + ht: 1.2 + max_velocity: 1.5 + - type: acceleration + frame: look_ahead_point_stabilized + x: 0 + y: 0.707 + z: -0.707 + dt: 0.2 + ht: 1.2 + max_velocity: 1.5 + - type: acceleration + frame: look_ahead_point_stabilized + x: 0 + y: -0.707 + z: -0.707 + dt: 0.2 + ht: 1.2 + max_velocity: 1.5 diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/config/backup.yaml b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/config/backup.yaml new file mode 100644 index 00000000..e43ca589 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/config/backup.yaml @@ -0,0 +1,54 @@ +trajectories: + - type: curve + linear_velocity: 1 + angular_velocity: -45 + frame: tracking_point_stabilized + dt: 0.2 + time: 3 + yaw: heading + - type: curve + linear_velocity: 1 + angular_velocity: -30 + frame: tracking_point_stabilized + dt: 0.2 + time: 3 + yaw: heading + - type: curve + linear_velocity: 1 + angular_velocity: -15 + frame: tracking_point_stabilized + dt: 0.2 + time: 3 + yaw: heading + - type: curve + linear_velocity: 1 + angular_velocity: 0 + frame: tracking_point_stabilized + dt: 0.2 + time: 3 + yaw: heading + - type: curve + linear_velocity: 1 + angular_velocity: 15 + frame: tracking_point_stabilized + dt: 0.2 + time: 3 + yaw: heading + - type: curve + linear_velocity: 1 + angular_velocity: 30 + frame: tracking_point_stabilized + dt: 0.2 + time: 3 + yaw: heading + - type: curve + linear_velocity: 1 + angular_velocity: 45 + frame: tracking_point_stabilized + dt: 0.2 + time: 3 + yaw: heading + - type: acceleration + frame: tracking_point_stabilized + x: 2 + yaw: heading \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/config/demo_trajectory_definitions.yaml b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/config/demo_trajectory_definitions.yaml new file mode 100644 index 00000000..6417bb27 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/config/demo_trajectory_definitions.yaml @@ -0,0 +1,78 @@ +trajectories: + - type: curve + linear_velocity: 0.2 + angular_velocity: -45 + frame: look_ahead_point_stabilized + dt: 0.2 + time: 3 + yaw: heading + - type: curve + linear_velocity: 0.2 + angular_velocity: -30 + frame: look_ahead_point_stabilized + dt: 0.2 + time: 3 + yaw: heading + - type: curve + linear_velocity: 0.2 + angular_velocity: -22 + frame: look_ahead_point_stabilized + dt: 0.2 + time: 3 + yaw: heading + - type: curve + linear_velocity: 0.2 + angular_velocity: -15 + frame: look_ahead_point_stabilized + dt: 0.2 + time: 3 + yaw: heading + - type: curve + linear_velocity: 0.2 + angular_velocity: -5 + frame: look_ahead_point_stabilized + dt: 0.2 + time: 3 + yaw: heading + - type: curve + linear_velocity: 0.2 + angular_velocity: 0 + frame: look_ahead_point_stabilized + dt: 0.2 + time: 3 + yaw: heading + - type: curve + linear_velocity: 0.2 + angular_velocity: 5 + frame: look_ahead_point_stabilized + dt: 0.2 + time: 3 + yaw: heading + - type: curve + linear_velocity: 0.2 + angular_velocity: 15 + frame: look_ahead_point_stabilized + dt: 0.2 + time: 3 + yaw: heading + - type: curve + linear_velocity: 0.2 + angular_velocity: 22 + frame: look_ahead_point_stabilized + dt: 0.2 + time: 3 + yaw: heading + - type: curve + linear_velocity: 0.2 + angular_velocity: 30 + frame: look_ahead_point_stabilized + dt: 0.2 + time: 3 + yaw: heading + - type: curve + linear_velocity: 0.2 + angular_velocity: 45 + frame: look_ahead_point_stabilized + dt: 0.2 + time: 3 + yaw: heading \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/config/fixed_trajectories.yaml b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/config/fixed_trajectories.yaml new file mode 100644 index 00000000..64c1f54d --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/config/fixed_trajectories.yaml @@ -0,0 +1,38 @@ +trajectories: + - Figure8: + attributes: + - frame_id + - velocity + - max_acceleration + - length + - width + - height + - Racetrack: + attributes: + - frame_id + - velocity + - max_acceleration + - length + - width + - height + - Circle: + attributes: + - frame_id + - velocity + - radius + - Line: + attributes: + - frame_id + - velocity + - max_acceleration + - length + - width + - height + - Point: + attributes: + - frame_id + - velocity + - max_acceleration + - x + - y + - height diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/config/test.yaml b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/config/test.yaml new file mode 100644 index 00000000..b63102ac --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/config/test.yaml @@ -0,0 +1,1298 @@ +--- +trajectories: + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 0 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 180 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 0 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 180 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 0 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 180 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 0 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 180 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 0 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 180 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 0 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: 180 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 0 + magnitude_yaw: -157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 0 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: 180 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 15 + magnitude_yaw: -157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 0 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: 180 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -15 + magnitude_yaw: -157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 0 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: 180 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 30 + magnitude_yaw: -157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 0 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: 180 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -22.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -45 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -67.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -90 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -112.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -135 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -30 + magnitude_yaw: -157.5 + max_velocity: $(param max_velocity) + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: 90 + magnitude_yaw: 0 + max_velocity: 0.3 + type: acceleration + - dt: $(param dt) + frame: look_ahead_point_stabilized + ht: $(param ht_long) + magnitude: $(param magnitude) + magnitude_pitch: -90 + magnitude_yaw: 0 + max_velocity: 0.3 + type: acceleration \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/include/trajectory_library/trajectory_library.hpp b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/include/trajectory_library/trajectory_library.hpp new file mode 100644 index 00000000..7eb8c56f --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/include/trajectory_library/trajectory_library.hpp @@ -0,0 +1,305 @@ +#ifndef _TRJAECTORY_LIBRARY_H_ +#define _TRJAECTORY_LIBRARY_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +class Trajectory; + +class Waypoint { + private: + double x_, y_, z_, yaw_, vx_, vy_, vz_, ax_, ay_, az_, jx_, jy_, jz_, time_; + + public: + Waypoint(double x, double y, double z, double yaw, double vx, double vy, double vz, double ax, + double ay, double az, double jx, double jy, double jz, double time = 0); + + double get_x() const { return x_; } + double get_y() const { return y_; } + double get_z() const { return z_; } + double get_vx() const { return vx_; } + double get_vy() const { return vy_; } + double get_vz() const { return vz_; } + double get_ax() const { return ax_; } + double get_ay() const { return ay_; } + double get_az() const { return az_; } + double get_jx() const { return jx_; } + double get_jy() const { return jy_; } + double get_jz() const { return jz_; } + double get_yaw() const { return yaw_; } + double get_time() const { return time_; } + + void set_time(double time) { time_ = time; } + + tf2::Quaternion quaternion() const; + tf2::Vector3 position() const; + tf2::Vector3 velocity() const; + tf2::Vector3 acceleration() const; + tf2::Vector3 jerk() const; + + /** + * @brief Convert Waypoint to Odometry message + */ + airstack_msgs::msg::Odometry as_odometry_msg(rclcpp::Time stamp, std::string frame_id) const; + + Waypoint interpolate(Waypoint wp, double t); + + friend class Trajectory; + + friend std::ostream& operator<<(std::ostream& os, const Waypoint& wp); +}; + +// Create one TransformListener instance to be used by every Trajectory instance +// TODO: change to smart pointer +static tf2_ros::Buffer* buffer = NULL; +static tf2_ros::TransformListener* listener = NULL; + +class Trajectory { + private: + std::string frame_id; + rclcpp::Time stamp; + std::vector waypoints; + + bool generated_waypoint_times; + void generate_waypoint_times(); + + // void init_listener(); + + std::string marker_namespace; + + public: + Trajectory(); + Trajectory(rclcpp::Node* node_ptr, std::string frame_id); + Trajectory(rclcpp::Node* node_ptr, airstack_msgs::msg::TrajectoryXYZVYaw path); + Trajectory(rclcpp::Node* node_ptr, nav_msgs::msg::Path path); + // Trajectory(airstack_msgs::msg::TrajectoryXYZVYaw path); + + void clear(); + bool get_closest_point(tf2::Vector3 point, Waypoint* closest, int* wp_index = NULL, + double* path_distance = NULL); + bool get_trajectory_distance_at_closest_point(tf2::Vector3 point, double* trajectory_distance); + bool merge(Trajectory traj, double min_time = 0.0); + + bool get_closest_waypoint(tf2::Vector3 point, double initial_time, double end_time, + Waypoint* waypoint); + bool get_waypoint_distance_ahead(double initial_time, double distance, Waypoint* waypoint); + bool get_waypoint_sphere_intersection(double initial_time, double ahead_distance, + double time_end, tf2::Vector3 sphere_center, + double sphere_radius, double min_velocity, + Waypoint* waypoint, Waypoint* end_waypoint); + bool get_waypoint(double time, Waypoint* waypoint); + + const std::vector& get_waypoints() const; + + double get_duration(); + bool get_odom(double time, airstack_msgs::msg::Odometry* odom, rclcpp::Time stamp); + Trajectory to_frame(std::string target_frame, rclcpp::Time time); + // Trajectory respace(double spacing); + // Trajectory shorten(double new_length); // replace shorten with get_subtraj_dist + Trajectory trim_trajectory_between_distances(double start_dist, double end_dist); + Trajectory get_reversed_trajectory(); + float get_skip_ahead_time(float start_time, float max_velocity, float max_distance); + + void set_fixed_height(double height); + + size_t get_num_waypoints() const; + const Waypoint& get_waypoint(int index) const; + const std::string& get_frame_id() const; + const rclcpp::Time& get_stamp() const { return stamp; } + airstack_msgs::msg::TrajectoryXYZVYaw get_TrajectoryXYZVYaw_msg(); + std::vector get_vector_PointStamped_msg(); + + visualization_msgs::msg::MarkerArray get_markers(rclcpp::Time stamp, + const std::string& marker_namespace, + float r = 1, float g = 0, float b = 0, + float a = 1, bool show_poses = false, + bool show_velocity = false, + float thickness = 0.03f); +}; + +//=================================================================================== +//--------------------------------- Dynamic Trajectories ---------------------------- +//=================================================================================== + +class DynamicTrajectory { + private: + public: + virtual airstack_msgs::msg::TrajectoryXYZVYaw get_trajectory( + airstack_msgs::msg::Odometry odom) = 0; +}; + +class AccelerationTrajectory : public DynamicTrajectory { + private: + // subscribers + // tf2_ros::Buffer* buffer; + // ros::Subscriber set_max_velocity_sub; + + double ax, ay, az; + double dt, ht; + double max_velocity; + std::string frame; + + // callbacks + // void set_max_velocity_callback(std_msgs::Float32 msg); + + public: + AccelerationTrajectory(tf2_ros::Buffer* buffer, std::string frame, double ax, double ay, + double az, double dt, double ht, double max_velocity); + virtual airstack_msgs::msg::TrajectoryXYZVYaw get_trajectory(airstack_msgs::msg::Odometry odom); +}; + +class TakeoffTrajectory : public DynamicTrajectory { + private: + double height, velocity; + double path_roll, path_pitch; + bool relative_to_orientation; + + public: + TakeoffTrajectory(double height, double velocity, double path_roll = 0., double path_pitch = 0., + bool relative_to_orientation = false); + airstack_msgs::msg::TrajectoryXYZVYaw get_trajectory(airstack_msgs::msg::Odometry odom); +}; + +//=================================================================================== +//--------------------------------- Static Trajectories ----------------------------- +//=================================================================================== + +class StaticTrajectory { + private: + public: + virtual airstack_msgs::msg::TrajectoryXYZVYaw get_trajectory() = 0; +}; + +class CurveTrajectory : public StaticTrajectory { + private: + airstack_msgs::msg::TrajectoryXYZVYaw trajectory; + + float linear_velocity, angular_velocity, dt, time, distance, yaw; + std::string frame; + bool use_heading; + + void generate_trajectory(); + + public: + CurveTrajectory(float linear_velocity, float angular_velocity, std::string frame, float time, + float dt, bool use_heading, float yaw = 0.f); + + virtual airstack_msgs::msg::TrajectoryXYZVYaw get_trajectory(); +}; + +//=================================================================================== +//--------------------------------- Trajectory Library ------------------------------ +//=================================================================================== + +class TrajectoryLibrary { + private: + std::vector static_trajectories; + std::vector dynamic_trajectories; + rclcpp::Node::SharedPtr node_ptr; + + public: + TrajectoryLibrary(std::string config_filename, + rclcpp::Node::SharedPtr node_ptr); // tf2_ros::Buffer* + + std::vector get_static_trajectories(); + std::vector get_dynamic_trajectories(airstack_msgs::msg::Odometry odom); + visualization_msgs::msg::MarkerArray get_markers( + std::vector trajectories); + + private: + std::string trim(std::string str) { + int trim_start = 0; + int trim_end = 0; + for (size_t i = 0; i < str.size(); i++) + if (std::isspace(str[i])) + trim_start++; + else + break; + + for (int i = str.size() - 1; i >= 0; i--) + if (std::isspace(str[i])) + trim_end++; + else + break; + + std::string trimmed = str.substr(trim_start, str.size() - trim_end - trim_start); + return trimmed; + } + + template + std::string get_string(T t) { + return std::to_string(t); + } + + std::string get_string(std::string t) { return t; } + + template + T parse(YAML::Node yamlnode) { + // std::cout << "trim test |" << trim(" asdjfaksfdj jdfkj jakdsf ") << "|" << std::endl; + + std::string str = yamlnode.as(); + std::size_t start; + + std::string param_label = "$(param"; + + // std::cout << "string: " << str << std::endl; + while ((start = str.find(param_label)) != std::string::npos) { + std::size_t end = str.find(")"); + // std::cout << start << " " << end << std::endl; + if (end == std::string::npos) { + RCLCPP_DEBUG_STREAM( + node_ptr->get_logger(), + "TRAJECTORY LIBRARY PARSING ERROR: No closing parenthesis for $(param"); + break; + } + + std::string parameter_name = + trim(str.substr(start + param_label.size(), end - (start + param_label.size()))); + RCLCPP_DEBUG(node_ptr->get_logger(), "parameter_name: %s", parameter_name.c_str()); + T parameter_value; + if (!this->node_ptr->get_parameter(parameter_name, parameter_value)) { + RCLCPP_INFO(node_ptr->get_logger(), + "Couldn't find parameter '%s'. This is either because it doesn't exist " + "or the type is incorrect in the launch file.", + parameter_name.c_str()); + } + RCLCPP_DEBUG_STREAM(node_ptr->get_logger(), "parameter_value: " << parameter_value); + std::cout << str << " " << start << " " << end << std::endl; + str = str.substr(0, start) + get_string(parameter_value) + + str.substr(end + 1, str.size() - (end + 1)); + std::cout << str << " " << str.find(param_label) << std::endl; + RCLCPP_DEBUG(node_ptr->get_logger(), "str: %s", str.c_str()); + } + + // std::cout << "final: " << str << std::endl; + YAML::Emitter out; + out << YAML::BeginMap; + out << YAML::Key << "key"; + out << YAML::Value << str; + out << YAML::EndMap; + std::string yaml = out.c_str(); + YAML::Node n = YAML::Load(out.c_str()); + // std::cout << "yaml: " << yaml << " " << n["key"].as() << std::endl; + + return n["key"].as(); + } +}; + +#endif diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/package.xml b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/package.xml new file mode 100644 index 00000000..fe463741 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/package.xml @@ -0,0 +1,28 @@ + + + + trajectory_library + 0.0.0 + TODO: Package description + uav + TODO: License declaration + + ament_cmake + + tf2 + tf2_ros + tf2_tools + rclcpp + nav_msgs + visualization_msgs + geometry_msgs + airstack_msgs + airstack_common + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/plugin.xml b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/plugin.xml new file mode 100644 index 00000000..ba492325 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/plugin.xml @@ -0,0 +1,15 @@ + + + + An interface for choosing fixed trajectories. + + + + + + + system-help + Fixed Trajectory Selector. + + + diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/scripts/fixed_trajectory_generator.py b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/scripts/fixed_trajectory_generator.py new file mode 100644 index 00000000..0675cc68 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/scripts/fixed_trajectory_generator.py @@ -0,0 +1,456 @@ +#!/usr/bin/python3 +import numpy as np +from nav_msgs.msg import Odometry +from geometry_msgs.msg import Point +from airstack_msgs.msg import WaypointXYZVYaw +from airstack_msgs.msg import TrajectoryXYZVYaw +from airstack_msgs.msg import FixedTrajectory +import time +import copy +import rclpy +from rclpy.node import Node + + +def get_velocities(traj, velocity, max_acc): + v_prev = 0.0 + + for i in range(len(traj.waypoints)): + j = (i + 1) % len(traj.waypoints) + dx = traj.waypoints[j].position.x - traj.waypoints[i].position.x + dy = traj.waypoints[j].position.y - traj.waypoints[i].position.y + + dist = np.sqrt(dx**2 + dy**2) + v_limit = np.sqrt(v_prev**2 + 2 * max_acc * dist) + traj.waypoints[i].velocity = min(velocity, v_limit) + v_prev = traj.waypoints[i].velocity + + +def get_velocities_dual(traj, velocity, max_acc): + v_prev = 0.0 + + for i in range(len(traj.waypoints)): + j = (i + 1) % len(traj.waypoints) + dx = traj.waypoints[j].position.x - traj.waypoints[i].position.x + dy = traj.waypoints[j].position.y - traj.waypoints[i].position.y + + dist = np.sqrt(dx**2 + dy**2) + v_limit = np.sqrt(v_prev**2 + 2 * max_acc * dist) + traj.waypoints[i].velocity = min(velocity, v_limit) + v_prev = traj.waypoints[i].velocity + + v_prev = 0.0 + for i in range(len(traj.waypoints) - 1, int(len(traj.waypoints) * 0.85), -1): + j = (i - 1) % len(traj.waypoints) + dx = traj.waypoints[j].position.x - traj.waypoints[i].position.x + dy = traj.waypoints[j].position.y - traj.waypoints[i].position.y + + dist = np.sqrt(dx**2 + dy**2) + v_limit = np.sqrt(v_prev**2 + 2 * max_acc * dist) + traj.waypoints[i].velocity = min(velocity, v_limit) + v_prev = traj.waypoints[i].velocity + + traj.waypoints[len(traj.waypoints) - 1].velocity = 0.0 + + +def get_accelerations(traj): + a_prev = 0.0 + + for i in range(len(traj.waypoints) - 2): + j = (i + 1) % len(traj.waypoints) + k = (i + 2) % len(traj.waypoints) + + dx1 = traj.waypoints[j].position.x - traj.waypoints[i].position.x + dy1 = traj.waypoints[j].position.y - traj.waypoints[i].position.y + dist1 = np.sqrt(dx1**2 + dy1**2) + + v_currx = traj.waypoints[i].velocity * (dx1) / dist1 + v_curry = traj.waypoints[i].velocity * (dy1) / dist1 + + dx2 = traj.waypoints[k].position.x - traj.waypoints[j].position.x + dy2 = traj.waypoints[k].position.y - traj.waypoints[j].position.y + dist2 = np.sqrt(dx2**2 + dy2**2) + + v_nextx = traj.waypoints[j].velocity * (dx2) / dist2 + v_nexty = traj.waypoints[j].velocity * (dy2) / dist2 + + acc_limit = 50 + if abs(dx1 - 0) < 1e-6: + traj.waypoints[i].acceleration.x = 0.0 + else: + traj.waypoints[i].acceleration.x = min( + (v_nextx**2 - v_currx**2) / (2 * dx1), acc_limit + ) + + if abs(dy1 - 0) < 1e-6: + traj.waypoints[i].acceleration.y = 0.0 + else: + traj.waypoints[i].acceleration.y = min( + (v_nexty**2 - v_curry**2) / (2 * dy1), acc_limit + ) + + traj.waypoints[i].acceleration.z = 0.0 + + traj.waypoints[len(traj.waypoints) - 2].acceleration.x = 0.0 + traj.waypoints[len(traj.waypoints) - 2].acceleration.y = 0.0 + traj.waypoints[len(traj.waypoints) - 2].acceleration.z = 0.0 + traj.waypoints[len(traj.waypoints) - 1].acceleration.x = 0.0 + traj.waypoints[len(traj.waypoints) - 1].acceleration.y = 0.0 + traj.waypoints[len(traj.waypoints) - 1].acceleration.z = 0.0 + + +def get_racetrack_waypoints(attributes): # length, width, height): + frame_id = str(attributes["frame_id"]) + length = float(attributes["length"]) + width = float(attributes["width"]) + height = float(attributes["height"]) + velocity = float(attributes["velocity"]) + max_acceleration = float(attributes["max_acceleration"]) + + traj = TrajectoryXYZVYaw() + traj.header.frame_id = frame_id + straightaway_length = length - width + + # first straightaway + xs1 = np.linspace(0, straightaway_length, 80) + ys1 = np.zeros(xs1.shape) + yaws1 = np.zeros(xs1.shape) + + # first turn + t = np.linspace(-np.pi / 2, np.pi / 2, 50)[1:-1] + xs2 = width / 2.0 * np.cos(t) + straightaway_length + ys2 = width / 2.0 * np.sin(t) + width / 2.0 + xs2d = -width / 2.0 * np.sin(t) # derivative of xs + ys2d = width / 2.0 * np.cos(t) # derivative of ys + yaws2 = np.arctan2(ys2d, xs2d) + + # second straightaway + xs3 = np.linspace(straightaway_length, 0, 80) + ys3 = width * np.ones(xs3.shape) + yaws3 = np.pi * np.ones(xs3.shape) + + # second turn + t = np.linspace(np.pi / 2, 3 * np.pi / 2, 50)[1:-1] + xs4 = width / 2.0 * np.cos(t) + ys4 = width / 2.0 * np.sin(t) + width / 2.0 + yaws4 = yaws2 + np.pi + + xs = np.hstack((xs1, xs2, xs3, xs4)) + ys = np.hstack((ys1, ys2, ys3, ys4)) + yaws = np.hstack((yaws1, yaws2, yaws3, yaws4)) + + # now = rospy.Time.now() + for i in range(xs.shape[0]): + wp = WaypointXYZVYaw() + wp.position.x = xs[i] + wp.position.y = ys[i] + wp.position.z = height + wp.yaw = yaws[i] + + traj.waypoints.append(wp) + + get_velocities_dual(traj, velocity, max_acceleration) + get_accelerations(traj) + + return traj + + +def get_figure8_waypoints(attributes): # length, width, height): + frame_id = str(attributes["frame_id"]) + length = float(attributes["length"]) + width = float(attributes["width"]) + height = float(attributes["height"]) + velocity = float(attributes["velocity"]) + max_acceleration = float(attributes["max_acceleration"]) + + traj = TrajectoryXYZVYaw() + traj.header.frame_id = frame_id + # now = rospy.Time.now() + + # figure 8 points + t = np.linspace(0, 2 * np.pi, 600) + x = np.cos(t) * length - length + y = np.cos(t) * np.sin(t) * 2 * width + + # derivative of figure 8 curve, used to find yaw + xd = -np.sin(t) * length + yd = (np.cos(t) ** 2 - np.sin(t) ** 2) * 2 * width + + for i in range(t.shape[0] - 1): + x1 = x[i] + y1 = y[i] + x2 = x1 + xd[i] + y2 = y1 + yd[i] + + yaw = np.arctan2(y2 - y1, x2 - x1) + + wp = WaypointXYZVYaw() + wp.position.x = x1 + wp.position.y = y1 + wp.position.z = height + wp.yaw = yaw + + traj.waypoints.append(wp) + + get_velocities_dual(traj, velocity, max_acceleration) + get_accelerations(traj) + + return traj + + +def get_line_waypoints(attributes): # length, height): + frame_id = str(attributes["frame_id"]) + length = float(attributes["length"]) + height = float(attributes["height"]) + velocity = float(attributes["velocity"]) + max_acceleration = float(attributes["max_acceleration"]) + + traj = TrajectoryXYZVYaw() + traj.header.frame_id = frame_id + + for y in np.arange(0, -length, -0.5): + wp = WaypointXYZVYaw() + wp.position.x = -y + wp.position.y = 0 + wp.position.z = height + wp.yaw = 0 + + traj.waypoints.append(wp) + + get_velocities(traj, velocity, max_acceleration) + + return traj + + +def get_point_waypoints(attributes): # length, height): + frame_id = str(attributes["frame_id"]) + x = float(attributes["x"]) + y = float(attributes["y"]) + height = float(attributes["height"]) + velocity = float(attributes["velocity"]) + max_acceleration = float(attributes["max_acceleration"]) + + traj = TrajectoryXYZVYaw() + traj.header.frame_id = frame_id + + # add first point + wp = WaypointXYZVYaw() + wp.position.x = x + wp.position.y = y + wp.position.z = height + wp.yaw = 0 + + traj.waypoints.append(wp) + + get_velocities(traj, velocity, max_acceleration) + + return traj + + +def get_box_waypoints(attributes): # length, height): + frame_id = str(attributes["frame_id"]) + length = float(attributes["length"]) + height = float(attributes["height"]) + velocity = float(attributes["velocity"]) + max_acceleration = float(attributes["max_acceleration"]) + + traj = TrajectoryXYZVYaw() + traj.header.frame_id = frame_id + + wp1 = WaypointXYZVYaw() + wp1.position.x = 0.0 + wp1.position.y = 0.0 + wp1.position.z = height + wp1.yaw = 0 + traj.waypoints.append(wp1) + + wp2 = WaypointXYZVYaw() + wp2.position.x = length + wp2.position.y = 0.0 + wp2.position.z = height + wp2.yaw = 0.0 + traj.waypoints.append(wp2) + + wp3 = WaypointXYZVYaw() + wp3.position.x = length + wp3.position.y = 0.0 + wp3.position.z = height + height + wp3.yaw = 0.0 + traj.waypoints.append(wp3) + + wp4 = WaypointXYZVYaw() + wp4.position.x = 0.0 + wp4.position.y = 0.0 + wp4.position.z = height + height + wp4.yaw = 0.0 + traj.waypoints.append(wp4) + + return traj + + +def get_vertical_lawnmower_waypoints(attributes): # length, width, height, velocity): + frame_id = str(attributes["frame_id"]) + length = float(attributes["length"]) + width = float(attributes["width"]) + height = float(attributes["height"]) + velocity = float(attributes["velocity"]) + + traj = TrajectoryXYZVYaw() + traj.header.frame_id = frame_id + + for i in range(abs(int(height / width))): + wp1 = WaypointXYZVYaw() + wp1.position.x = 0.0 + wp1.position.y = 0.0 + wp1.position.z = np.sign(height) * (i + 1) * width + wp1.yaw = 0.0 + wp1.velocity = 0.1 + + wp1_ = WaypointXYZVYaw() + wp1_.position.x = 0.0 + wp1_.position.y = 0.5 + wp1_.position.z = np.sign(height) * (i + 1) * width + wp1_.yaw = 0.0 + wp1_.velocity = velocity + + wp2 = WaypointXYZVYaw() + wp2.position.x = 0.0 + wp2.position.y = length + wp2.position.z = np.sign(height) * (i + 1) * width + wp2.yaw = 0.0 + wp2.velocity = 0.1 + + wp2_ = WaypointXYZVYaw() + wp2_.position.x = 0.0 + wp2_.position.y = length - 0.5 + wp2_.position.z = np.sign(height) * (i + 1) * width + wp2_.yaw = 0.0 + wp2_.velocity = velocity + + if i % 2 == 0: + traj.waypoints.append(wp1) + wp1_slow = copy.deepcopy(wp1_) + wp1_slow.velocity = 0.1 + traj.waypoints.append(wp1_slow) + traj.waypoints.append(wp1_) + traj.waypoints.append(wp2_) + traj.waypoints.append(wp2) + else: + traj.waypoints.append(wp2) + wp2_slow = copy.deepcopy(wp2_) + wp2_slow.velocity = 0.1 + traj.waypoints.append(wp2_slow) + traj.waypoints.append(wp2_) + traj.waypoints.append(wp1_) + traj.waypoints.append(wp1) + + wp = WaypointXYZVYaw() + wp.position.x = 0.0 + wp.position.y = 0.0 + wp.position.z = 0.0 + wp.yaw = 0.0 + wp.velocity = 0.5 + traj.waypoints.append(wp) + + return traj + + +def get_circle_waypoints(attributes): # radius, velocity, frame_id): + frame_id = str(attributes["frame_id"]) + radius = float(attributes["radius"]) + velocity = float(attributes["velocity"]) + + traj = TrajectoryXYZVYaw() + traj.header.frame_id = frame_id + # traj.header.stamp = rospy.Time.now() + + wp0 = WaypointXYZVYaw() + wp0.position.x = 0.0 + wp0.position.y = 0.0 + wp0.position.z = 0.0 + wp0.yaw = 0.0 + wp0.velocity = velocity + traj.waypoints.append(wp0) + + wp1 = WaypointXYZVYaw() + wp1.position.x = radius + wp1.position.y = 0.0 + wp1.position.z = 0.0 + wp1.yaw = 0.0 + wp1.velocity = velocity + traj.waypoints.append(wp1) + + for angle in np.arange(0, 2 * np.pi, 10.0 * np.pi / 180.0): + wp = WaypointXYZVYaw() + wp.position.x = radius * np.cos(angle) + wp.position.y = radius * np.sin(angle) + wp.position.z = 0.0 + wp.yaw = 0.0 + wp.velocity = velocity + traj.waypoints.append(wp) + + wp_end0 = WaypointXYZVYaw() + wp_end0.position.x = radius + wp_end0.position.y = 0.0 + wp_end0.position.z = 0.0 + wp_end0.yaw = 0.0 + wp_end0.velocity = velocity + traj.waypoints.append(wp_end0) + + wp_end1 = WaypointXYZVYaw() + wp_end1.position.x = 0.0 + wp_end1.position.y = 0.0 + wp_end1.position.z = 0.0 + wp_end1.yaw = 0.0 + wp_end1.velocity = velocity + traj.waypoints.append(wp_end1) + + return traj + + +class FixedTrajectoryGenerator(Node): + def __init__(self): + super().__init__("fixed_trajectory_generator") + self.fixed_trajectory_sub = self.create_subscription( + FixedTrajectory, + "fixed_trajectory_command", + self.fixed_trajectory_callback, + 1, + ) + self.trajectory_override_pub = self.create_publisher( + TrajectoryXYZVYaw, "trajectory_override", 1 + ) + + def fixed_trajectory_callback(self, msg): + print("generating") + attributes = {} + + for key_value in msg.attributes: + attributes[key_value.key] = key_value.value + + trajectory_msg = None + + if msg.type == "Figure8": + trajectory_msg = get_figure8_waypoints(attributes) + elif msg.type == "Circle": + trajectory_msg = get_circle_waypoints(attributes) + elif msg.type == "Racetrack": + trajectory_msg = get_racetrack_waypoints(attributes) + elif msg.type == "Line": + trajectory_msg = get_line_waypoints(attributes) + elif msg.type == "Point": + trajectory_msg = get_point_waypoints(attributes) + + if trajectory_msg != None: + self.trajectory_override_pub.publish(trajectory_msg) + else: + print("No trajectory sent.") + + +if __name__ == "__main__": + rclpy.init(args=None) + + node = FixedTrajectoryGenerator() + + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/scripts/rqt_fixed_trajectory_selector b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/scripts/rqt_fixed_trajectory_selector new file mode 100644 index 00000000..4d495110 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/scripts/rqt_fixed_trajectory_selector @@ -0,0 +1,10 @@ +#!/usr/bin/env python + +import sys + +from rqt_fixed_trajectory_selector.rqt_fixed_trajectory_selector import FixedTrajectorySelectorPlugin +from rqt_gui.main import Main + +plugin = 'rqt_fixed_trajectory_selector' +main = Main(filename=plugin) +sys.exit(main.main(standalone=plugin)) diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/setup.py b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/setup.py new file mode 100644 index 00000000..d8e789de --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/setup.py @@ -0,0 +1,9 @@ +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup( + packages=['rqt_fixed_trajectory_selector'], + package_dir={'': 'src'}, +) + +setup(**d) diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/src/fixed_trajectory_generator.py b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/src/fixed_trajectory_generator.py new file mode 100644 index 00000000..b13f7e7c --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/src/fixed_trajectory_generator.py @@ -0,0 +1,453 @@ +#!/usr/bin/python +import numpy as np +from nav_msgs.msg import Odometry +from geometry_msgs.msg import Point +import rospy +import tf.transformations as trans +import argparse + +# from at_fcs_mavros.msg import Trajectory +# from at_fcs_mavros.msg import Waypoint +# from ca_nav_msgs.msg import XYZVPsi +# from ca_nav_msgs.msg import PathXYZVPsi +from airstack_msgs.msg import WaypointXYZVYaw +from airstack_msgs.msg import TrajectoryXYZVYaw +from trajectory_controller.msg import Trajectory +from airstack_msgs.msg import FixedTrajectory +import time +import copy + + +def get_velocities(traj, velocity, max_acc): + v_prev = 0.0 + + for i in range(len(traj.waypoints)): + j = (i + 1) % len(traj.waypoints) + dx = traj.waypoints[j].position.x - traj.waypoints[i].position.x + dy = traj.waypoints[j].position.y - traj.waypoints[i].position.y + + dist = np.sqrt(dx**2 + dy**2) + v_limit = np.sqrt(v_prev**2 + 2 * max_acc * dist) + traj.waypoints[i].velocity = min(velocity, v_limit) + v_prev = traj.waypoints[i].velocity + + +def get_velocities_dual(traj, velocity, max_acc): + v_prev = 0.0 + + for i in range(len(traj.waypoints)): + j = (i + 1) % len(traj.waypoints) + dx = traj.waypoints[j].position.x - traj.waypoints[i].position.x + dy = traj.waypoints[j].position.y - traj.waypoints[i].position.y + + dist = np.sqrt(dx**2 + dy**2) + v_limit = np.sqrt(v_prev**2 + 2 * max_acc * dist) + traj.waypoints[i].velocity = min(velocity, v_limit) + v_prev = traj.waypoints[i].velocity + + v_prev = 0.0 + for i in range(len(traj.waypoints) - 1, int(len(traj.waypoints) * 0.85), -1): + j = (i - 1) % len(traj.waypoints) + dx = traj.waypoints[j].position.x - traj.waypoints[i].position.x + dy = traj.waypoints[j].position.y - traj.waypoints[i].position.y + + dist = np.sqrt(dx**2 + dy**2) + v_limit = np.sqrt(v_prev**2 + 2 * max_acc * dist) + traj.waypoints[i].velocity = min(velocity, v_limit) + v_prev = traj.waypoints[i].velocity + + traj.waypoints[len(traj.waypoints) - 1].velocity = 0 + + +def get_accelerations(traj): + a_prev = 0.0 + + for i in range(len(traj.waypoints) - 2): + j = (i + 1) % len(traj.waypoints) + k = (i + 2) % len(traj.waypoints) + + dx1 = traj.waypoints[j].position.x - traj.waypoints[i].position.x + dy1 = traj.waypoints[j].position.y - traj.waypoints[i].position.y + dist1 = np.sqrt(dx1**2 + dy1**2) + + v_currx = traj.waypoints[i].velocity * (dx1) / dist1 + v_curry = traj.waypoints[i].velocity * (dy1) / dist1 + + dx2 = traj.waypoints[k].position.x - traj.waypoints[j].position.x + dy2 = traj.waypoints[k].position.y - traj.waypoints[j].position.y + dist2 = np.sqrt(dx2**2 + dy2**2) + + v_nextx = traj.waypoints[j].velocity * (dx2) / dist2 + v_nexty = traj.waypoints[j].velocity * (dy2) / dist2 + + acc_limit = 50 + if abs(dx1 - 0) < 1e-6: + traj.waypoints[i].acceleration.x = 0 + else: + traj.waypoints[i].acceleration.x = min( + (v_nextx**2 - v_currx**2) / (2 * dx1), acc_limit + ) + + if abs(dy1 - 0) < 1e-6: + traj.waypoints[i].acceleration.y = 0 + else: + traj.waypoints[i].acceleration.y = min( + (v_nexty**2 - v_curry**2) / (2 * dy1), acc_limit + ) + + traj.waypoints[i].acceleration.z = 0 + + traj.waypoints[len(traj.waypoints) - 2].acceleration.x = 0 + traj.waypoints[len(traj.waypoints) - 2].acceleration.y = 0 + traj.waypoints[len(traj.waypoints) - 2].acceleration.z = 0 + traj.waypoints[len(traj.waypoints) - 1].acceleration.x = 0 + traj.waypoints[len(traj.waypoints) - 1].acceleration.y = 0 + traj.waypoints[len(traj.waypoints) - 1].acceleration.z = 0 + + +def get_racetrack_waypoints(attributes): # length, width, height): + frame_id = str(attributes["frame_id"]) + length = float(attributes["length"]) + width = float(attributes["width"]) + height = float(attributes["height"]) + velocity = float(attributes["velocity"]) + max_acceleration = float(attributes["max_acceleration"]) + + traj = TrajectoryXYZVYaw() + traj.header.frame_id = frame_id + straightaway_length = length - width + + # first straightaway + xs1 = np.linspace(0, straightaway_length, 80) + ys1 = np.zeros(xs1.shape) + yaws1 = np.zeros(xs1.shape) + + # first turn + t = np.linspace(-np.pi / 2, np.pi / 2, 50)[1:-1] + xs2 = width / 2.0 * np.cos(t) + straightaway_length + ys2 = width / 2.0 * np.sin(t) + width / 2.0 + xs2d = -width / 2.0 * np.sin(t) # derivative of xs + ys2d = width / 2.0 * np.cos(t) # derivative of ys + yaws2 = np.arctan2(ys2d, xs2d) + + # second straightaway + xs3 = np.linspace(straightaway_length, 0, 80) + ys3 = width * np.ones(xs3.shape) + yaws3 = np.pi * np.ones(xs3.shape) + + # second turn + t = np.linspace(np.pi / 2, 3 * np.pi / 2, 50)[1:-1] + xs4 = width / 2.0 * np.cos(t) + ys4 = width / 2.0 * np.sin(t) + width / 2.0 + yaws4 = yaws2 + np.pi + + xs = np.hstack((xs1, xs2, xs3, xs4)) + ys = np.hstack((ys1, ys2, ys3, ys4)) + yaws = np.hstack((yaws1, yaws2, yaws3, yaws4)) + + now = rospy.Time.now() + for i in range(xs.shape[0]): + wp = WaypointXYZVYaw() + wp.position.x = xs[i] + wp.position.y = ys[i] + wp.position.z = height + wp.yaw = yaws[i] + + traj.waypoints.append(wp) + + get_velocities_dual(traj, velocity, max_acceleration) + get_accelerations(traj) + + return traj + + +def get_figure8_waypoints(attributes): # length, width, height): + frame_id = str(attributes["frame_id"]) + length = float(attributes["length"]) + width = float(attributes["width"]) + height = float(attributes["height"]) + velocity = float(attributes["velocity"]) + max_acceleration = float(attributes["max_acceleration"]) + + traj = TrajectoryXYZVYaw() + traj.header.frame_id = frame_id + now = rospy.Time.now() + + # figure 8 points + t = np.linspace(0, 2 * np.pi, 600) + x = np.cos(t) * length - length + y = np.cos(t) * np.sin(t) * 2 * width + + # derivative of figure 8 curve, used to find yaw + xd = -np.sin(t) * length + yd = (np.cos(t) ** 2 - np.sin(t) ** 2) * 2 * width + + for i in range(t.shape[0] - 1): + x1 = x[i] + y1 = y[i] + x2 = x1 + xd[i] + y2 = y1 + yd[i] + + yaw = np.arctan2(y2 - y1, x2 - x1) + + wp = WaypointXYZVYaw() + wp.position.x = x1 + wp.position.y = y1 + wp.position.z = height + wp.yaw = yaw + + traj.waypoints.append(wp) + + get_velocities_dual(traj, velocity, max_acceleration) + get_accelerations(traj) + + return traj + + +def get_line_waypoints(attributes): # length, height): + frame_id = str(attributes["frame_id"]) + length = float(attributes["length"]) + height = float(attributes["height"]) + velocity = float(attributes["velocity"]) + max_acceleration = float(attributes["max_acceleration"]) + + traj = TrajectoryXYZVYaw() + traj.header.frame_id = frame_id + + for y in np.arange(0, -length, -0.5): + wp = WaypointXYZVYaw() + wp.position.x = -y + wp.position.y = 0 + wp.position.z = height + wp.yaw = 0 + + traj.waypoints.append(wp) + + get_velocities(traj, velocity, max_acceleration) + + return traj + + +def get_point_waypoints(attributes): # length, height): + frame_id = str(attributes["frame_id"]) + x = float(attributes["x"]) + y = float(attributes["y"]) + height = float(attributes["height"]) + velocity = float(attributes["velocity"]) + max_acceleration = float(attributes["max_acceleration"]) + + traj = TrajectoryXYZVYaw() + traj.header.frame_id = frame_id + + # add first point + wp = WaypointXYZVYaw() + wp.position.x = x + wp.position.y = y + wp.position.z = height + wp.yaw = 0 + + traj.waypoints.append(wp) + + get_velocities(traj, velocity, max_acceleration) + + return traj + + +def get_box_waypoints(attributes): # length, height): + frame_id = str(attributes["frame_id"]) + length = float(attributes["length"]) + height = float(attributes["height"]) + velocity = float(attributes["velocity"]) + max_acceleration = float(attributes["max_acceleration"]) + + traj = TrajectoryXYZVYaw() + traj.header.frame_id = frame_id + + wp1 = WaypointXYZVYaw() + wp1.position.x = 0.0 + wp1.position.y = 0.0 + wp1.position.z = height + wp1.yaw = 0 + traj.waypoints.append(wp1) + + wp2 = WaypointXYZVYaw() + wp2.position.x = length + wp2.position.y = 0.0 + wp2.position.z = height + wp2.yaw = 0 + traj.waypoints.append(wp2) + + wp3 = WaypointXYZVYaw() + wp3.position.x = length + wp3.position.y = 0.0 + wp3.position.z = height + height + wp3.yaw = 0 + traj.waypoints.append(wp3) + + wp4 = WaypointXYZVYaw() + wp4.position.x = 0.0 + wp4.position.y = 0.0 + wp4.position.z = height + height + wp4.yaw = 0 + traj.waypoints.append(wp4) + + return traj + + +def get_vertical_lawnmower_waypoints(attributes): # length, width, height, velocity): + frame_id = str(attributes["frame_id"]) + length = float(attributes["length"]) + width = float(attributes["width"]) + height = float(attributes["height"]) + velocity = float(attributes["velocity"]) + + traj = TrajectoryXYZVYaw() + traj.header.frame_id = frame_id + + for i in range(abs(int(height / width))): + wp1 = WaypointXYZVYaw() + wp1.position.x = 0 + wp1.position.y = 0 + wp1.position.z = np.sign(height) * (i + 1) * width + wp1.yaw = 0 + wp1.velocity = 0.1 + + wp1_ = WaypointXYZVYaw() + wp1_.position.x = 0 + wp1_.position.y = 0.5 + wp1_.position.z = np.sign(height) * (i + 1) * width + wp1_.yaw = 0 + wp1_.velocity = velocity + + wp2 = WaypointXYZVYaw() + wp2.position.x = 0 + wp2.position.y = length + wp2.position.z = np.sign(height) * (i + 1) * width + wp2.yaw = 0 + wp2.velocity = 0.1 + + wp2_ = WaypointXYZVYaw() + wp2_.position.x = 0 + wp2_.position.y = length - 0.5 + wp2_.position.z = np.sign(height) * (i + 1) * width + wp2_.yaw = 0 + wp2_.velocity = velocity + + if i % 2 == 0: + traj.waypoints.append(wp1) + wp1_slow = copy.deepcopy(wp1_) + wp1_slow.velocity = 0.1 + traj.waypoints.append(wp1_slow) + traj.waypoints.append(wp1_) + traj.waypoints.append(wp2_) + traj.waypoints.append(wp2) + else: + traj.waypoints.append(wp2) + wp2_slow = copy.deepcopy(wp2_) + wp2_slow.velocity = 0.1 + traj.waypoints.append(wp2_slow) + traj.waypoints.append(wp2_) + traj.waypoints.append(wp1_) + traj.waypoints.append(wp1) + + wp = WaypointXYZVYaw() + wp.position.x = 0 + wp.position.y = 0 + wp.position.z = 0 + wp.yaw = 0 + wp.velocity = 0.5 + traj.waypoints.append(wp) + + return traj + + +def get_circle_waypoints(attributes): # radius, velocity, frame_id): + frame_id = str(attributes["frame_id"]) + radius = float(attributes["radius"]) + velocity = float(attributes["velocity"]) + + traj = TrajectoryXYZVYaw() + traj.header.frame_id = frame_id + traj.header.stamp = rospy.Time.now() + + wp0 = WaypointXYZVYaw() + wp0.position.x = 0 + wp0.position.y = 0 + wp0.position.z = 0 + wp0.yaw = 0 + wp0.velocity = velocity + traj.waypoints.append(wp0) + + wp1 = WaypointXYZVYaw() + wp1.position.x = radius + wp1.position.y = 0 + wp1.position.z = 0 + wp1.yaw = 0 + wp1.velocity = velocity + traj.waypoints.append(wp1) + + for angle in np.arange(0, 2 * np.pi, 10.0 * np.pi / 180.0): + wp = WaypointXYZVYaw() + wp.position.x = radius * np.cos(angle) + wp.position.y = radius * np.sin(angle) + wp.position.z = 0 + wp.yaw = 0 + wp.velocity = velocity + traj.waypoints.append(wp) + + wp_end0 = WaypointXYZVYaw() + wp_end0.position.x = radius + wp_end0.position.y = 0 + wp_end0.position.z = 0 + wp_end0.yaw = 0 + wp_end0.velocity = velocity + traj.waypoints.append(wp_end0) + + wp_end1 = WaypointXYZVYaw() + wp_end1.position.x = 0 + wp_end1.position.y = 0 + wp_end1.position.z = 0 + wp_end1.yaw = 0 + wp_end1.velocity = velocity + traj.waypoints.append(wp_end1) + + return traj + + +def fixed_trajectory_callback(msg): + attributes = {} + + for key_value in msg.attributes: + attributes[key_value.key] = key_value.value + + trajectory_msg = None + + if msg.type == "Figure8": + trajectory_msg = get_figure8_waypoints(attributes) + elif msg.type == "Circle": + trajectory_msg = get_circle_waypoints(attributes) + elif msg.type == "Racetrack": + trajectory_msg = get_racetrack_waypoints(attributes) + elif msg.type == "Line": + trajectory_msg = get_line_waypoints(attributes) + elif msg.type == "Point": + trajectory_msg = get_point_waypoints(attributes) + + if trajectory_msg != None: + trajectory_override_pub.publish(trajectory_msg) + else: + print("No trajectory sent.") + + +if __name__ == "__main__": + rospy.init_node("fixed_trajectory_generator") + + fixed_trajectory_sub = rospy.Subscriber( + "fixed_trajectory", FixedTrajectory, fixed_trajectory_callback + ) + + trajectory_override_pub = rospy.Publisher( + "trajectory_override", TrajectoryXYZVYaw, queue_size=1 + ) + + rospy.spin() diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/src/rqt_fixed_trajectory_selector/__init__.py b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/src/rqt_fixed_trajectory_selector/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/src/rqt_fixed_trajectory_selector/rqt_fixed_trajectory_selector.py b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/src/rqt_fixed_trajectory_selector/rqt_fixed_trajectory_selector.py new file mode 100644 index 00000000..a8fd1a8f --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/src/rqt_fixed_trajectory_selector/rqt_fixed_trajectory_selector.py @@ -0,0 +1,204 @@ +import os +import time +import rospy +import rospkg +from std_msgs.msg import String, Bool +from behavior_tree_msgs.msg import Status +import numpy as np +import yaml +import collections + +from qt_gui.plugin import Plugin +import python_qt_binding.QtWidgets as qt +import python_qt_binding.QtCore as core +import python_qt_binding.QtGui as gui + +from python_qt_binding import QT_BINDING, QT_BINDING_VERSION + +from python_qt_binding.QtCore import Slot, Qt, qVersion, qWarning, Signal +from python_qt_binding.QtGui import QColor +from python_qt_binding.QtWidgets import QWidget, QVBoxLayout, QSizePolicy + +from airstack_msgs.msg import FixedTrajectory +from diagnostic_msgs.msg import KeyValue + +class FixedTrajectorySelectorPlugin(Plugin): + def __init__(self, context): + super(FixedTrajectorySelectorPlugin, self).__init__(context) + self.setObjectName('FixedTrajectorySelectorPlugin') + + self.config_filename = '' + + self.button_dct = {} + self.attribute_settings = collections.OrderedDict() + + self.timer = rospy.Timer(rospy.Duration(1./10.), self.timer_callback) + + self.fixed_trajectory_pub = rospy.Publisher('fixed_trajectory_command', FixedTrajectory, queue_size=1) + self.global_plan_fixed_trajectory_pub = rospy.Publisher('global_plan_fixed_trajectory', FixedTrajectory, queue_size=1) + + # main layout + self.widget = QWidget() + self.vbox = qt.QVBoxLayout() + self.widget.setLayout(self.vbox) + context.add_widget(self.widget) + + # config widget + self.config_widget = qt.QWidget() + self.config_widget.setStyleSheet('QWidget{margin-left:-1px;}') + self.config_layout = qt.QHBoxLayout() + self.config_widget.setLayout(self.config_layout) + self.config_widget.setFixedHeight(50) + + self.config_button = qt.QPushButton('Open Config...') + self.config_button.clicked.connect(self.select_config_file) + self.config_layout.addWidget(self.config_button) + + self.config_label = qt.QLabel('config filename: ') + self.config_layout.addWidget(self.config_label) + self.vbox.addWidget(self.config_widget) + + # trajectory widget + self.trajectory_widget = qt.QWidget() + self.trajectory_layout = qt.QVBoxLayout() + self.trajectory_widget.setLayout(self.trajectory_layout) + self.vbox.addWidget(self.trajectory_widget) + + self.tab_widget = qt.QTabWidget() + self.trajectory_layout.addWidget(self.tab_widget) + + # button widget + self.button_widget = qt.QWidget() + self.button_layout = qt.QHBoxLayout() + self.button_widget.setLayout(self.button_layout) + self.vbox.addWidget(self.button_widget) + + self.publish_button = qt.QPushButton('Publish') + self.publish_button.clicked.connect(self.publish_trajectory) + self.button_layout.addWidget(self.publish_button) + + self.trajectory_type_label = qt.QLabel('Type: ') + self.button_layout.addWidget(self.trajectory_type_label) + + self.trajectory_type_combo_box = qt.QComboBox() + self.trajectory_type_combo_box.addItem('Fixed Trajectory') + self.trajectory_type_combo_box.addItem('Global Plan') + self.button_layout.addWidget(self.trajectory_type_combo_box) + + def publish_trajectory(self): + trajectory_type = self.trajectory_type_combo_box.currentText() + trajectory_name = self.tab_widget.tabText(self.tab_widget.currentIndex()) + msg = FixedTrajectory() + msg.type = trajectory_name + for attribute, value in self.attribute_settings[trajectory_name].iteritems(): + key_value = KeyValue() + key_value.key = attribute + key_value.value = value + msg.attributes.append(key_value) + if trajectory_type == 'Fixed Trajectory': + self.fixed_trajectory_pub.publish(msg) + elif trajectory_type == 'Global Plan': + self.global_plan_fixed_trajectory_pub.publish(msg) + + + def select_config_file(self): + starting_path = os.path.join(rospkg.RosPack().get_path('trajectory_library'), 'config') + filename = qt.QFileDialog.getOpenFileName(self.widget, 'Open Config File', starting_path, "Config Files (*.yaml)")[0] + self.set_config(filename) + + def set_config(self, filename): + if filename != '': + self.config_filename = filename + if self.config_filename != None: + self.config_label.setText('config filename: ' + self.config_filename) + self.init_buttons(filename) + + def init_buttons(self, filename): + y = yaml.load(open(filename, 'r').read()) + print(y) + + def get_attribute_changed_function(trajectory_name, attribute_name): + def attribute_changed(text): + if trajectory_name not in self.attribute_settings: + self.attribute_settings[trajectory_name] = {} + self.attribute_settings[trajectory_name][attribute_name] = text + return attribute_changed + + def get_publish_function(trajectory_name): + def publish_function(): + msg = FixedTrajectory() + msg.type = trajectory_name + for attribute, value in self.attribute_settings[trajectory_name].iteritems(): + key_value = KeyValue() + key_value.key = attribute + key_value.value = value + msg.attributes.append(key_value) + self.fixed_trajectory_pub.publish(msg) + return publish_function + + + for trajectory in y['trajectories']: + trajectory_name = trajectory.keys()[0] + attributes = trajectory[trajectory_name]['attributes'] + + trajectory_tab = qt.QWidget() + trajectory_layout = qt.QVBoxLayout() + trajectory_tab.setLayout(trajectory_layout) + + for attribute in attributes: + attribute_widget = qt.QWidget() + attribute_layout = qt.QHBoxLayout() + attribute_widget.setLayout(attribute_layout) + + attribute_label = qt.QLabel() + attribute_label.setText(attribute) + attribute_layout.addWidget(attribute_label) + + attribute_default = '0' + if attribute == 'frame_id': + attribute_default = 'world' + if trajectory_name in self.attribute_settings.keys(): + if attribute in self.attribute_settings[trajectory_name].keys(): + attribute_default = self.attribute_settings[trajectory_name][attribute] + + attribute_edit = qt.QLineEdit() + attribute_edit.textChanged.connect(get_attribute_changed_function(trajectory_name, + attribute)) + attribute_edit.setText(attribute_default) + + attribute_layout.addWidget(attribute_edit) + + trajectory_layout.addWidget(attribute_widget) + + #publish_button = qt.QPushButton('Publish') + #publish_button.clicked.connect(get_publish_function(trajectory_name)) + #trajectory_layout.addWidget(publish_button) + + self.tab_widget.addTab(trajectory_tab, trajectory_name) + + + + def timer_callback(self, msg): + bool_msg = Bool() + for key in self.button_dct.keys(): + bool_msg.data = self.button_dct[key]['data'] + self.button_dct[key]['publisher'].publish(bool_msg) + + def shutdown_plugin(self): + pass + + def save_settings(self, plugin_settings, instance_settings): + instance_settings.set_value('config_filename', self.config_filename) + instance_settings.set_value('attribute_settings', self.attribute_settings) + + def restore_settings(self, plugin_settings, instance_settings): + attribute_settings = instance_settings.value('attribute_settings') + if attribute_settings != None: + self.attribute_settings = attribute_settings + self.set_config(instance_settings.value('config_filename')) + + #def trigger_configuration(self): + # Comment in to signal that the plugin has a way to configure + # This will enable a setting button (gear icon) in each dock widget title bar + # Usually used to open a modal configuration dialog + diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/src/trajectory_library.cpp b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/src/trajectory_library.cpp new file mode 100644 index 00000000..9e71f5ad --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/src/trajectory_library.cpp @@ -0,0 +1,1308 @@ +#include + +Waypoint::Waypoint(double x, double y, double z, double yaw, double vx, double vy, double vz, + double ax, double ay, double az, double jx, double jy, double jz, double time) { + x_ = x; + y_ = y; + z_ = z; + vx_ = vx; + vy_ = vy; + vz_ = vz; + ax_ = ax; + ay_ = ay; + az_ = az; + jx_ = jx; + jy_ = jy; + jz_ = jz; + yaw_ = yaw; + time_ = time; +} + +tf2::Quaternion Waypoint::quaternion() const { + tf2::Quaternion quat; + quat.setRPY(0, 0, yaw_); + return quat; +} + +tf2::Vector3 Waypoint::position() const { return tf2::Vector3(x_, y_, z_); } + +tf2::Vector3 Waypoint::velocity() const { return tf2::Vector3(vx_, vy_, vz_); } + +tf2::Vector3 Waypoint::acceleration() const { return tf2::Vector3(ax_, ay_, az_); } + +tf2::Vector3 Waypoint::jerk() const { return tf2::Vector3(jx_, jy_, jz_); } + +airstack_msgs::msg::Odometry Waypoint::as_odometry_msg(rclcpp::Time stamp, + std::string frame_id) const { + airstack_msgs::msg::Odometry odom; + odom.header.stamp = stamp; + odom.header.frame_id = frame_id; + odom.child_frame_id = frame_id; + + odom.pose.position.x = x_; + odom.pose.position.y = y_; + odom.pose.position.z = z_; + + tf2::Quaternion quat = quaternion(); + odom.pose.orientation.x = quat.x(); + odom.pose.orientation.y = quat.y(); + odom.pose.orientation.z = quat.z(); + odom.pose.orientation.w = quat.w(); + + odom.twist.linear.x = vx_; + odom.twist.linear.y = vy_; + odom.twist.linear.z = vz_; + + odom.acceleration.x = ax_; + odom.acceleration.y = ay_; + odom.acceleration.z = az_; + + odom.jerk.x = jx_; + odom.jerk.y = jy_; + odom.jerk.z = jz_; + + return odom; +} + +Waypoint Waypoint::interpolate(Waypoint wp, double t) { + tf2::Vector3 pos_interp = position() + t * (wp.position() - position()); + tf2::Quaternion q_interp = quaternion().slerp(wp.quaternion(), t); + tf2::Vector3 vel_interp = velocity() + t * (wp.velocity() - velocity()); + tf2::Vector3 accel_interp = acceleration() + t * (wp.acceleration() - acceleration()); + tf2::Vector3 jerk_interp = jerk() + t * (wp.jerk() - jerk()); + double time_interp = get_time() + t * (wp.get_time() - get_time()); + + Waypoint wp_interp(pos_interp.x(), pos_interp.y(), pos_interp.z(), tf2::getYaw(q_interp), + vel_interp.x(), vel_interp.y(), vel_interp.z(), accel_interp.x(), + accel_interp.y(), accel_interp.z(), jerk_interp.x(), jerk_interp.y(), + jerk_interp.z(), time_interp); + + return wp_interp; +} + +std::ostream& operator<<(std::ostream& os, const Waypoint& wp) { + return os << "[pos: " << wp.get_x() << ", " << wp.get_y() << ", " << wp.get_z() + << " vel: " << wp.get_vx() << ", " << wp.get_vy() << ", " << wp.get_vz() + << " acc: " << wp.get_ax() << ", " << wp.get_ay() << ", " << wp.get_az() + << " jerk: " << wp.get_jx() << ", " << wp.get_jy() << ", " << wp.get_jz() + << " yaw: " << wp.get_yaw() << " time: " << wp.get_time(); +} + +Trajectory::Trajectory() { generated_waypoint_times = false; } + +Trajectory::Trajectory(rclcpp::Node* node_ptr, std::string frame_id) : Trajectory() { + if (buffer == NULL) { + buffer = new tf2_ros::Buffer(node_ptr->get_clock()); + listener = new tf2_ros::TransformListener(*buffer); + } + + this->frame_id = frame_id; +} + +/** + * @brief Construct a new Trajectory:: Trajectory object + * Converts a nav_msgs::Path to a Trajectory object. + * Only the position of each waypoint is used. + * The velocity of each waypoint is set to 0. + * TODO: add a time component to the trajectory? + * + * @param node_ptr + * @param path + */ +Trajectory::Trajectory(rclcpp::Node* node_ptr, nav_msgs::msg::Path path) + : Trajectory(node_ptr, path.header.frame_id) { + this->stamp = path.header.stamp; + + // remove consecutive waypoints + nav_msgs::msg::Path cleaned_path; + cleaned_path.header = path.header; + for (size_t i = 0; i < path.poses.size(); i++) { + geometry_msgs::msg::PoseStamped pose = path.poses[i]; + if (i == 0) + cleaned_path.poses.push_back(pose); + else if (!(pose.pose.position.x == cleaned_path.poses.back().pose.position.x && + pose.pose.position.y == cleaned_path.poses.back().pose.position.y && + pose.pose.position.z == cleaned_path.poses.back().pose.position.z)) + cleaned_path.poses.push_back(pose); + } + + // translate to Waypoint objects + for (size_t i = 0; i < cleaned_path.poses.size(); i++) { + geometry_msgs::msg::PoseStamped pose = cleaned_path.poses[i]; + + Waypoint waypoint(pose.pose.position.x, pose.pose.position.y, pose.pose.position.z, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0); + this->waypoints.push_back(waypoint); + } +} + +Trajectory::Trajectory(rclcpp::Node* node_ptr, airstack_msgs::msg::TrajectoryXYZVYaw path) + : Trajectory(node_ptr, path.header.frame_id) + +{ + this->stamp = path.header.stamp; + + // remove any consecutive duplicate waypoints + airstack_msgs::msg::TrajectoryXYZVYaw cleaned_path; + cleaned_path.header = path.header; + for (size_t i = 0; i < path.waypoints.size(); i++) { + airstack_msgs::msg::WaypointXYZVYaw wp = path.waypoints[i]; + if (i == 0) + cleaned_path.waypoints.push_back(wp); + else if (!(wp.position.x == cleaned_path.waypoints.back().position.x && + wp.position.y == cleaned_path.waypoints.back().position.y && + wp.position.z == cleaned_path.waypoints.back().position.z)) + cleaned_path.waypoints.push_back(wp); + } + + // translate to Waypoint objects + for (size_t i = 0; i < cleaned_path.waypoints.size(); i++) { + airstack_msgs::msg::WaypointXYZVYaw wp = cleaned_path.waypoints[i]; + + // calculate the x, y, z components of velocity + tf2::Vector3 wp1(wp.position.x, wp.position.y, wp.position.z); + tf2::Vector3 wp2(wp1); + bool same = false; + // for the first waypoint use the next waypoint to figure out direction of travel + if (i == 0) { + if (cleaned_path.waypoints.size() > 1) + wp2 = tf2::Vector3(cleaned_path.waypoints[i + 1].position.x, + cleaned_path.waypoints[i + 1].position.y, + cleaned_path.waypoints[i + 1].position.z); + else + same = true; + } else { // otherwise use the previous waypoint + wp1 = tf2::Vector3(cleaned_path.waypoints[i - 1].position.x, + cleaned_path.waypoints[i - 1].position.y, + cleaned_path.waypoints[i - 1].position.z); + } + + tf2::Vector3 direction = (wp2 - wp1).normalized(); + // ROS_INFO_STREAM("direction: " << direction.x() << ", " << direction.y() << ", " << + // direction.z()); + tf2::Vector3 vel = wp.velocity * direction; + // if i == 0 and cleaned_path.waypoints is empty, wp1 will equal wp2 which will make vel nan + if (same) vel = tf2::Vector3(0, 0, 0); + + // ROS_INFO_STREAM("WAYPOINT " << i << " vel: " << vel.x() << " " << vel.y() << " " << + // vel.z()); ROS_INFO_STREAM("\twp1: " << wp1.x() << " " << wp1.y() << " " << wp1.z()); + // ROS_INFO_STREAM("\twp2: " << wp2.x() << " " << wp2.y() << " " << wp2.z()); + // ROS_INFO_STREAM("\tdirection: " << direction.x() << " " << direction.y() << " " << + // direction.z() << " wp.vel: " << wp.vel); + + Waypoint waypoint(wp.position.x, wp.position.y, wp.position.z, wp.yaw, vel.x(), vel.y(), + vel.z(), wp.acceleration.x, wp.acceleration.y, wp.acceleration.z, + wp.jerk.x, wp.jerk.y, wp.jerk.z); + this->waypoints.push_back(waypoint); + } +} +/* +void Trajectory::init_listener(){ + if(listener == NULL) + listener = new tf2_ros::Buffer(); +} +*/ +void Trajectory::clear() { + waypoints.clear(); + generated_waypoint_times = false; +} + +/** + * @brief Uses each waypoint's position and velocity to generate the expected time to reach each + * waypoint. + * + */ +void Trajectory::generate_waypoint_times() { + if (generated_waypoint_times) return; + + if (waypoints.size() > 1) { + waypoints[0].set_time(0.); + Waypoint& curr_wp = waypoints[0]; + } + + for (size_t i = 1; i < waypoints.size(); i++) { + Waypoint& curr_wp = waypoints[i]; + Waypoint& prev_wp = waypoints[i - 1]; + + // ROS_INFO_STREAM(i << " wp vel: " << curr_wp.velocity().x() << ", " << + // curr_wp.velocity().y() << ", " << curr_wp.velocity().z() + // << " | " << prev_wp.velocity().x() << ", " << prev_wp.velocity().y() << ", " << + // prev_wp.velocity().z()); + + double distance = curr_wp.position().distance(prev_wp.position()); + if (distance == 0) { + waypoints.erase(waypoints.begin() + i); + i--; + continue; + } + double velocity = + std::max(0.01, (curr_wp.velocity().length() + prev_wp.velocity().length()) / 2.); + // if(i+1 < waypoints.size()){ + // velocity = (velocity + waypoints[i+1].velocity().length())/2.0; + // } + + // ROS_INFO_STREAM("WAYPOINT " << i << " dist: " << distance << " vel: " << velocity << " + // time: " << (prev_wp.time() + distance/velocity) << " prev_time: " << prev_wp.time() << " + // inc: " << (distance/velocity)); + + curr_wp.set_time(prev_wp.get_time() + distance / velocity); + } + + generated_waypoint_times = true; +} + +bool Trajectory::get_closest_point(tf2::Vector3 point, Waypoint* closest, int* wp_index, + double* path_distance) { + double closest_distance = std::numeric_limits::max(); + double best_t = 0; + size_t best_wp_index = 0; + + if (waypoints.size() == 0) { + if (wp_index != NULL) *wp_index = 0; + if (path_distance != NULL) path_distance = 0; + return false; + } else if (waypoints.size() == 1) { + *closest = waypoints[0]; + best_wp_index = 0; + if (path_distance != NULL) path_distance = 0; + } else { + for (size_t i = 1; i < waypoints.size(); i++) { + // parameteric representation of segment between waypoints: segment_start + + // t*segment_vec + tf2::Vector3 segment_start = waypoints[i - 1].position(); + tf2::Vector3 segment_end = waypoints[i].position(); + tf2::Vector3 segment_vec = segment_end - segment_start; + + // project the vector (point - segment_start) onto the parametric line + double t = ((point - segment_start).dot(segment_vec)) / (segment_vec.dot(segment_vec)); + t = std::max(0.0, std::min(1.0, t)); + // tf2::Vector3 closest_point = segment_start + t*segment_vec; + Waypoint closest_waypoint = waypoints[i - 1].interpolate(waypoints[i], t); + + // find the distance between the point and the closest point on the current segment + double distance = closest_waypoint.position().distance(point); + if (distance < closest_distance) { + closest_distance = distance; + *closest = closest_waypoint; // closest_point; + best_wp_index = i - 1; + best_t = t; + } + } + + if (path_distance != NULL) { + *path_distance = 0; + for (size_t i = 0; i <= best_wp_index; i++) { + tf2::Vector3 segment_start = waypoints[i].position(); + tf2::Vector3 segment_end = waypoints[i + 1].position(); + tf2::Vector3 segment_vec = segment_end - segment_start; + + if (i == best_wp_index) + *path_distance += best_t * segment_vec.length(); + else + *path_distance += segment_vec.length(); + } + } + } + + if (wp_index != NULL) *wp_index = best_wp_index; + + return true; +} + +bool Trajectory::get_trajectory_distance_at_closest_point(tf2::Vector3 point, + double* trajectory_distance) { + double closest_distance = std::numeric_limits::max(); + *trajectory_distance = 0; + double current_trajectory_distance = 0; + + if (waypoints.size() == 0) + return false; + else if (waypoints.size() == 1) + return true; + else { + for (size_t i = 1; i < waypoints.size(); i++) { + // parameteric representation of segment between waypoints: segment_start + + // t*segment_vec + tf2::Vector3 segment_start = waypoints[i - 1].position(); + tf2::Vector3 segment_end = waypoints[i].position(); + tf2::Vector3 segment_vec = segment_end - segment_start; + double segment_length = segment_start.distance(segment_end); + + // project the vector (point - segment_start) onto the parametric line + double t = ((point - segment_start).dot(segment_vec)) / (segment_vec.dot(segment_vec)); + t = std::max(0.0, std::min(1.0, t)); + tf2::Vector3 closest_point = segment_start + t * segment_vec; + + // find the distance between the point and the closest point on the current segment + double distance = closest_point.distance(point); + if (distance < closest_distance) { + closest_distance = distance; + *trajectory_distance = current_trajectory_distance + t * segment_length; + } + + current_trajectory_distance += segment_length; + } + } + + return true; +} + +bool Trajectory::merge(Trajectory traj, double min_time) { + if (traj.waypoints.size() == 0) { + return true; + } + generated_waypoint_times = false; + + Trajectory transformed_traj; + try { + transformed_traj = traj.to_frame(frame_id, this->stamp); + } catch (tf2::TransformException& ex) { + std::cout << "Transform exception while merging trajectories: " << ex.what(); + } + if (transformed_traj.waypoints.size() == 0) { + return true; + } + + if (waypoints.size() == 0) { + waypoints.insert(waypoints.end(), transformed_traj.waypoints.begin(), + transformed_traj.waypoints.end()); + return true; + } + + // tf2::Vector3 closest_point; + Waypoint closest_waypoint(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); + int wp_index; + bool valid = + get_closest_point(transformed_traj.waypoints[0].position(), &closest_waypoint, &wp_index); + + if (closest_waypoint.get_time() >= min_time) { + waypoints.erase(waypoints.begin() + wp_index + 1, waypoints.end()); + waypoints.insert(waypoints.end(), transformed_traj.waypoints.begin(), + transformed_traj.waypoints.end()); + if (wp_index == 0) { + // ROS_WARN("FIRST TIME GENERATION"); + generate_waypoint_times(); + } + return true; + } + + std::cout << "COULDN'T MERGE BECAUSE OF min_time: " << closest_waypoint.get_time() << " " + << min_time; + return false; +} + +bool Trajectory::get_closest_waypoint(tf2::Vector3 point, double initial_time, double end_time, + Waypoint* waypoint) { + double closest_distance = std::numeric_limits::max(); + bool found = false; + + for (size_t i = 1; i < waypoints.size(); i++) { + if (waypoints[i].get_time() < initial_time) continue; + if (waypoints[i - 1].get_time() > end_time) break; + + Waypoint wp_start = waypoints[i - 1]; + Waypoint wp_end = waypoints[i]; + + // handle the case that the initial_time is between waypoint i-1 and waypoint i + if (wp_start.get_time() < initial_time) + wp_start = wp_start.interpolate(wp_end, (initial_time - wp_start.get_time()) / + (wp_end.get_time() - wp_start.get_time())); + // handle the case that the end_time is between waypoint i-1 and waypoint i + if (wp_end.get_time() > end_time) + wp_end = wp_start.interpolate(wp_end, (end_time - wp_start.get_time()) / + (wp_end.get_time() - wp_start.get_time())); + + // parameteric representation of segment between waypoints: segment_start + t*segment_vec + tf2::Vector3 segment_start = wp_start.position(); + tf2::Vector3 segment_end = wp_end.position(); + tf2::Vector3 segment_vec = segment_end - segment_start; + + // project the vector (point - segment_start) onto the parametric line + double t = ((point - segment_start).dot(segment_vec)) / (segment_vec.dot(segment_vec)); + t = std::max(0.0, std::min(1.0, t)); + tf2::Vector3 closest_point = segment_start + t * segment_vec; + + // find the distance between the point and the closest point on the current segment + double distance = closest_point.distance(point); + if (distance < closest_distance) { + closest_distance = distance; + found = true; + *waypoint = wp_start.interpolate(wp_end, t); + } + } + + return found; +} + +bool Trajectory::get_waypoint_distance_ahead(double initial_time, double distance, + Waypoint* waypoint) { + double current_distance = 0; + + for (size_t i = 1; i < waypoints.size(); i++) { + if (waypoints[i].get_time() < initial_time) continue; + + Waypoint wp_start = waypoints[i - 1]; + Waypoint wp_end = waypoints[i]; + + // handle the case that the initial_time is between waypoint i-1 and waypoint i + if (wp_start.get_time() < initial_time) + wp_start = wp_start.interpolate(wp_end, (initial_time - wp_start.get_time()) / + (wp_end.get_time() - wp_start.get_time())); + + double segment_distance = wp_start.position().distance(wp_end.position()); + if (current_distance + segment_distance >= distance) { + *waypoint = + wp_start.interpolate(wp_end, (distance - current_distance) / segment_distance); + return true; + } else + current_distance += segment_distance; + + if (i == waypoints.size() - 1 && wp_end.get_time() >= initial_time) { + *waypoint = wp_end; + return true; + } + } + + return false; +} + +bool Trajectory::get_waypoint_sphere_intersection(double initial_time, double ahead_distance, + double time_end, tf2::Vector3 sphere_center, + double sphere_radius, double min_velocity, + Waypoint* waypoint, Waypoint* end_waypoint) { + double current_path_distance = 0; + bool found = false; + + // ROS_INFO("\n\n"); + for (size_t i = 1; i < waypoints.size(); i++) { + if (waypoints[i].get_time() < initial_time) continue; + + Waypoint wp_start = waypoints[i - 1]; + Waypoint wp_end = waypoints[i]; + + // handle the case that the initial_time is between waypoint i-1 and waypoint i + if (wp_start.get_time() < initial_time) + wp_start = wp_start.interpolate(wp_end, (initial_time - wp_start.get_time()) / + (wp_end.get_time() - wp_start.get_time())); + + double segment_distance = wp_start.position().distance(wp_end.position()); + bool should_break = false; + if (current_path_distance + segment_distance >= ahead_distance) { + should_break = true; + wp_end = wp_start.interpolate( + wp_end, (ahead_distance - current_path_distance) / segment_distance); + if (end_waypoint != NULL) *end_waypoint = wp_end; + } else if (wp_end.get_time() > time_end) { + should_break = true; + wp_end = wp_start.interpolate(wp_end, (time_end - wp_start.get_time()) / + (wp_end.get_time() - wp_start.get_time())); + if (end_waypoint != NULL) *end_waypoint = wp_end; + } else + current_path_distance += segment_distance; + + // sphere line intersection equations: + // http://www.ambrsoft.com/TrigoCalc/Sphere/SpherLineIntersection_.htm + double x1 = wp_start.get_x(); + double y1 = wp_start.get_y(); + double z1 = wp_start.get_z(); + double x2 = wp_end.get_x(); + double y2 = wp_end.get_y(); + double z2 = wp_end.get_z(); + double xc = sphere_center.x(); + double yc = sphere_center.y(); + double zc = sphere_center.z(); + double a = pow(x2 - x1, 2) + pow(y2 - y1, 2) + pow(z2 - z1, 2); + double b = -2 * ((x2 - x1) * (xc - x1) + (y2 - y1) * (yc - y1) + (z2 - z1) * (zc - z1)); + double c = pow(xc - x1, 2) + pow(yc - y1, 2) + pow(zc - z1, 2) - pow(sphere_radius, 2); + + bool is_intersection = (b * b - 4 * a * c) >= 0; + + /* + ROS_INFO_STREAM("xyz1: " << x1 << ", " << y1 << ", " << z1 << " xyz2: " << x2 << ", " << y2 + << ", " << z2 + << " c: " << xc << ", " << yc << ", " << zc << " abc: " << a << ", " << b << + ", " << c + << "rad: " << sphere_radius << " int: " << is_intersection); + //*/ + if (is_intersection) { + double t1 = (-b + sqrt(b * b - 4 * a * c)) / (2 * a); + double t2 = (-b - sqrt(b * b - 4 * a * c)) / (2 * a); + // ROS_INFO_STREAM("t: " << t1 << ", " << t2); + + double t = 0.; + bool t1_in_range = (t1 >= 0.0) && (t1 <= 1.0); + bool t2_in_range = (t2 >= 0.0) && (t2 <= 1.0); + if (t1_in_range || t2_in_range) { + if (t1_in_range && t2_in_range) + t = std::max(t1, t2); + else if (t1_in_range) + t = t1; + else if (t2_in_range) + t = t2; + + if (waypoint != NULL) { + // ROS_INFO_STREAM("t: " << t); + *waypoint = wp_start.interpolate(wp_end, t); + found = true; + } + } + } + + if (should_break) break; + } + // if(!found) + // ROS_INFO("NOT FOUND!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"); + + return found; +} + +bool Trajectory::get_waypoint(double time, Waypoint* waypoint) { + generate_waypoint_times(); + + for (size_t i = 1; i < waypoints.size(); i++) { + if (waypoints[i].get_time() < time) continue; + + Waypoint wp_start = waypoints[i - 1]; + Waypoint wp_end = waypoints[i]; + + *waypoint = wp_start.interpolate( + wp_end, (time - wp_start.get_time()) / (wp_end.get_time() - wp_start.get_time())); + return true; + } + + if (waypoints.size() > 0) { + *waypoint = waypoints.back(); + return true; + } + + return false; +} + +double Trajectory::get_duration() { + generate_waypoint_times(); + + if (waypoints.size() == 0) return 0; + + return waypoints.back().get_time(); +} + +/** + * @brief Get the expected odometry at a given time. Performs interpolation if the time is between + * waypoints. + * + * @param time + * @param odom + * @param stamp + * @return true + * @return false + */ +bool Trajectory::get_odom(double time, airstack_msgs::msg::Odometry* odom, rclcpp::Time stamp) { + generate_waypoint_times(); + + if (waypoints.size() == 0) { + return false; + } else if (waypoints.size() == 1) { + *odom = waypoints[0].as_odometry_msg(stamp, frame_id); // rclcpp::Time::now(), frame_id); + return true; + } + + // figure out what waypoints we are between + Waypoint prev_wp = waypoints.front(); + Waypoint curr_wp = waypoints.front(); + for (size_t i = waypoints.size() - 1; i >= 1; i--) { + if (time >= waypoints[i - 1].get_time()) { + prev_wp = waypoints[i - 1]; + curr_wp = waypoints[i]; + break; + } + } + + // figure out where we are in between the current and previous waypoints + double t = (time - prev_wp.get_time()) / (curr_wp.get_time() - prev_wp.get_time()); + t = std::max(0.0, std::min(1.0, t)); // TODO check for nan + // ROS_INFO_STREAM("t: " << t); + + *odom = prev_wp.interpolate(curr_wp, t) + .as_odometry_msg(stamp, frame_id); // rclcpp::Time::now(), frame_id); + // ROS_INFO_STREAM("odom: " << odom->pose.position.x << ", " << odom->pose.position.y << ", " << + // odom->pose.position.z << " | " + // << odom->twist.linear.x << ", " << odom->twist.linear.y << ", " << + // odom->twist.linear.z); + + return true; +} + +Trajectory Trajectory::to_frame(std::string target_frame, rclcpp::Time time) { + // ROS_INFO("to frame"); + // init_listener(); + + tf2::Stamped transform; + geometry_msgs::msg::TransformStamped t; + t = buffer->lookupTransform(target_frame, frame_id, time, rclcpp::Duration::from_seconds(0.1)); + tf2::fromMsg(t, transform); + // buffer->waitForTransform(target_frame, frame_id, time, rclcpp::Duration(0.1)); + // buffer->lookupTransform(target_frame, frame_id, time, transform); + tf2::Transform rot(transform.getRotation()); + + Trajectory transformed_traj; + transformed_traj.stamp = time; + transformed_traj.frame_id = target_frame; + + for (size_t i = 0; i < waypoints.size(); i++) { + Waypoint wp = waypoints[i]; + + tf2::Vector3 transformed_position = transform * wp.position(); + tf2::Vector3 transformed_velocity = rot * wp.velocity(); + tf2::Vector3 transformed_acceleration = rot * wp.acceleration(); + tf2::Vector3 transformed_jerk = rot * wp.jerk(); + tf2::Quaternion transformed_q = transform * wp.quaternion(); + + // ROS_INFO_STREAM("jerk: " << wp.jerk().x() << ", " << wp.jerk().y() << ", " << + // wp.jerk().z() << " | " + // << transformed_jerk.x() << ", " << transformed_jerk.y() << ", " << + // transformed_jerk.z()); + + Waypoint transformed_wp(transformed_position.x(), transformed_position.y(), + transformed_position.z(), tf2::getYaw(transformed_q), + transformed_velocity.x(), transformed_velocity.y(), + transformed_velocity.z(), transformed_acceleration.x(), + transformed_acceleration.y(), transformed_acceleration.z(), + transformed_jerk.x(), transformed_jerk.y(), transformed_jerk.z()); + + transformed_traj.waypoints.push_back(transformed_wp); + } + + return transformed_traj; +} + +/*Trajectory Trajectory::respace(double spacing){ + Trajectory traj; + traj.stamp = stamp; + traj.frame_id = frame_id; + //ROS_INFO("RESPACE"); + + if(waypoints.size() > 0){ + double distance = 0; + traj.waypoints.push_back(waypoints[0]); // always add the first waypoint + + for(size_t i = 1; i < waypoints.size(); i++){ + if(i == waypoints.size()-1){ // always add the last waypoint + traj.waypoints.push_back(waypoints[i]); + continue; + } + //ROS_INFO_STREAM("Waypoint " << i); + Waypoint prev_wp = waypoints[i-1]; + Waypoint curr_wp = waypoints[i]; + double segment_length = prev_wp.position().distance(curr_wp.position()); + //ROS_INFO_STREAM("segment_length: " << segment_length << " distance: " << distance << " +spacing: " << spacing); + + if(distance + segment_length >= spacing){ + //ROS_INFO("interpolating"); + Waypoint interp_wp = prev_wp.interpolate(curr_wp, (spacing - distance)/segment_length); + traj.waypoints.push_back(interp_wp); + distance = 0; + } + else + distance += segment_length; + } + } + + return traj; +} + +Trajectory Trajectory::shorten(double new_length){ + Trajectory traj; + traj.stamp = stamp; + traj.frame_id = frame_id; + + if(waypoints.size() > 0){ + double distance = 0; + traj.waypoints.push_back(waypoints[0]); // always add the first waypoint + + for(size_t i = 1; i < waypoints.size(); i++){ + Waypoint prev_wp = waypoints[i-1]; + Waypoint curr_wp = waypoints[i]; + double segment_length = prev_wp.position().distance(curr_wp.position()); + + if(distance + segment_length >= new_length){ + Waypoint interp_wp = prev_wp.interpolate(curr_wp, (new_length - distance)/segment_length); + traj.waypoints.push_back(interp_wp); + break; + } + else{ + traj.waypoints.push_back(curr_wp); + distance += segment_length; + } + } + } + + return traj; +}*/ + +/** + * @brief Trim a subtrajectory between a start distance and end distance + * + * @param start + * @param end + * @return Trajectory + */ +Trajectory Trajectory::trim_trajectory_between_distances(double start_dist, double end_dist) { + Trajectory traj; + traj.stamp = this->stamp; + traj.frame_id = this->frame_id; + + if (this->waypoints.size() > 0) { + double distance = 0; + + if (start_dist == 0. && this->waypoints.size() == 1) + traj.waypoints.push_back(this->waypoints[0]); + + for (size_t i = 1; i < this->waypoints.size(); i++) { + Waypoint prev_wp = this->waypoints[i - 1]; + Waypoint curr_wp = this->waypoints[i]; + double segment_length = prev_wp.position().distance(curr_wp.position()); + + if (start_dist >= distance && start_dist <= distance + segment_length) { + Waypoint interp_start_wp = + prev_wp.interpolate(curr_wp, (start_dist - distance) / segment_length); + traj.waypoints.push_back(interp_start_wp); + } + + if (distance + segment_length > start_dist && distance + segment_length < end_dist) { + traj.waypoints.push_back(curr_wp); + } + + if (end_dist >= distance && end_dist <= distance + segment_length) { + Waypoint interp_end_wp = + prev_wp.interpolate(curr_wp, (end_dist - distance) / segment_length); + traj.waypoints.push_back(interp_end_wp); + } + + distance += segment_length; + } + } + + return traj; +} + +Trajectory Trajectory::get_reversed_trajectory() { + generated_waypoint_times = false; + generate_waypoint_times(); + Trajectory traj; + traj.frame_id = this->frame_id; + traj.stamp = this->stamp; + traj.waypoints.assign(waypoints.begin(), waypoints.end()); + std::reverse(traj.waypoints.begin(), traj.waypoints.end()); + for (size_t i = 0; i < traj.waypoints.size(); i++) { + Waypoint& wp = traj.waypoints[i]; + wp.vx_ *= -1; + wp.vy_ *= -1; + wp.vz_ *= -1; + /* + wp.ax_ *= -1; + wp.ay_ *= -1; + wp.az_ *= -1; + wp.jx_ *= -1; + wp.jy_ *= -1; + wp.jz_ *= -1; + */ + } + traj.generate_waypoint_times(); + return traj; +} + +float Trajectory::get_skip_ahead_time(float start_time, float max_velocity, float max_distance) { + generate_waypoint_times(); + double current_path_distance = 0; + float skip_time = start_time; + + for (size_t i = 1; i < waypoints.size(); i++) { + if (waypoints[i].get_time() < start_time) continue; + + Waypoint wp_start = waypoints[i - 1]; + float wp_start_velocity = wp_start.velocity().length(); + if (wp_start_velocity > max_velocity) break; + Waypoint wp_end = waypoints[i]; + float wp_end_velocity = wp_end.velocity().length(); + + if (wp_start.get_time() < start_time) + wp_start = wp_start.interpolate(wp_end, (start_time - wp_start.get_time()) / + (wp_end.get_time() - wp_start.get_time())); + + double segment_distance = wp_start.position().distance(wp_end.position()); + bool should_break = false; + if (current_path_distance + segment_distance >= max_distance) { + should_break = true; + wp_end = wp_start.interpolate( + wp_end, (max_distance - current_path_distance) / segment_distance); + } + + if (wp_end_velocity > max_velocity) { + should_break = true; + wp_end = wp_start.interpolate( + wp_end, std::min(1.f, std::max(0.f, (max_velocity - wp_start_velocity) / + (wp_end_velocity - wp_start_velocity)))); + } + + skip_time = wp_end.get_time(); + + if (should_break) break; + + current_path_distance += segment_distance; + } + + return skip_time; +} + +void Trajectory::set_fixed_height(double height) { + for (size_t i = 0; i < waypoints.size(); i++) waypoints[i].z_ = height; +} + +size_t Trajectory::get_num_waypoints() const { return waypoints.size(); } + +const Waypoint& Trajectory::get_waypoint(int index) const { return waypoints[index]; } + +const std::vector& Trajectory::get_waypoints() const { return waypoints; } + +const std::string& Trajectory::get_frame_id() const { return frame_id; } + +airstack_msgs::msg::TrajectoryXYZVYaw Trajectory::get_TrajectoryXYZVYaw_msg() { + airstack_msgs::msg::TrajectoryXYZVYaw path; + path.header.stamp = this->stamp; + path.header.frame_id = this->frame_id; + + for (size_t i = 0; i < waypoints.size(); i++) { + Waypoint wp = waypoints[i]; + + airstack_msgs::msg::WaypointXYZVYaw w; + w.position.x = wp.get_x(); + w.position.y = wp.get_y(); + w.position.z = wp.get_z(); + w.yaw = tf2::getYaw(wp.quaternion()); + w.velocity = wp.velocity().length(); + + path.waypoints.push_back(w); + } + + return path; +} + +std::vector Trajectory::get_vector_PointStamped_msg() { + std::vector points; + + for (size_t i = 0; i < waypoints.size(); i++) { + Waypoint wp = waypoints[i]; + + geometry_msgs::msg::PointStamped point; + point.header.stamp = this->stamp; + point.header.frame_id = this->frame_id; + point.point.x = wp.get_x(); + point.point.y = wp.get_y(); + point.point.z = wp.get_z(); + + points.push_back(point); + } + + return points; +} + +visualization_msgs::msg::MarkerArray Trajectory::get_markers(rclcpp::Time stamp, + const std::string& marker_namespace, + float r, float g, float b, float a, + bool show_poses, bool show_velocity, + float thickness) { + visualization_msgs::msg::MarkerArray marker_array; + // rclcpp::Time now = rclcpp::Time::now(); + + visualization_msgs::msg::Marker clear; + clear.ns = marker_namespace; + clear.action = visualization_msgs::msg::Marker::DELETEALL; + marker_array.markers.push_back(clear); + + visualization_msgs::msg::Marker lines; + lines.header.stamp = stamp; + lines.header.frame_id = frame_id; + lines.ns = marker_namespace; + lines.id = waypoints.size() + 1; + lines.type = visualization_msgs::msg::Marker::LINE_STRIP; + lines.action = visualization_msgs::msg::Marker::ADD; + lines.scale.x = thickness; + + for (size_t i = 0; i < waypoints.size(); i++) { + Waypoint wp = waypoints[i]; + + if (show_poses) { + visualization_msgs::msg::Marker arrow; + arrow.header.stamp = stamp; + arrow.header.frame_id = frame_id; + arrow.ns = marker_namespace; + arrow.id = i; + arrow.type = visualization_msgs::msg::Marker::ARROW; + arrow.action = visualization_msgs::msg::Marker::ADD; + + arrow.pose.position.x = wp.position().x(); + arrow.pose.position.y = wp.position().y(); + arrow.pose.position.z = wp.position().z(); + arrow.pose.orientation.x = wp.quaternion().x(); + arrow.pose.orientation.y = wp.quaternion().y(); + arrow.pose.orientation.z = wp.quaternion().z(); + arrow.pose.orientation.w = wp.quaternion().w(); + arrow.scale.x = 0.2; // length + arrow.scale.y = 0.05; // width + arrow.scale.z = 0.05; // height + arrow.color.r = r; + arrow.color.g = g; + arrow.color.b = b; + arrow.color.a = a; + + marker_array.markers.push_back(arrow); + } + + if (show_velocity) { + visualization_msgs::msg::Marker vel_arrow; + vel_arrow.header.stamp = stamp; + vel_arrow.header.frame_id = frame_id; + vel_arrow.ns = marker_namespace + "_velocities"; + vel_arrow.id = i; + vel_arrow.type = visualization_msgs::msg::Marker::ARROW; + vel_arrow.action = visualization_msgs::msg::Marker::ADD; + + geometry_msgs::msg::Point point1, point2; + point1.x = wp.position().x(); + point1.y = wp.position().y(); + point1.z = wp.position().z() + 0.2; + point2.x = wp.position().x() + wp.velocity().normalized().x() / 3.; + point2.y = wp.position().y() + wp.velocity().normalized().y() / 3.; + point2.z = wp.position().z() + 0.2 + wp.velocity().normalized().z() / 3.; + vel_arrow.points.push_back(point1); + vel_arrow.points.push_back(point2); + vel_arrow.scale.x = 0.1; + vel_arrow.scale.y = 0.15; + vel_arrow.scale.z = 0.1; + vel_arrow.color.r = r; + vel_arrow.color.g = g; + vel_arrow.color.b = b; + vel_arrow.color.a = a; + + marker_array.markers.push_back(vel_arrow); + } + + geometry_msgs::msg::Point p; + p.x = wp.position().x(); + p.y = wp.position().y(); + p.z = wp.position().z(); + lines.points.push_back(p); + std_msgs::msg::ColorRGBA color; + color.r = r; + color.g = g; + color.b = b; + color.a = a; + lines.colors.push_back(color); + } + + marker_array.markers.push_back(lines); + + return marker_array; +} + +//=================================================================================== +//--------------------------------- Dynamic Trajectories ---------------------------- +//=================================================================================== + +TakeoffTrajectory::TakeoffTrajectory(double height, double velocity, double path_roll, + double path_pitch, bool relative_to_orientation) { + this->height = height; + this->velocity = velocity; + this->path_roll = path_roll; + this->path_pitch = path_pitch; + this->relative_to_orientation = relative_to_orientation; +} + +airstack_msgs::msg::TrajectoryXYZVYaw TakeoffTrajectory::get_trajectory( + airstack_msgs::msg::Odometry odom) { + airstack_msgs::msg::TrajectoryXYZVYaw traj; + traj.header.frame_id = odom.header.frame_id; + traj.header.stamp = odom.header.stamp; + + airstack_msgs::msg::WaypointXYZVYaw wp1, wp2, wp3; + + wp1.position.x = odom.pose.position.x; + wp1.position.y = odom.pose.position.y; + wp1.position.z = odom.pose.position.z; + + tf2::Quaternion q(odom.pose.orientation.x, odom.pose.orientation.y, odom.pose.orientation.z, + odom.pose.orientation.w); + double roll, pitch, yaw; + tf2::Matrix3x3(q).getRPY(roll, pitch, yaw); + wp1.yaw = yaw; // tf2::getYaw(q); + wp1.velocity = velocity; + + wp2.position.x = + odom.pose.position.x + height * sin(path_pitch + (relative_to_orientation ? pitch : 0)); + wp2.position.y = + odom.pose.position.y + height * sin(path_roll - (relative_to_orientation ? roll : 0)); + wp2.position.z = odom.pose.position.z + height - 0.01; + wp2.yaw = wp1.yaw; + wp2.velocity = velocity; + + wp3.position.x = wp2.position.x; + wp3.position.y = wp2.position.y; + wp3.position.z = wp2.position.z + 0.01; + wp3.yaw = wp2.yaw; + wp3.velocity = 0.01; + + traj.waypoints.push_back(wp1); + traj.waypoints.push_back(wp2); + traj.waypoints.push_back(wp3); + + return traj; +} + +AccelerationTrajectory::AccelerationTrajectory(tf2_ros::Buffer* buffer, std::string frame, + double ax, double ay, double az, double dt, + double ht, double max_velocity) { + // this->buffer = buffer; + + // ros::NodeHandle nh("acceleration_trajectory"); + // set_max_velocity_sub = nh.subscribe("set_max_velocity", 1, + // &AccelerationTrajectory::set_max_velocity_callback, this); + + this->ax = ax; + this->ay = ay; + this->az = az; + this->dt = dt; + this->ht = ht; + this->max_velocity = max_velocity; + this->frame = frame; +} + +// void AccelerationTrajectory::set_max_velocity_callback(std_msgs::Float32 msg){ +// max_velocity = msg.data; +// } + +airstack_msgs::msg::TrajectoryXYZVYaw AccelerationTrajectory::get_trajectory( + airstack_msgs::msg::Odometry odom) { + airstack_msgs::msg::TrajectoryXYZVYaw traj; + traj.header.frame_id = frame; + traj.header.stamp = odom.header.stamp; + + try { + tf2::Stamped pose_tf; + geometry_msgs::msg::TransformStamped t; + t = buffer->lookupTransform(frame, odom.header.frame_id, rclcpp::Time(odom.header.stamp), + rclcpp::Duration::from_seconds(0.1)); + tf2::fromMsg(t, pose_tf); + // buffer->waitForTransform(frame, odom.header.frame_id, odom.header.stamp, + // rclcpp::Duration(0.1)); buffer->lookupTransform(frame, odom.header.frame_id, + // odom.header.stamp, pose_tf); + tf2::Stamped velocity_tf; + t = buffer->lookupTransform(frame, odom.child_frame_id, rclcpp::Time(odom.header.stamp), + rclcpp::Duration::from_seconds(0.1)); + tf2::fromMsg(t, velocity_tf); + // buffer->waitForTransform(frame, odom.child_frame_id, odom.header.stamp, + // rclcpp::Duration(0.1)); buffer->lookupTransform(frame, odom.child_frame_id, + // odom.header.stamp, velocity_tf); + velocity_tf.setOrigin( + tf2::Vector3(0, 0, 0)); // only use rotation to transform the velocity + + tf2::Vector3 position = pose_tf * tflib::to_tf(odom.pose.position); + tf2::Quaternion orientation = pose_tf * tflib::to_tf(odom.pose.orientation); + tf2::Vector3 velocity = velocity_tf * tflib::to_tf(odom.twist.linear); + + tf2::Vector3 acceleration(ax, ay, az); + + airstack_msgs::msg::WaypointXYZVYaw wp0; + wp0.position.x = position.x(); + wp0.position.y = position.y(); + wp0.position.z = position.z(); + wp0.yaw = 0; + wp0.velocity = velocity.length(); + traj.waypoints.push_back(wp0); + + for (double t = 0; t < ht; t += dt) { + velocity += acceleration * dt; + position += velocity * dt; + + airstack_msgs::msg::WaypointXYZVYaw wp; + wp.position.x = position.x(); + wp.position.y = position.y(); + wp.position.z = position.z(); + wp.yaw = 0; + wp.velocity = std::min(velocity.length(), max_velocity); + traj.waypoints.push_back(wp); + } + } catch (tf2::TransformException& e) { + std::cout << "TransformException in AccelerationTrajectory::get_trajectory: " << e.what(); + } + + return traj; +} + +//=================================================================================== +//--------------------------------- Static Trajectories ----------------------------- +//=================================================================================== + +CurveTrajectory::CurveTrajectory(float linear_velocity, float angular_velocity, std::string frame, + float time, float dt, bool use_heading, float yaw) { + this->linear_velocity = linear_velocity; + this->angular_velocity = angular_velocity; + this->frame = frame; + this->time = time; + this->dt = dt; + this->use_heading = use_heading; + this->yaw = yaw; + + generate_trajectory(); +} + +void CurveTrajectory::generate_trajectory() { + float prev_x = 0.f; + float prev_y = 0.f; + float prev_heading = 0.f; + + // TODO: when to fill in trajectory.header.stamp? + trajectory.header.frame_id = frame; + + for (float t = 0; t <= time; t += dt) { + airstack_msgs::msg::WaypointXYZVYaw wp; + + if (t > 0) { + wp.position.x = prev_x + cos(prev_heading) * linear_velocity * dt; + wp.position.y = prev_y + sin(prev_heading) * linear_velocity * dt; + wp.yaw = prev_heading + angular_velocity * dt; + prev_x = wp.position.x; + prev_y = wp.position.y; + prev_heading = wp.yaw; + } + + wp.velocity = linear_velocity; + if (!use_heading) wp.yaw = yaw; + + trajectory.waypoints.push_back(wp); + } +} + +airstack_msgs::msg::TrajectoryXYZVYaw CurveTrajectory::get_trajectory() { return trajectory; } + +//=================================================================================== +//--------------------------------- Trajectory Library ------------------------------ +//=================================================================================== + +TrajectoryLibrary::TrajectoryLibrary(std::string config_filename, + rclcpp::Node::SharedPtr node_ptr) { // tf2_ros::Buffer* b){ + + RCLCPP_DEBUG(node_ptr->get_logger(), "TrajectoryLibrary constructor"); + RCLCPP_DEBUG_STREAM(node_ptr->get_logger(), + "Traj Library node_ptr name is: " << node_ptr->get_name()); + + // accelertion trajectory parameters + node_ptr->declare_parameter("dt", 69.); + node_ptr->declare_parameter("ht", 69.); + node_ptr->declare_parameter("ht_long", 69.); + node_ptr->declare_parameter("max_velocity", 69.); + node_ptr->declare_parameter("magnitude", 69.); + + this->node_ptr = node_ptr; + if (buffer == NULL) { + buffer = new tf2_ros::Buffer(node_ptr->get_clock()); + listener = new tf2_ros::TransformListener(*buffer); + } + + YAML::Node yaml_config = YAML::LoadFile(config_filename); + + YAML::Node trajectories_yaml = yaml_config["trajectories"]; + for (size_t i = 0; i < trajectories_yaml.size(); i++) { + YAML::Node traj_node = trajectories_yaml[i]; + std::string type = parse(traj_node["type"]); + + if (type == "curve") { + float linear_velocity = parse(traj_node["linear_velocity"]); + float angular_velocity = M_PI / 180. * parse(traj_node["angular_velocity"]); + std::string frame = parse(traj_node["frame"]); + float time = parse(traj_node["time"]); + float dt = parse(traj_node["dt"]); + bool use_heading = true; + float yaw = 0.f; + if (parse(traj_node["yaw"]) == std::string("heading")) + use_heading = true; + else { + use_heading = false; + yaw = M_PI / 180. * parse(traj_node["yaw"]); + } + + CurveTrajectory* traj = new CurveTrajectory(linear_velocity, angular_velocity, frame, + time, dt, use_heading, yaw); + + static_trajectories.push_back(traj); + } else if (type == "acceleration") { + std::string frame = parse(traj_node["frame"]); + double x; + double y; + double z; + + if (traj_node["x"] && traj_node["y"] && traj_node["z"]) { + x = parse(traj_node["x"]); + y = parse(traj_node["y"]); + z = parse(traj_node["z"]); + } else if (traj_node["magnitude"]) { + double magnitude = parse(traj_node["magnitude"]); + double magnitude_yaw = M_PI / 180. * parse(traj_node["magnitude_yaw"]); + double magnitude_pitch = M_PI / 180. * parse(traj_node["magnitude_pitch"]); + + tf2::Quaternion q; + q.setRPY(0, magnitude_pitch, magnitude_yaw); + tf2::Vector3 vector = tf2::Transform(q) * tf2::Vector3(magnitude, 0, 0); + x = vector.x(); + y = vector.y(); + z = vector.z(); + } else { + std::cout << "Couldn't construct acceleration trajectory. Need to have either x, " + "y, z or magnitude, magnitude_yaw, magnitude_pitch." + << std::endl; + continue; + } + double dt = parse(traj_node["dt"]); + double ht = parse(traj_node["ht"]); + double max_velocity = parse(traj_node["max_velocity"]); + + AccelerationTrajectory* traj = + new AccelerationTrajectory(buffer, frame, x, y, z, dt, ht, max_velocity); + + dynamic_trajectories.push_back(traj); + } + } + RCLCPP_DEBUG_STREAM(node_ptr->get_logger(), + "dt: " << node_ptr->get_parameter("dt").as_double()); + RCLCPP_DEBUG_STREAM(node_ptr->get_logger(), + "ht: " << node_ptr->get_parameter("ht").as_double()); + RCLCPP_DEBUG_STREAM(node_ptr->get_logger(), + "ht_long: " << node_ptr->get_parameter("ht_long").as_double()); + RCLCPP_DEBUG_STREAM(node_ptr->get_logger(), + "max_velocity: " << node_ptr->get_parameter("max_velocity").as_double()); + RCLCPP_DEBUG_STREAM(node_ptr->get_logger(), + "magnitude: " << node_ptr->get_parameter("magnitude").as_double()); +} + +std::vector TrajectoryLibrary::get_static_trajectories() { + std::vector trajectories; + + for (size_t i = 0; i < static_trajectories.size(); i++) { + trajectories.push_back( + Trajectory(node_ptr.get(), static_trajectories[i]->get_trajectory())); + } + + return trajectories; +} + +std::vector TrajectoryLibrary::get_dynamic_trajectories( + airstack_msgs::msg::Odometry odom) { + std::vector trajectories; + + for (size_t i = 0; i < dynamic_trajectories.size(); i++) { + airstack_msgs::msg::TrajectoryXYZVYaw path = dynamic_trajectories[i]->get_trajectory(odom); + if (path.waypoints.size() > 0) trajectories.push_back(Trajectory(node_ptr.get(), path)); + } + + return trajectories; +} diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/src/trajectory_library_generator.py b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/src/trajectory_library_generator.py new file mode 100644 index 00000000..8335ab71 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/src/trajectory_library_generator.py @@ -0,0 +1,75 @@ +#!/usr/bin/python +import numpy as np +import matplotlib.pyplot as plt +import yaml +import sys + +def generate_curve(traj_config): + lin_vel = float(traj_config['linear_velocity']) + ang_vel = np.pi/180.*float(traj_config['angular_velocity']) + + dt = 0.1 + if 'dt' in traj_config.keys(): + dt = traj_config['dt'] + + time = 0. + if 'distance' in traj_config.keys(): + time = float(traj_config['distance'])/lin_vel + if 'time' in traj_config.keys(): + time = traj_config['time'] + + xs = [0.] + ys = [0.] + yaws = [0.] + + for t in np.arange(0., time, dt): + xs.append(xs[-1] + np.cos(yaws[-1])*lin_vel*dt) + ys.append(ys[-1] + np.sin(yaws[-1])*lin_vel*dt) + yaws.append(yaws[-1] + ang_vel*dt) + + if 'yaw' in traj_config.keys(): + if traj_config['yaw'] != 'heading': + yaw = np.pi/180.*float(traj_config['yaw']) + for i in range(len(yaws)): + yaws[i] = yaw + + plt.plot(xs, ys, '.-') + plt.axis('equal') + plt.show() + + +def generate_arc(traj_config): + radius = float(traj_config['radius']) + angle = np.pi/180.*float(traj_config['angle']) + angle_increment = np.pi/180.*float(traj_config['angle_increment']) + + xs = radius*np.cos(np.arange(0., angle, angle_increment) - np.pi/2.) + ys = radius*np.sin(np.arange(0., angle, angle_increment) - np.pi/2.) + print(xs) + + plt.plot(xs, ys, '.-') + plt.axis('equal') + plt.show() + +def generate_acceleration(traj_config): + pass + +def generate_trajectory(traj_config): + if traj_config['type'] == 'curve': + return generate_curve(traj_config) + elif traj_config['type'] == 'arc': + return generate_arc(traj_config) + elif traj_config['type'] == 'acceleration': + return generate_acceleration(traj_config) + +if __name__ == '__main__': + input_filename = sys.argv[1] + output_filename = sys.argv[2] + + + config = yaml.load(open(input_filename, 'r').read()) + + for traj_config in config['trajectories']: + traj_config = traj_config['trajectory'] + traj = generate_trajectory(traj_config) + diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/index.html b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/index.html new file mode 100644 index 00000000..e6faefa5 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/index.html @@ -0,0 +1,2604 @@ + + + + + + + + + + + + + + + + + + + mav_comm - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

mav_comm

+

This repository contains message and service definitions used for mavs. All future message definitions go in here, existing ones in other stacks should be moved here where possible.

+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ +
+ + + +
+
+
+ +
+ Copyright © 2024 - 2030 AirLab CMU +
+ + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + +
+ +
+
+
+ +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_comm/CHANGELOG.rst b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_comm/CHANGELOG.rst new file mode 100644 index 00000000..71ecf3dc --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_comm/CHANGELOG.rst @@ -0,0 +1,32 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package mav_comm +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.3.3 (2019-08-16) +------------------ +* Add 6DOF trajectory compatibility. +* See mav_msgs and mav_planning_msgs changelogs for details. + +3.3.2 (2018-08-22) +------------------ +* Fix indigo eigen3 compatibility. + +3.3.1 (2018-08-21) +------------------ +* Change maintainer. +* Add dependencies, see planning_msgs changelog for details. + +3.3.0 (2018-08-17) +------------------ +* More utilities and lower level UAV kinematics, see mav_msgs changelog for details. +* 2D polygon planning msgs, see planings_msgs changelog for details. + +3.1.0 (2016-12-01) +------------------ +* More helper functions, see mav_msgs changelog for details. + +3.0.0 (2015-08-09) +------------------ +* Changed API for mav_msgs, see mav_msgs changelog for details. + +2.0.3 (2015-05-22) +------------------ diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_comm/CMakeLists.txt b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_comm/CMakeLists.txt new file mode 100644 index 00000000..6358e80a --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_comm/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 3.14.4) +project(mav_comm) + +find_package(ament_cmake REQUIRED) + +find_package(mav_msgs REQUIRED) +find_package(mav_planning_msgs REQUIRED) +find_package(mav_state_machine_msgs REQUIRED) +find_package(mav_system_msgs REQUIRED) + +ament_package() diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_comm/package.xml b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_comm/package.xml new file mode 100644 index 00000000..eda02626 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_comm/package.xml @@ -0,0 +1,38 @@ + + + + mav_comm + 3.3.3 + Contains messages and services for MAV communication + + Rik Bähnemann + + Simon Lynen + Markus Achtelik + Pascal Gohl + Sammy Omari + Michael Burri + Fadri Furrer + Helen Oleynikova + Mina Kamel + Karen Bodie + Rik Bähnemann + + ASL 2.0 + + https://github.com/ethz-asl/mav_comm + https://github.com/ethz-asl/mav_comm/issues + + + ament_cmake + + + mav_msgs + mav_planning_msgs + mav_state_machine_msgs + mav_system_msgs + + + ament_cmake + + diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/CHANGELOG.rst b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/CHANGELOG.rst new file mode 100644 index 00000000..42284764 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/CHANGELOG.rst @@ -0,0 +1,60 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package mav_msgs +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.3.3 (2019-08-16) +------------------ +* Add `degrees_of_freedom` to EigenTrajectoryPoint for 6DOF compatibility. +* Add functions to common.h: +* skewMatrixFromVector, vectorFromSkewMatrix, isRotationMatrix, +* matrixFromRotationVector, vectorFromRotationMatrix, omegaFromRotationVector +* omegaDotFromRotationVector + +3.3.2 (2018-08-22) +------------------ +* Fix indigo eigen3 compatibility. + +3.3.1 (2018-08-21) +------------------ +* Fix Eigen3 warning. Migration from Jade. +* Change maintainer. + +3.3.0 (2018-08-17) +------------------ +* Add time conversion utilities. +* Add motor position and force default topics. +* Add conversion from pose message to Eigen trajectory point. +* Add angular accelerations as member of EigenMavState to calculate motor speeds. +* Contributors: Helen Oleynikova, Karen Bodie, Rik Bähnemann + +3.2.0 (2017-03-02) +------------------ +* Access covariance in eigen odometry +* External force default topic +* External wind speed default topic + +3.1.0 (2016-12-01) +------------------ +* Add getEulerAngles method to EigenOdometry message. +* Improved quaternionFromMsg unit quaternion checking. +* Add EigenMavState to eigen_mav_msgs. +* Add EigenMavStateFromEigenTrajectoryPoint conversion. +* Add `timestamp_ns` to EigenTrajectoryPoint. +* Add default values in a seperate header. +* Add in_air bool to Status.msg. +* Add many helper function to calculate earth gravitational field based on hight and latitude, get euler angles from quaternion and shortest distance between two yaw angles. +* Contributors: Mina Kamel, Helen Oleynikova, Michael Burri + +3.0.0 (2015-08-09) +------------------ +* Dropped "Command" from the names of all messages. +* Converted CommandPositionYawTrajectory message to MultiDOFJointTrajectory (with an additional option to use a geometry_msgs/PoseStamped instead) as the two ways to send trajectory/waypoint commands. +* Added conversions between the Eigen and ROS message types. +* Switched to using full orientation instead of just yaw where appropriate. +* Documented reference frame in the Eigen messages where possible. +* Contributors: Helen Oleynikova, Markus Achtelik + +2.0.3 (2015-05-22) +------------------ +* added install target for include + Headers can be included outside of this package. +* Contributors: Fadri Furrer diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/CMakeLists.txt b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/CMakeLists.txt new file mode 100644 index 00000000..149dbbb4 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/CMakeLists.txt @@ -0,0 +1,38 @@ +cmake_minimum_required(VERSION 3.14.4...3.27) +project(mav_msgs) + +if(${CMAKE_VERSION} VERSION_GREATER 3.27) + cmake_policy(SET CMP0148 OLD) +endif() + +find_package(ament_cmake REQUIRED) + +find_package(Eigen3 REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(trajectory_msgs REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + msg/Actuators.msg + msg/AttitudeThrust.msg + msg/FilteredSensorData.msg + msg/GpsWaypoint.msg + msg/RateThrust.msg + msg/RollPitchYawrateThrust.msg + msg/Status.msg + msg/TorqueThrust.msg + DEPENDENCIES std_msgs geometry_msgs nav_msgs +) + +ament_export_dependencies(rosidl_default_runtime) + +install( + DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} + FILES_MATCHING PATTERN "*.hpp" +) + +ament_package() diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/include/mav_msgs/common.hpp b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/include/mav_msgs/common.hpp new file mode 100644 index 00000000..aa818a9c --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/include/mav_msgs/common.hpp @@ -0,0 +1,367 @@ +/* + * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland + * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland + * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland + * Copyright 2015 Helen Oleynikova, ASL, ETH Zurich, Switzerland + * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +// Common conversion functions between geometry messages, Eigen types, and yaw. + +#ifndef MAV_MSGS_COMMON_H +#define MAV_MSGS_COMMON_H + +#include +#include +#include +#include +#include + +namespace mav_msgs { + +const double kSmallValueCheck = 1.e-6; +const double kNumNanosecondsPerSecond = 1.e9; + +/// Magnitude of Earth's gravitational field at specific height [m] and latitude +/// [rad] (from wikipedia). +inline double MagnitudeOfGravity(const double height, + const double latitude_radians) { + // gravity calculation constants + const double kGravity_0 = 9.780327; + const double kGravity_a = 0.0053024; + const double kGravity_b = 0.0000058; + const double kGravity_c = 3.155 * 1e-7; + + double sin_squared_latitude = + std::sin(latitude_radians) * std::sin(latitude_radians); + double sin_squared_twice_latitude = + std::sin(2 * latitude_radians) * std::sin(2 * latitude_radians); + return kGravity_0 * ((1 + kGravity_a * sin_squared_latitude - + kGravity_b * sin_squared_twice_latitude) - + kGravity_c * height); +} + +inline Eigen::Vector3d vector3FromMsg(const geometry_msgs::msg::Vector3& msg) { + return Eigen::Vector3d(msg.x, msg.y, msg.z); +} + +inline Eigen::Vector3d vector3FromPointMsg( + const geometry_msgs::msg::Point& msg) { + return Eigen::Vector3d(msg.x, msg.y, msg.z); +} + +inline Eigen::Quaterniond quaternionFromMsg( + const geometry_msgs::msg::Quaternion& msg) { + // Make sure this always returns a valid Quaternion, even if the message was + // uninitialized or only approximately set. + Eigen::Quaterniond quaternion(msg.w, msg.x, msg.y, msg.z); + if (quaternion.norm() < std::numeric_limits::epsilon()) { + quaternion.setIdentity(); + } else { + quaternion.normalize(); + } + return quaternion; +} + +inline void vectorEigenToMsg(const Eigen::Vector3d& eigen, + geometry_msgs::msg::Vector3* msg) { + assert(msg != NULL); + msg->x = eigen.x(); + msg->y = eigen.y(); + msg->z = eigen.z(); +} + +inline void pointEigenToMsg(const Eigen::Vector3d& eigen, + geometry_msgs::msg::Point* msg) { + assert(msg != NULL); + msg->x = eigen.x(); + msg->y = eigen.y(); + msg->z = eigen.z(); +} + +inline void quaternionEigenToMsg(const Eigen::Quaterniond& eigen, + geometry_msgs::msg::Quaternion* msg) { + assert(msg != NULL); + msg->x = eigen.x(); + msg->y = eigen.y(); + msg->z = eigen.z(); + msg->w = eigen.w(); +} + +/** + * \brief Extracts the yaw part from a quaternion, using RPY / euler (z-y'-z'') + * angles. + * RPY rotates about the fixed axes in the order x-y-z, + * which is the same as euler angles in the order z-y'-x''. + */ +inline double yawFromQuaternion(const Eigen::Quaterniond& q) { + return std::atan2(2.0 * (q.w() * q.z() + q.x() * q.y()), + 1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z())); +} + +inline Eigen::Quaterniond quaternionFromYaw(double yaw) { + return Eigen::Quaterniond(Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())); +} + +inline void setQuaternionMsgFromYaw(double yaw, + geometry_msgs::msg::Quaternion* msg) { + assert(msg != NULL); + Eigen::Quaterniond q_yaw = quaternionFromYaw(yaw); + msg->x = q_yaw.x(); + msg->y = q_yaw.y(); + msg->z = q_yaw.z(); + msg->w = q_yaw.w(); +} + +inline void setAngularVelocityMsgFromYawRate(double yaw_rate, + geometry_msgs::msg::Vector3* msg) { + assert(msg != NULL); + msg->x = 0.0; + msg->y = 0.0; + msg->z = yaw_rate; +} + +inline void getEulerAnglesFromQuaternion(const Eigen::Quaternion& q, + Eigen::Vector3d* euler_angles) { + { + assert(euler_angles != NULL); + + *euler_angles << std::atan2(2.0 * (q.w() * q.x() + q.y() * q.z()), + 1.0 - 2.0 * (q.x() * q.x() + q.y() * q.y())), + std::asin(2.0 * (q.w() * q.y() - q.z() * q.x())), + std::atan2(2.0 * (q.w() * q.z() + q.x() * q.y()), + 1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z())); + } +} + +inline void skewMatrixFromVector(const Eigen::Vector3d& vec, + Eigen::Matrix3d* vec_skew) { + assert(vec_skew); + *vec_skew << 0.0f, -vec(2), vec(1), vec(2), 0.0f, -vec(0), -vec(1), vec(0), + 0.0f; +} + +inline bool vectorFromSkewMatrix(const Eigen::Matrix3d& vec_skew, + Eigen::Vector3d* vec) { + assert(vec); + if ((vec_skew + vec_skew.transpose()).norm() < kSmallValueCheck) { + *vec << vec_skew(2, 1), vec_skew(0, 2), vec_skew(1, 0); + return true; + } else { + std::cerr << "[mav_msgs] Matrix is not skew-symmetric." << std::endl; + *vec = Eigen::Vector3d::Zero(); + return false; + } +} + +inline bool isRotationMatrix(const Eigen::Matrix3d& mat) { + // Check that R^T * R = I + if ((mat.transpose() * mat - Eigen::Matrix3d::Identity()).norm() > + kSmallValueCheck) { + std::cerr << "[mav_msgs::common] Rotation matrix requirement violated (R^T " + "* R = I)" + << std::endl; + return false; + } + // Check that det(R) = 1 + if (mat.determinant() - 1.0 > kSmallValueCheck) { + std::cerr << "[mav_msgs::common] Rotation matrix requirement violated " + "(det(R) = 1)" + << std::endl; + return false; + } + return true; +} + +// Rotation matrix from rotation vector as described in +// "Computationally Efficient Trajectory Generation for Fully Actuated +// Multirotor Vehicles" Brescianini 2018 +inline void matrixFromRotationVector(const Eigen::Vector3d& vec, + Eigen::Matrix3d* mat) { + // R = (I + sin||r|| / ||r||) [r] + ((1 - cos||r||)/||r||^2) [r]^2 + // where [r] is the skew matrix of r vector + assert(mat); + double r_norm = vec.norm(); + Eigen::Matrix3d vec_skew_norm = Eigen::Matrix3d::Zero(); + if (r_norm > 0.0) { + skewMatrixFromVector(vec / r_norm, &vec_skew_norm); + } + + *mat = Eigen::Matrix3d::Identity() + vec_skew_norm * std::sin(r_norm) + + vec_skew_norm * vec_skew_norm * (1 - std::cos(r_norm)); +} + +// Rotation vector from rotation matrix as described in +// "Computationally Efficient Trajectory Generation for Fully Actuated +// Multirotor Vehicles" Brescianini 2018 +inline bool vectorFromRotationMatrix(const Eigen::Matrix3d& mat, + Eigen::Vector3d* vec) { + // [r] = phi / 2sin(phi) * (R - R^T) + // where [r] is the skew matrix of r vector + // and phi satisfies 1 + 2cos(phi) = trace(R) + assert(vec); + + if (!isRotationMatrix(mat)) { + std::cerr << "[mav_msgs::common] Not a rotation matrix." << std::endl; + return false; + } + + if ((mat - Eigen::Matrix3d::Identity()).norm() < kSmallValueCheck) { + *vec = Eigen::Vector3d::Zero(); + return true; + } + + // Compute cosine of angle and clamp in range [-1, 1] + double cos_phi = (mat.trace() - 1.0) / 2.0; + double cos_phi_clamped = boost::algorithm::clamp(cos_phi, -1.0, 1.0); + double phi = std::acos(cos_phi_clamped); + + if (phi < kSmallValueCheck) { + *vec = Eigen::Vector3d::Zero(); + } else { + Eigen::Matrix3d vec_skew = + (mat - mat.transpose()) * phi / (2.0 * std::sin(phi)); + Eigen::Vector3d vec_unskewed; + if (vectorFromSkewMatrix(vec_skew, &vec_unskewed)) { + *vec = vec_unskewed; + } else { + return false; + } + } + return true; +} + +// Calculates angular velocity (omega) from rotation vector derivative +// based on formula derived in "Finite rotations and angular velocity" by Asher +// Peres +inline Eigen::Vector3d omegaFromRotationVector( + const Eigen::Vector3d& rot_vec, const Eigen::Vector3d& rot_vec_vel) { + double phi = rot_vec.norm(); + if (std::abs(phi) < 1.0e-3) { + // This captures the case of zero rotation + return rot_vec_vel; + } + + double phi_inv = 1.0 / phi; + double phi_2_inv = phi_inv / phi; + double phi_3_inv = phi_2_inv / phi; + + // Create skew-symmetric matrix from rotation vector + Eigen::Matrix3d phi_skew; + skewMatrixFromVector(rot_vec, &phi_skew); + + // Set up matrix to calculate omega + Eigen::Matrix3d W; + W = Eigen::MatrixXd::Identity(3, 3) + + phi_skew * (1 - std::cos(phi)) * phi_2_inv + + phi_skew * phi_skew * (phi - std::sin(phi)) * phi_3_inv; + return W * rot_vec_vel; +} + +// Calculates angular acceleration (omegaDot) from rotation vector derivative +// based on formula derived in "Finite rotations and angular velocity" by Asher +// Peres +inline Eigen::Vector3d omegaDotFromRotationVector( + const Eigen::Vector3d& rot_vec, const Eigen::Vector3d& rot_vec_vel, + const Eigen::Vector3d& rot_vec_acc) { + double phi = rot_vec.norm(); + if (std::abs(phi) < 1.0e-3) { + // This captures the case of zero rotation + return rot_vec_acc; + } + + double phi_dot = rot_vec.dot(rot_vec_vel) / phi; + + double phi_inv = 1.0 / phi; + double phi_2_inv = phi_inv / phi; + double phi_3_inv = phi_2_inv / phi; + double phi_4_inv = phi_3_inv / phi; + + // Create skew-symmetric matrix from rotation vector and velocity + Eigen::Matrix3d phi_skew; + Eigen::Matrix3d phi_dot_skew; + + skewMatrixFromVector(rot_vec, &phi_skew); + skewMatrixFromVector(rot_vec_vel, &phi_dot_skew); + + // Set up matrices to calculate omega dot + Eigen::Matrix3d W_vel; + Eigen::Matrix3d W_acc; + W_vel = phi_skew * (phi * std::sin(phi) - 2.0f + 2.0f * std::cos(phi)) * + phi_dot * phi_3_inv + + phi_skew * phi_skew * + (-2.0f * phi - phi * std::cos(phi) + 3.0f * std::sin(phi)) * + phi_dot * phi_4_inv + + phi_dot_skew * phi_skew * (phi - std::sin(phi)) * phi_3_inv; + + W_acc = Eigen::MatrixXd::Identity(3, 3) + + phi_skew * (1.0f - std::cos(phi)) * phi_2_inv + + phi_skew * phi_skew * (phi - std::sin(phi)) * phi_3_inv; + + return W_vel * rot_vec_vel + W_acc * rot_vec_acc; +} + +// Calculate the nominal rotor rates given the MAV mass, allocation matrix, +// angular velocity, angular acceleration, and body acceleration (normalized +// thrust). +// +// [torques, thrust]' = A * n^2, where +// torques = J * ang_acc + ang_vel x J +// thrust = m * norm(acc) +// +// The allocation matrix A has of a hexacopter is: +// A = K * B, where +// K = diag(l*c_T, l*c_T, c_M, c_T), +// [ s 1 s -s -1 -s] +// B = [-c 0 c c 0 -c] +// [-1 1 -1 1 -1 1] +// [ 1 1 1 1 1 1], +// l: arm length +// c_T: thrust constant +// c_M: moment constant +// s: sin(30°) +// c: cos(30°) +// +// The inverse can be computed computationally efficient: +// A^-1 \approx B^pseudo * K^-1 +inline void getSquaredRotorSpeedsFromAllocationAndState( + const Eigen::MatrixXd& allocation_inv, const Eigen::Vector3d& inertia, + double mass, const Eigen::Vector3d& angular_velocity_B, + const Eigen::Vector3d& angular_acceleration_B, + const Eigen::Vector3d& acceleration_B, + Eigen::VectorXd* rotor_rates_squared) { + const Eigen::Vector3d torque = + inertia.asDiagonal() * angular_acceleration_B + + angular_velocity_B.cross(inertia.asDiagonal() * angular_velocity_B); + const double thrust_force = mass * acceleration_B.norm(); + Eigen::Vector4d input; + input << torque, thrust_force; + *rotor_rates_squared = allocation_inv * input; +} + +inline double nanosecondsToSeconds(int64_t nanoseconds) { + double seconds = nanoseconds / kNumNanosecondsPerSecond; + return seconds; +} + +inline int64_t secondsToNanoseconds(double seconds) { + int64_t nanoseconds = + static_cast(seconds * kNumNanosecondsPerSecond); + return nanoseconds; +} + +} // namespace mav_msgs + +#endif // MAV_MSGS_COMMON_H diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/include/mav_msgs/conversions.hpp b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/include/mav_msgs/conversions.hpp new file mode 100644 index 00000000..f0962586 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/include/mav_msgs/conversions.hpp @@ -0,0 +1,598 @@ +/* + * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland + * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland + * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland + * Copyright 2015 Helen Oleynikova, ASL, ETH Zurich, Switzerland + * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +// Conversion functions between Eigen types and MAV ROS message types. + +#ifndef MAV_MSGS_CONVERSIONS_H +#define MAV_MSGS_CONVERSIONS_H + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "mav_msgs/common.hpp" +#include "mav_msgs/default_values.hpp" +#include "mav_msgs/eigen_mav_msgs.hpp" +#include "mav_msgs/msg/actuators.hpp" +#include "mav_msgs/msg/attitude_thrust.hpp" +#include "mav_msgs/msg/rate_thrust.hpp" +#include "mav_msgs/msg/roll_pitch_yawrate_thrust.hpp" +#include "mav_msgs/msg/torque_thrust.hpp" + +namespace mav_msgs { + +inline void eigenAttitudeThrustFromMsg(const mav_msgs::msg::AttitudeThrust& msg, + EigenAttitudeThrust* attitude_thrust) { + assert(attitude_thrust != NULL); + + attitude_thrust->attitude = quaternionFromMsg(msg.attitude); + attitude_thrust->thrust = vector3FromMsg(msg.thrust); +} + +inline void eigenActuatorsFromMsg(const mav_msgs::msg::Actuators& msg, + EigenActuators* actuators) { + assert(actuators != NULL); + + // Angle of the actuator in [rad]. + actuators->angles.resize(msg.angles.size()); + for (unsigned int i = 0; i < msg.angles.size(); ++i) { + actuators->angles[i] = msg.angles[i]; + } + + // Angular velocities of the actuator in [rad/s]. + actuators->angular_velocities.resize(msg.angular_velocities.size()); + for (unsigned int i = 0; i < msg.angular_velocities.size(); ++i) { + actuators->angular_velocities[i] = msg.angular_velocities[i]; + } + + // Normalized: Everything that does not fit the above, normalized + // between [-1 ... 1]. + actuators->normalized.resize(msg.normalized.size()); + for (unsigned int i = 0; i < msg.normalized.size(); ++i) { + actuators->normalized[i] = msg.normalized[i]; + } +} + +inline void eigenRateThrustFromMsg(const mav_msgs::msg::RateThrust& msg, + EigenRateThrust* rate_thrust) { + assert(rate_thrust != NULL); + + rate_thrust->angular_rates = vector3FromMsg(msg.angular_rates); + rate_thrust->thrust = vector3FromMsg(msg.thrust); +} + +inline void eigenTorqueThrustFromMsg(const mav_msgs::msg::TorqueThrust& msg, + EigenTorqueThrust* torque_thrust) { + assert(torque_thrust != NULL); + + torque_thrust->torque = vector3FromMsg(msg.torque); + torque_thrust->thrust = vector3FromMsg(msg.thrust); +} + +inline void eigenRollPitchYawrateThrustFromMsg( + const mav_msgs::msg::RollPitchYawrateThrust& msg, + EigenRollPitchYawrateThrust* roll_pitch_yawrate_thrust) { + assert(roll_pitch_yawrate_thrust != NULL); + + roll_pitch_yawrate_thrust->roll = msg.roll; + roll_pitch_yawrate_thrust->pitch = msg.pitch; + roll_pitch_yawrate_thrust->yaw_rate = msg.yaw_rate; + roll_pitch_yawrate_thrust->thrust = vector3FromMsg(msg.thrust); +} + +inline void eigenOdometryFromMsg(const nav_msgs::msg::Odometry& msg, + EigenOdometry* odometry) { + assert(odometry != NULL); + + rclcpp::Time timestamp = msg.header.stamp; + + odometry->timestamp_ns = timestamp.nanoseconds(); + odometry->position_W = vector3FromPointMsg(msg.pose.pose.position); + odometry->orientation_W_B = quaternionFromMsg(msg.pose.pose.orientation); + odometry->velocity_B = vector3FromMsg(msg.twist.twist.linear); + odometry->angular_velocity_B = vector3FromMsg(msg.twist.twist.angular); + odometry->pose_covariance_ = + Eigen::Map>(msg.pose.covariance.data()); + odometry->twist_covariance_ = Eigen::Map>( + msg.twist.covariance.data()); +} + +inline void eigenTrajectoryPointFromTransformMsg( + const geometry_msgs::msg::TransformStamped& msg, + EigenTrajectoryPoint* trajectory_point) { + assert(trajectory_point != NULL); + + rclcpp::Time timestamp = msg.header.stamp; + + trajectory_point->timestamp_ns = timestamp.nanoseconds(); + trajectory_point->position_W = vector3FromMsg(msg.transform.translation); + trajectory_point->orientation_W_B = quaternionFromMsg(msg.transform.rotation); + trajectory_point->velocity_W.setZero(); + trajectory_point->angular_velocity_W.setZero(); + trajectory_point->acceleration_W.setZero(); + trajectory_point->angular_acceleration_W.setZero(); + trajectory_point->jerk_W.setZero(); + trajectory_point->snap_W.setZero(); +} + +// 2 versions of this: one for PoseStamped (which fills in the timestamps +// correctly and should be used if at all possible), and one for a raw pose +// message. +inline void eigenTrajectoryPointFromPoseMsg( + const geometry_msgs::msg::Pose& msg, + EigenTrajectoryPoint* trajectory_point) { + assert(trajectory_point != NULL); + + trajectory_point->position_W = vector3FromPointMsg(msg.position); + trajectory_point->orientation_W_B = quaternionFromMsg(msg.orientation); + trajectory_point->velocity_W.setZero(); + trajectory_point->angular_velocity_W.setZero(); + trajectory_point->acceleration_W.setZero(); + trajectory_point->angular_acceleration_W.setZero(); + trajectory_point->jerk_W.setZero(); + trajectory_point->snap_W.setZero(); +} + +inline void eigenTrajectoryPointFromPoseMsg( + const geometry_msgs::msg::PoseStamped& msg, + EigenTrajectoryPoint* trajectory_point) { + assert(trajectory_point != NULL); + + rclcpp::Time timestamp = msg.header.stamp; + + trajectory_point->timestamp_ns = timestamp.nanoseconds(); + eigenTrajectoryPointFromPoseMsg(msg.pose, trajectory_point); +} + +/** + * \brief Computes the MAV state (position, velocity, attitude, angular + * velocity, angular acceleration) from the flat state. + * Additionally, computes the acceleration expressed in body coordinates, + * i.e. the acceleration measured by the IMU. No air-drag is assumed here. + * + * \param[in] acceleration Acceleration of the MAV, expressed in world + * coordinates. + * \param[in] jerk Jerk of the MAV, expressed in world coordinates. + * \param[in] snap Snap of the MAV, expressed in world coordinates. + * \param[in] yaw Yaw angle of the MAV, expressed in world coordinates. + * \param[in] yaw_rate Yaw rate, expressed in world coordinates. + * \param[in] yaw_acceleration Yaw acceleration, expressed in world coordinates. + * \param[in] magnitude_of_gravity Magnitude of the gravity vector. + * \param[out] orientation Quaternion representing the attitude of the MAV. + * \param[out] acceleration_body Acceleration expressed in body coordinates, + * i.e. the acceleration usually by the IMU. + * \param[out] angular_velocity_body Angular velocity of the MAV, expressed in + * body coordinates. + * \param[out] angular_acceleration_body Angular acceleration of the MAV, + * expressed in body coordinates. + */ +void EigenMavStateFromEigenTrajectoryPoint( + const Eigen::Vector3d& acceleration, const Eigen::Vector3d& jerk, + const Eigen::Vector3d& snap, double yaw, double yaw_rate, + double yaw_acceleration, double magnitude_of_gravity, + Eigen::Quaterniond* orientation, Eigen::Vector3d* acceleration_body, + Eigen::Vector3d* angular_velocity_body, + Eigen::Vector3d* angular_acceleration_body); + +/// Convenience function with default value for the magnitude of the gravity +/// vector. +inline void EigenMavStateFromEigenTrajectoryPoint( + const Eigen::Vector3d& acceleration, const Eigen::Vector3d& jerk, + const Eigen::Vector3d& snap, double yaw, double yaw_rate, + double yaw_acceleration, Eigen::Quaterniond* orientation, + Eigen::Vector3d* acceleration_body, Eigen::Vector3d* angular_velocity_body, + Eigen::Vector3d* angular_acceleration_body) { + EigenMavStateFromEigenTrajectoryPoint( + acceleration, jerk, snap, yaw, yaw_rate, yaw_acceleration, kGravity, + orientation, acceleration_body, angular_velocity_body, + angular_acceleration_body); +} + +/// Convenience function with EigenTrajectoryPoint as input and EigenMavState as +/// output. +inline void EigenMavStateFromEigenTrajectoryPoint( + const EigenTrajectoryPoint& trajectory_point, double magnitude_of_gravity, + EigenMavState* mav_state) { + assert(mav_state != NULL); + if (trajectory_point.degrees_of_freedom == MavActuation::DOF4) { + EigenMavStateFromEigenTrajectoryPoint( + trajectory_point.acceleration_W, trajectory_point.jerk_W, + trajectory_point.snap_W, trajectory_point.getYaw(), + trajectory_point.getYawRate(), trajectory_point.getYawAcc(), + magnitude_of_gravity, &(mav_state->orientation_W_B), + &(mav_state->acceleration_B), &(mav_state->angular_velocity_B), + &(mav_state->angular_acceleration_B)); + mav_state->position_W = trajectory_point.position_W; + mav_state->velocity_W = trajectory_point.velocity_W; + } else { + // Assume fully actuated vehicle, can track trajectory point. + Eigen::Matrix3d R = trajectory_point.orientation_W_B.toRotationMatrix(); + mav_state->position_W = trajectory_point.position_W; + mav_state->velocity_W = trajectory_point.velocity_W; + mav_state->acceleration_B = + R.transpose() * (trajectory_point.acceleration_W + + Eigen::Vector3d(0.0, 0.0, magnitude_of_gravity)); + mav_state->orientation_W_B = trajectory_point.orientation_W_B; + mav_state->angular_velocity_B = + R.transpose() * trajectory_point.angular_velocity_W; + mav_state->angular_acceleration_B = + R.transpose() * trajectory_point.angular_acceleration_W; + } +} + +/** + * \brief Convenience function with EigenTrajectoryPoint as input and + * EigenMavState as output + * with default value for the magnitude of the gravity vector. + */ +inline void EigenMavStateFromEigenTrajectoryPoint( + const EigenTrajectoryPoint& flat_state, EigenMavState* mav_state) { + EigenMavStateFromEigenTrajectoryPoint(flat_state, kGravity, mav_state); +} + +inline void EigenMavStateFromEigenTrajectoryPoint( + const Eigen::Vector3d& acceleration, const Eigen::Vector3d& jerk, + const Eigen::Vector3d& snap, double yaw, double yaw_rate, + double yaw_acceleration, double magnitude_of_gravity, + Eigen::Quaterniond* orientation, Eigen::Vector3d* acceleration_body, + Eigen::Vector3d* angular_velocity_body, + Eigen::Vector3d* angular_acceleration_body) { + // Mapping from flat state to full state following to Mellinger [1]: + // See [1]: Mellinger, Daniel Warren. "Trajectory generation and control for + // quadrotors." (2012), Phd-thesis, p. 15ff. + // http://repository.upenn.edu/cgi/viewcontent.cgi?article=1705&context=edissertations + // + // zb = acc+[0 0 magnitude_of_gravity]'; + // thrust = norm(zb); + // zb = zb / thrust; + // + // xc = [cos(yaw) sin(yaw) 0]'; + // + // yb = cross(zb, xc); + // yb = yb/norm(yb); + // + // xb = cross(yb, zb); + // + // q(:,i) = rot2quat([xb yb zb]); + // + // h_w = 1/thrust*(acc_dot - zb' * acc_dot * zb; + // + // w(1,i) = -h_w'*yb; + // w(2,i) = h_w'*xb; + // w(3,i) = yaw_dot*[0 0 1]*zb; + + assert(orientation != nullptr); + assert(acceleration_body != nullptr); + assert(angular_velocity_body != nullptr); + assert(angular_acceleration_body != nullptr); + + Eigen::Vector3d xb; + Eigen::Vector3d yb; + Eigen::Vector3d zb(acceleration); + + zb[2] += magnitude_of_gravity; + const double thrust = zb.norm(); + const double inv_thrust = 1.0 / thrust; + zb = zb * inv_thrust; + + yb = zb.cross(Eigen::Vector3d(cos(yaw), sin(yaw), 0.0)); + yb.normalize(); + + xb = yb.cross(zb); + + const Eigen::Matrix3d R((Eigen::Matrix3d() << xb, yb, zb).finished()); + + const Eigen::Vector3d h_w = + inv_thrust * (jerk - double(zb.transpose() * jerk) * zb); + + *orientation = Eigen::Quaterniond(R); + *acceleration_body = R.transpose() * zb * thrust; + (*angular_velocity_body)[0] = -h_w.transpose() * yb; + (*angular_velocity_body)[1] = h_w.transpose() * xb; + (*angular_velocity_body)[2] = yaw_rate * zb[2]; + + // Calculate angular accelerations. + const Eigen::Vector3d wcrossz = (*angular_velocity_body).cross(zb); + const Eigen::Vector3d wcrosswcrossz = (*angular_velocity_body).cross(wcrossz); + const Eigen::Vector3d h_a = + inv_thrust * (snap - double(zb.transpose() * snap) * zb) - 2 * wcrossz - + wcrosswcrossz + double(zb.transpose() * wcrosswcrossz) * zb; + + (*angular_acceleration_body)[0] = -h_a.transpose() * yb; + (*angular_acceleration_body)[1] = h_a.transpose() * xb; + (*angular_acceleration_body)[2] = yaw_acceleration * zb[2]; +} + +inline void eigenTrajectoryPointFromMsg( + const trajectory_msgs::msg::MultiDOFJointTrajectoryPoint& msg, + EigenTrajectoryPoint* trajectory_point) { + assert(trajectory_point != NULL); + + if (msg.transforms.empty()) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), + "MultiDofJointTrajectoryPoint is empty."); + return; + } + + if (msg.transforms.size() > 1) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "MultiDofJointTrajectoryPoint message should have one joint, but has " + "%lu. Using first joint.", + msg.transforms.size()); + } + + rclcpp::Duration duration = msg.time_from_start; + + trajectory_point->time_from_start_ns = duration.nanoseconds(); + trajectory_point->position_W = vector3FromMsg(msg.transforms[0].translation); + trajectory_point->orientation_W_B = + quaternionFromMsg(msg.transforms[0].rotation); + if (msg.velocities.size() > 0) { + trajectory_point->velocity_W = vector3FromMsg(msg.velocities[0].linear); + trajectory_point->angular_velocity_W = + vector3FromMsg(msg.velocities[0].angular); + } else { + trajectory_point->velocity_W.setZero(); + trajectory_point->angular_velocity_W.setZero(); + } + if (msg.accelerations.size() > 0) { + trajectory_point->acceleration_W = + vector3FromMsg(msg.accelerations[0].linear); + trajectory_point->angular_acceleration_W = + vector3FromMsg(msg.accelerations[0].angular); + } else { + trajectory_point->acceleration_W.setZero(); + trajectory_point->angular_acceleration_W.setZero(); + } + trajectory_point->jerk_W.setZero(); + trajectory_point->snap_W.setZero(); +} + +inline void eigenTrajectoryPointVectorFromMsg( + const trajectory_msgs::msg::MultiDOFJointTrajectory& msg, + EigenTrajectoryPointVector* trajectory) { + assert(trajectory != NULL); + trajectory->clear(); + for (const auto& msg_point : msg.points) { + EigenTrajectoryPoint point; + eigenTrajectoryPointFromMsg(msg_point, &point); + trajectory->push_back(point); + } +} + +inline void eigenTrajectoryPointDequeFromMsg( + const trajectory_msgs::msg::MultiDOFJointTrajectory& msg, + EigenTrajectoryPointDeque* trajectory) { + assert(trajectory != NULL); + trajectory->clear(); + for (const auto& msg_point : msg.points) { + EigenTrajectoryPoint point; + eigenTrajectoryPointFromMsg(msg_point, &point); + trajectory->push_back(point); + } +} + +// In all these conversions, client is responsible for filling in message +// header. +inline void msgActuatorsFromEigen(const EigenActuators& actuators, + mav_msgs::msg::Actuators* msg) { + assert(msg != NULL); + + msg->angles.resize(actuators.angles.size()); + for (unsigned int i = 0; i < actuators.angles.size(); ++i) { + msg->angles[i] = actuators.angles[i]; + } + + msg->angular_velocities.resize(actuators.angular_velocities.size()); + for (unsigned int i = 0; i < actuators.angular_velocities.size(); ++i) { + msg->angular_velocities[i] = actuators.angular_velocities[i]; + } + + msg->normalized.resize(actuators.normalized.size()); + for (unsigned int i = 0; i < actuators.normalized.size(); ++i) { + msg->normalized[i] = actuators.normalized[i]; + } +} + +inline void msgAttitudeThrustFromEigen( + const EigenAttitudeThrust& attitude_thrust, + mav_msgs::msg::AttitudeThrust* msg) { + assert(msg != NULL); + quaternionEigenToMsg(attitude_thrust.attitude, &msg->attitude); + vectorEigenToMsg(attitude_thrust.thrust, &msg->thrust); +} + +inline void msgRateThrustFromEigen(EigenRateThrust& rate_thrust, + mav_msgs::msg::RateThrust* msg) { + assert(msg != NULL); + vectorEigenToMsg(rate_thrust.angular_rates, &msg->angular_rates); + vectorEigenToMsg(rate_thrust.thrust, &msg->thrust); +} + +inline void msgTorqueThrustFromEigen(EigenTorqueThrust& torque_thrust, + mav_msgs::msg::TorqueThrust* msg) { + assert(msg != NULL); + vectorEigenToMsg(torque_thrust.torque, &msg->torque); + vectorEigenToMsg(torque_thrust.thrust, &msg->thrust); +} + +inline void msgRollPitchYawrateThrustFromEigen( + const EigenRollPitchYawrateThrust& roll_pitch_yawrate_thrust, + mav_msgs::msg::RollPitchYawrateThrust* msg) { + assert(msg != NULL); + msg->roll = roll_pitch_yawrate_thrust.roll; + msg->pitch = roll_pitch_yawrate_thrust.pitch; + msg->yaw_rate = roll_pitch_yawrate_thrust.yaw_rate; + vectorEigenToMsg(roll_pitch_yawrate_thrust.thrust, &msg->thrust); +} + +inline void msgOdometryFromEigen(const EigenOdometry& odometry, + nav_msgs::msg::Odometry* msg) { + assert(msg != NULL); + + rclcpp::Time timestamp(odometry.timestamp_ns); + + if (odometry.timestamp_ns >= 0) { + msg->header.stamp = timestamp; + } + pointEigenToMsg(odometry.position_W, &msg->pose.pose.position); + quaternionEigenToMsg(odometry.orientation_W_B, &msg->pose.pose.orientation); + + vectorEigenToMsg(odometry.velocity_B, &msg->twist.twist.linear); + vectorEigenToMsg(odometry.angular_velocity_B, &msg->twist.twist.angular); +} + +// WARNING: discards all derivatives, etc. +inline void msgPoseStampedFromEigenTrajectoryPoint( + const EigenTrajectoryPoint& trajectory_point, + geometry_msgs::msg::PoseStamped* msg) { + if (trajectory_point.timestamp_ns >= 0) { + rclcpp::Time timestamp(trajectory_point.timestamp_ns); + msg->header.stamp = timestamp; + } + pointEigenToMsg(trajectory_point.position_W, &msg->pose.position); + quaternionEigenToMsg(trajectory_point.orientation_W_B, + &msg->pose.orientation); +} + +inline void msgMultiDofJointTrajectoryPointFromEigen( + const EigenTrajectoryPoint& trajectory_point, + trajectory_msgs::msg::MultiDOFJointTrajectoryPoint* msg) { + assert(msg != NULL); + + std::chrono::nanoseconds time_from_start_ns{ + trajectory_point.time_from_start_ns}; + rclcpp::Duration time_from_start(time_from_start_ns); + + msg->time_from_start = time_from_start; + msg->transforms.resize(1); + msg->velocities.resize(1); + msg->accelerations.resize(1); + + vectorEigenToMsg(trajectory_point.position_W, + &msg->transforms[0].translation); + quaternionEigenToMsg(trajectory_point.orientation_W_B, + &msg->transforms[0].rotation); + vectorEigenToMsg(trajectory_point.velocity_W, &msg->velocities[0].linear); + vectorEigenToMsg(trajectory_point.angular_velocity_W, + &msg->velocities[0].angular); + vectorEigenToMsg(trajectory_point.acceleration_W, + &msg->accelerations[0].linear); + vectorEigenToMsg(trajectory_point.angular_acceleration_W, + &msg->accelerations[0].angular); +} + +inline void msgMultiDofJointTrajectoryFromEigen( + const EigenTrajectoryPoint& trajectory_point, const std::string& link_name, + trajectory_msgs::msg::MultiDOFJointTrajectory* msg) { + assert(msg != NULL); + trajectory_msgs::msg::MultiDOFJointTrajectoryPoint point_msg; + msgMultiDofJointTrajectoryPointFromEigen(trajectory_point, &point_msg); + + msg->joint_names.clear(); + msg->points.clear(); + msg->joint_names.push_back(link_name); + msg->points.push_back(point_msg); +} + +inline void msgMultiDofJointTrajectoryFromEigen( + const EigenTrajectoryPoint& trajectory_point, + trajectory_msgs::msg::MultiDOFJointTrajectory* msg) { + msgMultiDofJointTrajectoryFromEigen(trajectory_point, "base_link", msg); +} + +// Convenience method to quickly create a trajectory from a single waypoint. +inline void msgMultiDofJointTrajectoryFromPositionYaw( + const Eigen::Vector3d& position, double yaw, + trajectory_msgs::msg::MultiDOFJointTrajectory* msg) { + assert(msg != NULL); + + EigenTrajectoryPoint point; + point.position_W = position; + point.setFromYaw(yaw); + + msgMultiDofJointTrajectoryFromEigen(point, msg); +} + +inline void msgMultiDofJointTrajectoryFromEigen( + const EigenTrajectoryPointVector& trajectory, const std::string& link_name, + trajectory_msgs::msg::MultiDOFJointTrajectory* msg) { + assert(msg != NULL); + + if (trajectory.empty()) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), + "EigenTrajectoryPointVector is empty."); + return; + } + + msg->joint_names.clear(); + msg->joint_names.push_back(link_name); + msg->points.clear(); + + for (const auto& trajectory_point : trajectory) { + trajectory_msgs::msg::MultiDOFJointTrajectoryPoint point_msg; + msgMultiDofJointTrajectoryPointFromEigen(trajectory_point, &point_msg); + msg->points.push_back(point_msg); + } +} + +inline void msgMultiDofJointTrajectoryFromEigen( + const EigenTrajectoryPointVector& trajectory, + trajectory_msgs::msg::MultiDOFJointTrajectory* msg) { + msgMultiDofJointTrajectoryFromEigen(trajectory, "base_link", msg); +} + +inline void msgMultiDofJointTrajectoryFromEigen( + const EigenTrajectoryPointDeque& trajectory, const std::string& link_name, + trajectory_msgs::msg::MultiDOFJointTrajectory* msg) { + assert(msg != NULL); + + if (trajectory.empty()) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), + "EigenTrajectoryPointVector is empty."); + return; + } + + msg->joint_names.clear(); + msg->joint_names.push_back(link_name); + msg->points.clear(); + + for (const auto& trajectory_point : trajectory) { + trajectory_msgs::msg::MultiDOFJointTrajectoryPoint point_msg; + msgMultiDofJointTrajectoryPointFromEigen(trajectory_point, &point_msg); + msg->points.push_back(point_msg); + } +} + +inline void msgMultiDofJointTrajectoryFromEigen( + const EigenTrajectoryPointDeque& trajectory, + trajectory_msgs::msg::MultiDOFJointTrajectory* msg) { + msgMultiDofJointTrajectoryFromEigen(trajectory, "base_link", msg); +} + +} // end namespace mav_msgs + +#endif // MAV_MSGS_CONVERSIONS_H diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/include/mav_msgs/default_topics.hpp b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/include/mav_msgs/default_topics.hpp new file mode 100644 index 00000000..b90b169c --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/include/mav_msgs/default_topics.hpp @@ -0,0 +1,71 @@ +/* + * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland + * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland + * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland + * Copyright 2015 Helen Oleynikova, ASL, ETH Zurich, Switzerland + * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef DEFAULT_TOPICS_H_ +#define DEFAULT_TOPICS_H_ + +namespace mav_msgs { +namespace default_topics { + +static constexpr char IMU[] = "imu"; +static constexpr char MOTOR_MEASUREMENT[] = "motor_speed"; +static constexpr char MOTOR_POSITION_MEASUREMENT[] = "motor_position"; +static constexpr char MOTOR_FORCE_MEASUREMENT[] = "motor_force"; +static constexpr char MAGNETIC_FIELD[] = "magnetic_field"; +static constexpr char GPS[] = "gps"; +static constexpr char RC[] = "rc"; +static constexpr char STATUS[] = "status"; +static constexpr char FILTERED_SENSOR_DATA[] = "filtered_sensor_data"; +static constexpr char AIR_SPEED[] = "air_speed"; +static constexpr char GROUND_SPEED[] = "ground_speed"; + +static constexpr char COMMAND_ACTUATORS[] = "command/motor_speed"; +static constexpr char COMMAND_RATE_THRUST[] = "command/rate_thrust"; +static constexpr char COMMAND_ROLL_PITCH_YAWRATE_THRUST[] = + "command/roll_pitch_yawrate_thrust"; +static constexpr char COMMAND_ATTITUDE_THRUST[] = "command/attitude_thrust"; +static constexpr char COMMAND_TRAJECTORY[] = "command/trajectory"; +static constexpr char COMMAND_POSE[] = "command/pose"; +static constexpr char COMMAND_GPS_WAYPOINT[] = "command/gps_waypoint"; + +static constexpr char POSE[] = "pose"; +static constexpr char POSE_WITH_COVARIANCE[] = "pose_with_covariance"; +static constexpr char TRANSFORM[] = "transform"; +static constexpr char ODOMETRY[] = "odometry"; +static constexpr char POSITION[] = "position"; + +// Simulation-specific topic names. +static constexpr char WRENCH[] = "wrench"; +static constexpr char WIND_SPEED[] = "wind_speed"; +static constexpr char EXTERNAL_FORCE[] = "external_force"; + +static constexpr char GROUND_TRUTH_POSE[] = "ground_truth/pose"; +static constexpr char GROUND_TRUTH_TWIST[] = "ground_truth/twist"; + +// Flight controller specific topic names +static constexpr char FLIGHT_CONTROLLER_ARM[] = "mavros/cmd/arming"; +static constexpr char FLIGHT_CONTROLLER_SET_MODE[] = "mavros/set_mode"; +static constexpr char FLIGHT_CONTROLLER_STATE[] = "mavros/state"; +static constexpr char FLIGHT_CONTROLLER_BATTERY[] = "mavros/battery"; + +} // end namespace default_topics +} // end namespace mav_msgs + +#endif /* DEFAULT_TOPICS_H_ */ diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/include/mav_msgs/default_values.hpp b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/include/mav_msgs/default_values.hpp new file mode 100644 index 00000000..aa24b5a0 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/include/mav_msgs/default_values.hpp @@ -0,0 +1,33 @@ +/* + * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland + * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland + * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland + * Copyright 2015 Helen Oleynikova, ASL, ETH Zurich, Switzerland + * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef MAV_MSGS_DEFAULT_VALUES_H_ +#define MAV_MSGS_DEFAULT_VALUES_H_ + +#include "mav_msgs/common.hpp" + +namespace mav_msgs { + +const double kZurichLatitude = 0.8267; +const double kZurichHeight = 405.94; +const double kGravity = MagnitudeOfGravity(kZurichHeight, kZurichLatitude); +} // namespace mav_msgs + +#endif /* MAV_MSGS_DEFAULT_VALUES_H_ */ diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/include/mav_msgs/eigen_mav_msgs.hpp b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/include/mav_msgs/eigen_mav_msgs.hpp new file mode 100644 index 00000000..6df1ac47 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/include/mav_msgs/eigen_mav_msgs.hpp @@ -0,0 +1,349 @@ +/* + * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland + * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland + * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland + * Copyright 2015 Helen Oleynikova, ASL, ETH Zurich, Switzerland + * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef MAV_MSGS_EIGEN_MAV_MSGS_H +#define MAV_MSGS_EIGEN_MAV_MSGS_H + +#include +#include +#include + +#include "mav_msgs/common.hpp" + +namespace mav_msgs { + +/// Actuated degrees of freedom. +enum MavActuation { DOF4 = 4, DOF6 = 6 }; + +struct EigenAttitudeThrust { + EigenAttitudeThrust() + : attitude(Eigen::Quaterniond::Identity()), + thrust(Eigen::Vector3d::Zero()) {} + EigenAttitudeThrust(const Eigen::Quaterniond& _attitude, + const Eigen::Vector3d& _thrust) { + attitude = _attitude; + thrust = _thrust; + } + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + Eigen::Quaterniond attitude; + Eigen::Vector3d thrust; +}; + +struct EigenActuators { + // TODO(ffurrer): Find a proper way of initializing :) + + EigenActuators(const Eigen::VectorXd& _angular_velocities) { + angular_velocities = _angular_velocities; + } + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + Eigen::VectorXd angles; // In rad. + Eigen::VectorXd angular_velocities; // In rad/s. + Eigen::VectorXd normalized; // Everything else, normalized [-1 to 1]. +}; + +struct EigenRateThrust { + EigenRateThrust() + : angular_rates(Eigen::Vector3d::Zero()), + thrust(Eigen::Vector3d::Zero()) {} + + EigenRateThrust(const Eigen::Vector3d& _angular_rates, + const Eigen::Vector3d _thrust) + : angular_rates(_angular_rates), thrust(_thrust) {} + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + Eigen::Vector3d angular_rates; + Eigen::Vector3d thrust; +}; + +struct EigenTorqueThrust { + EigenTorqueThrust() + : torque(Eigen::Vector3d::Zero()), thrust(Eigen::Vector3d::Zero()) {} + + EigenTorqueThrust(const Eigen::Vector3d& _torque, + const Eigen::Vector3d _thrust) + : torque(_torque), thrust(_thrust) {} + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + Eigen::Vector3d torque; + Eigen::Vector3d thrust; +}; + +struct EigenRollPitchYawrateThrust { + EigenRollPitchYawrateThrust() + : roll(0.0), pitch(0.0), yaw_rate(0.0), thrust(Eigen::Vector3d::Zero()) {} + + EigenRollPitchYawrateThrust(double _roll, double _pitch, double _yaw_rate, + const Eigen::Vector3d& _thrust) + : roll(_roll), pitch(_pitch), yaw_rate(_yaw_rate), thrust(_thrust) {} + + double roll; + double pitch; + double yaw_rate; + Eigen::Vector3d thrust; +}; + +/** + * \brief Container holding the state of a MAV: position, velocity, attitude and + * angular velocity. + * In addition, holds the acceleration expressed in body coordinates, + * which is what the accelerometer + * usually measures. + */ +class EigenMavState { + public: + typedef std::vector> + Vector; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /// Initializes all members to zero / identity. + EigenMavState() + : position_W(Eigen::Vector3d::Zero()), + velocity_W(Eigen::Vector3d::Zero()), + acceleration_B(Eigen::Vector3d::Zero()), + orientation_W_B(Eigen::Quaterniond::Identity()), + angular_velocity_B(Eigen::Vector3d::Zero()), + angular_acceleration_B(Eigen::Vector3d::Zero()) {} + + EigenMavState(const Eigen::Vector3d& position_W, + const Eigen::Vector3d& velocity_W, + const Eigen::Vector3d& acceleration_B, + const Eigen::Quaterniond& orientation_W_B, + const Eigen::Vector3d& angular_velocity_B, + const Eigen::Vector3d& angular_acceleration_B) + : position_W(position_W), + velocity_W(velocity_W), + acceleration_B(acceleration_B), + orientation_W_B(orientation_W_B), + angular_velocity_B(angular_velocity_B), + angular_acceleration_B(angular_acceleration_B) {} + + std::string toString() const { + std::stringstream ss; + ss << "position: " << position_W.transpose() << std::endl + << "velocity: " << velocity_W.transpose() << std::endl + << "acceleration_body: " << acceleration_B.transpose() << std::endl + << "orientation (w-x-y-z): " << orientation_W_B.w() << " " + << orientation_W_B.x() << " " << orientation_W_B.y() << " " + << orientation_W_B.z() << " " << std::endl + << "angular_velocity_body: " << angular_velocity_B.transpose() + << std::endl + << "angular_acceleration_body: " << angular_acceleration_B.transpose() + << std::endl; + + return ss.str(); + } + + Eigen::Vector3d position_W; + Eigen::Vector3d velocity_W; + Eigen::Vector3d acceleration_B; + Eigen::Quaterniond orientation_W_B; + Eigen::Vector3d angular_velocity_B; + Eigen::Vector3d angular_acceleration_B; +}; + +struct EigenTrajectoryPoint { + typedef std::vector> + Vector; + EigenTrajectoryPoint() + : timestamp_ns(-1), + time_from_start_ns(0), + position_W(Eigen::Vector3d::Zero()), + velocity_W(Eigen::Vector3d::Zero()), + acceleration_W(Eigen::Vector3d::Zero()), + jerk_W(Eigen::Vector3d::Zero()), + snap_W(Eigen::Vector3d::Zero()), + orientation_W_B(Eigen::Quaterniond::Identity()), + angular_velocity_W(Eigen::Vector3d::Zero()), + angular_acceleration_W(Eigen::Vector3d::Zero()), + degrees_of_freedom(MavActuation::DOF4) {} + + EigenTrajectoryPoint( + int64_t _time_from_start_ns, const Eigen::Vector3d& _position, + const Eigen::Vector3d& _velocity, const Eigen::Vector3d& _acceleration, + const Eigen::Vector3d& _jerk, const Eigen::Vector3d& _snap, + const Eigen::Quaterniond& _orientation, + const Eigen::Vector3d& _angular_velocity, + const Eigen::Vector3d& _angular_acceleration, + const MavActuation& _degrees_of_freedom = MavActuation::DOF4) + : time_from_start_ns(_time_from_start_ns), + position_W(_position), + velocity_W(_velocity), + acceleration_W(_acceleration), + jerk_W(_jerk), + snap_W(_snap), + orientation_W_B(_orientation), + angular_velocity_W(_angular_velocity), + angular_acceleration_W(_angular_acceleration), + degrees_of_freedom(_degrees_of_freedom) {} + + EigenTrajectoryPoint( + int64_t _time_from_start_ns, const Eigen::Vector3d& _position, + const Eigen::Vector3d& _velocity, const Eigen::Vector3d& _acceleration, + const Eigen::Vector3d& _jerk, const Eigen::Vector3d& _snap, + const Eigen::Quaterniond& _orientation, + const Eigen::Vector3d& _angular_velocity, + const MavActuation& _degrees_of_freedom = MavActuation::DOF4) + : EigenTrajectoryPoint(_time_from_start_ns, _position, _velocity, + _acceleration, _jerk, _snap, _orientation, + _angular_velocity, Eigen::Vector3d::Zero(), + _degrees_of_freedom) {} + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + int64_t + timestamp_ns; // Time since epoch, negative value = invalid timestamp. + int64_t time_from_start_ns; + Eigen::Vector3d position_W; + Eigen::Vector3d velocity_W; + Eigen::Vector3d acceleration_W; + Eigen::Vector3d jerk_W; + Eigen::Vector3d snap_W; + + Eigen::Quaterniond orientation_W_B; + Eigen::Vector3d angular_velocity_W; + Eigen::Vector3d angular_acceleration_W; + MavActuation degrees_of_freedom; + + // Accessors for making dealing with orientation/angular velocity easier. + inline double getYaw() const { return yawFromQuaternion(orientation_W_B); } + inline double getYawRate() const { return angular_velocity_W.z(); } + inline double getYawAcc() const { return angular_acceleration_W.z(); } + // WARNING: sets roll and pitch to 0. + inline void setFromYaw(double yaw) { + orientation_W_B = quaternionFromYaw(yaw); + } + inline void setFromYawRate(double yaw_rate) { + angular_velocity_W.x() = 0.0; + angular_velocity_W.y() = 0.0; + angular_velocity_W.z() = yaw_rate; + } + inline void setFromYawAcc(double yaw_acc) { + angular_acceleration_W.x() = 0.0; + angular_acceleration_W.y() = 0.0; + angular_acceleration_W.z() = yaw_acc; + } + + std::string toString() const { + std::stringstream ss; + ss << "position: " << position_W.transpose() << std::endl + << "velocity: " << velocity_W.transpose() << std::endl + << "acceleration: " << acceleration_W.transpose() << std::endl + << "jerk: " << jerk_W.transpose() << std::endl + << "snap: " << snap_W.transpose() << std::endl + << "yaw: " << getYaw() << std::endl + << "yaw_rate: " << getYawRate() << std::endl + << "yaw_acc: " << getYawAcc() << std::endl; + + return ss.str(); + } +}; + +// Operator overload to transform Trajectory Points according to the Eigen +// interfaces (uses operator* for this). +// Has to be outside of class. +// Example: +// Eigen::Affine3d transform; EigenTrajectoryPoint point; +// EigenTrajectoryPoint transformed = transform * point; +inline EigenTrajectoryPoint operator*(const Eigen::Affine3d& lhs, + const EigenTrajectoryPoint& rhs) { + EigenTrajectoryPoint transformed(rhs); + transformed.position_W = lhs * rhs.position_W; + transformed.velocity_W = lhs.rotation() * rhs.velocity_W; + transformed.acceleration_W = lhs.rotation() * rhs.acceleration_W; + transformed.jerk_W = lhs.rotation() * rhs.jerk_W; + transformed.snap_W = lhs.rotation() * rhs.snap_W; + transformed.orientation_W_B = lhs.rotation() * rhs.orientation_W_B; + transformed.angular_velocity_W = lhs.rotation() * rhs.angular_velocity_W; + transformed.angular_acceleration_W = + lhs.rotation() * rhs.angular_acceleration_W; + return transformed; +} + +struct EigenOdometry { + EigenOdometry() + : timestamp_ns(-1), + position_W(Eigen::Vector3d::Zero()), + orientation_W_B(Eigen::Quaterniond::Identity()), + velocity_B(Eigen::Vector3d::Zero()), + angular_velocity_B(Eigen::Vector3d::Zero()) {} + + EigenOdometry(const Eigen::Vector3d& _position, + const Eigen::Quaterniond& _orientation, + const Eigen::Vector3d& _velocity_body, + const Eigen::Vector3d& _angular_velocity) + : position_W(_position), + orientation_W_B(_orientation), + velocity_B(_velocity_body), + angular_velocity_B(_angular_velocity) {} + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + int64_t + timestamp_ns; // Time since epoch, negative value = invalid timestamp. + Eigen::Vector3d position_W; + Eigen::Quaterniond orientation_W_B; + Eigen::Vector3d velocity_B; // Velocity in expressed in the Body frame! + Eigen::Vector3d angular_velocity_B; + Eigen::Matrix pose_covariance_; + Eigen::Matrix twist_covariance_; + + // Accessors for making dealing with orientation/angular velocity easier. + inline double getYaw() const { return yawFromQuaternion(orientation_W_B); } + inline void getEulerAngles(Eigen::Vector3d* euler_angles) const { + getEulerAnglesFromQuaternion(orientation_W_B, euler_angles); + } + inline double getYawRate() const { return angular_velocity_B.z(); } + // WARNING: sets roll and pitch to 0. + inline void setFromYaw(double yaw) { + orientation_W_B = quaternionFromYaw(yaw); + } + inline void setFromYawRate(double yaw_rate) { + angular_velocity_B.x() = 0.0; + angular_velocity_B.y() = 0.0; + angular_velocity_B.z() = yaw_rate; + } + + inline Eigen::Vector3d getVelocityWorld() const { + return orientation_W_B * velocity_B; + } + inline void setVelocityWorld(const Eigen::Vector3d& velocity_world) { + velocity_B = orientation_W_B.inverse() * velocity_world; + } +}; + +// TODO(helenol): replaced with aligned allocator headers from Simon. +#define MAV_MSGS_CONCATENATE(x, y) x##y +#define MAV_MSGS_CONCATENATE2(x, y) MAV_MSGS_CONCATENATE(x, y) +#define MAV_MSGS_MAKE_ALIGNED_CONTAINERS(EIGEN_TYPE) \ + typedef std::vector> \ + MAV_MSGS_CONCATENATE2(EIGEN_TYPE, Vector); \ + typedef std::deque> \ + MAV_MSGS_CONCATENATE2(EIGEN_TYPE, Deque); + +MAV_MSGS_MAKE_ALIGNED_CONTAINERS(EigenAttitudeThrust) +MAV_MSGS_MAKE_ALIGNED_CONTAINERS(EigenActuators) +MAV_MSGS_MAKE_ALIGNED_CONTAINERS(EigenRateThrust) +MAV_MSGS_MAKE_ALIGNED_CONTAINERS(EigenTrajectoryPoint) +MAV_MSGS_MAKE_ALIGNED_CONTAINERS(EigenRollPitchYawrateThrust) +MAV_MSGS_MAKE_ALIGNED_CONTAINERS(EigenOdometry) +} // namespace mav_msgs + +#endif // MAV_MSGS_EIGEN_MAV_MSGS_H diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/msg/Actuators.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/msg/Actuators.msg new file mode 100644 index 00000000..913213b2 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/msg/Actuators.msg @@ -0,0 +1,9 @@ +std_msgs/Header header + +# This message defines lowest level commands to be sent to the actuator(s). + +float64[] angles # Angle of the actuator in [rad]. + # E.g. servo angle of a control surface(not angle of the surface!), orientation-angle of a thruster. +float64[] angular_velocities # Angular velocities of the actuator in [rad/s]. + # E.g. "rpm" of rotors, propellers, thrusters +float64[] normalized # Everything that does not fit the above, normalized between [-1 ... 1]. \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/msg/AttitudeThrust.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/msg/AttitudeThrust.msg new file mode 100644 index 00000000..18e417f2 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/msg/AttitudeThrust.msg @@ -0,0 +1,7 @@ +std_msgs/Header header + +geometry_msgs/Quaternion attitude # Attitude expressed in the header/frame_id frame. +geometry_msgs/Vector3 thrust # Thrust [N] expressed in the body frame. + # For a fixed-wing, usually the x-component is used. + # For a multi-rotor, usually the z-component is used. + # Set all un-used components to 0. diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/msg/FilteredSensorData.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/msg/FilteredSensorData.msg new file mode 100644 index 00000000..ecd6d895 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/msg/FilteredSensorData.msg @@ -0,0 +1,6 @@ +std_msgs/Header header + +geometry_msgs/Vector3 accelerometer # acceleration in vehicle frame [m/s^2] +geometry_msgs/Vector3 gyroscope # rotational velocity in vehicle frame [rad/s] +geometry_msgs/Vector3 magnetometer # Magnetometer measurements in vehicle frame [uT] +float64 barometer # Height from barometer relative to start-up point [m] diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/msg/GpsWaypoint.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/msg/GpsWaypoint.msg new file mode 100644 index 00000000..127e7b43 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/msg/GpsWaypoint.msg @@ -0,0 +1,8 @@ +std_msgs/Header header + +float64 latitude # latitude in degree +float64 longitude # longitude in degree +float64 altitude # above start-up point +float64 heading # GPS heading +float64 max_speed # maximum approach speed +float64 max_acc # maximum approach accelerations diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/msg/RateThrust.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/msg/RateThrust.msg new file mode 100644 index 00000000..06f0e1f8 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/msg/RateThrust.msg @@ -0,0 +1,12 @@ +std_msgs/Header header + +# We use the coordinate frames with the following convention: +# x: forward +# y: left +# z: up + +geometry_msgs/Vector3 angular_rates # Roll-, pitch-, yaw-rate around body axes [rad/s] +geometry_msgs/Vector3 thrust # Thrust [N] expressed in the body frame. + # For a fixed-wing, usually the x-component is used. + # For a multi-rotor, usually the z-component is used. + # Set all un-used components to 0. diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/msg/RollPitchYawrateThrust.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/msg/RollPitchYawrateThrust.msg new file mode 100644 index 00000000..a602b1aa --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/msg/RollPitchYawrateThrust.msg @@ -0,0 +1,23 @@ +std_msgs/Header header + +# We use the coordinate frames with the following convention: +# x: forward +# y: left +# z: up + +# rotation convention (z-y'-x''): +# yaw rotates around fixed frame's z axis +# pitch rotates around new y-axis (y') +# roll rotates around new x-axis (x'') + +# This is a convenience-message to support that low-level (microcontroller-based) state +# estimators may not have knowledge about the absolute yaw. +# Roll- and pitch-angle should be specified in the header/frame_id frame +float64 roll # Roll angle [rad] +float64 pitch # Pitch angle [rad] +float64 yaw_rate # Yaw rate around z-axis [rad/s] + +geometry_msgs/Vector3 thrust # Thrust [N] expressed in the body frame. + # For a fixed-wing, usually the x-component is used. + # For a multi-rotor, usually the z-component is used. + # Set all un-used components to 0. diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/msg/Status.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/msg/Status.msg new file mode 100644 index 00000000..c47dea9a --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/msg/Status.msg @@ -0,0 +1,29 @@ +std_msgs/Header header + +# If values are not known / available, set to -1 or empty string. +string vehicle_name +string vehicle_type # E.g. firefly, pelican ... +float32 battery_voltage # Battery voltage in V. +string rc_command_mode # Command mode set on the 3 position switch on the rc. +bool command_interface_enabled # Reports whether the serial command interface is enabled. +float32 flight_time # Flight time in s. +float32 system_uptime # MAV uptime in s. +float32 cpu_load # MAV CPU load: 0.0 ... 1.0 + +string motor_status # Current motor status: running, stopped, starting, stopping. +bool in_air # True if vehicle is actually in air, false otherwise + +string gps_status # GPS status: lock, no_lock +int32 gps_num_satellites # Number of visible satellites + +string RC_COMMAND_ATTITUDE="attitude_thrust" +string RC_COMMAND_ATTITUDE_HEIGHT="attitude_height" +string RC_COMMAND_POSITION="position" + +string MOTOR_STATUS_RUNNING="running" +string MOTOR_STATUS_STOPPED="stopped" +string MOTOR_STATUS_STARTING="starting" +string MOTOR_STATUS_STOPPING="stopping" + +string GPS_STATUS_LOCK="lock" +string GPS_STATUS_NO_LOCK="no_lock" diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/msg/TorqueThrust.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/msg/TorqueThrust.msg new file mode 100644 index 00000000..75d046e0 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/msg/TorqueThrust.msg @@ -0,0 +1,12 @@ +std_msgs/Header header + +# We use the coordinate frames with the following convention: +# x: forward +# y: left +# z: up + +geometry_msgs/Vector3 torque # Torque expressed in the body frame [Nm]. +geometry_msgs/Vector3 thrust # Thrust [N] expressed in the body frame. + # For a fixed-wing, usually the x-component is used. + # For a multi-rotor, usually the z-component is used. + # Set all un-used components to 0. diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/package.xml b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/package.xml new file mode 100644 index 00000000..566855ce --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_msgs/package.xml @@ -0,0 +1,44 @@ + + + + mav_msgs + 3.3.3 + Package containing messages for communicating with rotary wing MAVs + + Rik Bähnemann + + Simon Lynen + Markus Achtelik + Pascal Gohl + Sammy Omari + Michael Burri + Fadri Furrer + Helen Oleynikova + Mina Kamel + Karen Bodie + Rik Bähnemann + + ASL 2.0 + + https://github.com/ethz-asl/mav_comm + https://github.com/ethz-asl/mav_comm/issues + + ament_cmake + rosidl_default_generators + + eigen + rclcpp + + geometry_msgs + nav_msgs + std_msgs + trajectory_msgs + + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/CHANGELOG.rst b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/CHANGELOG.rst new file mode 100644 index 00000000..3722eef9 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/CHANGELOG.rst @@ -0,0 +1,25 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package mav_planning_msgs +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.3.3 (2019-08-16) +------------------ +* Add PolynomialSegment/Trajectory messages with 3D rotation vector. +* Update conversions, deprecate old conversions. +* Update planner service to take 4D or full trajectory message + +3.3.2 (2018-08-22) +------------------ +* Fix indigo eigen3 compatibility. + +3.3.1 (2018-08-21) +------------------ +* Fix Eigen3 warning. Migration from Jade. +* Add std_msgs, eigen and cmake_modules dependencies. + +3.3.0 (2018-08-17) +------------------ +* Add 2D polygon messages and services. +* Add ROS and Eigen conversions. +* Add standard services and trajectory messages. +* Add mav_planning_msgs +* Contributors: Helen Oleynikova, Rik Bähnemann diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/CMakeLists.txt b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/CMakeLists.txt new file mode 100644 index 00000000..0a23a65c --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/CMakeLists.txt @@ -0,0 +1,44 @@ +cmake_minimum_required(VERSION 3.14.4...3.27) +project(mav_planning_msgs) + +if(${CMAKE_VERSION} VERSION_GREATER 3.27) + cmake_policy(SET CMP0148 OLD) +endif() + +find_package(ament_cmake REQUIRED) + +find_package(Eigen3 REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +find_package(builtin_interfaces REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(mav_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(trajectory_msgs REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + msg/Point2D.msg + msg/PointCloudWithPose.msg + msg/Polygon2D.msg + msg/PolygonWithHoles.msg + msg/PolygonWithHolesStamped.msg + msg/PolynomialSegment.msg + msg/PolynomialSegment4D.msg + msg/PolynomialTrajectory.msg + msg/PolynomialTrajectory4D.msg + srv/PlannerService.srv + srv/PolygonService.srv + srv/ChangeNameService.srv + DEPENDENCIES builtin_interfaces geometry_msgs mav_msgs sensor_msgs std_msgs trajectory_msgs +) + +ament_export_dependencies(rosidl_default_runtime) + +install( + DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} + FILES_MATCHING PATTERN "*.hpp" +) + +ament_package() diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/include/mav_planning_msgs/conversions.hpp b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/include/mav_planning_msgs/conversions.hpp new file mode 100644 index 00000000..820b2524 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/include/mav_planning_msgs/conversions.hpp @@ -0,0 +1,117 @@ +/* + * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland + * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland + * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef MAV_PLANNING_MSGS_CONVERSIONS_H +#define MAV_PLANNING_MSGS_CONVERSIONS_H + +#include +#include +#include + +#include "mav_planning_msgs/eigen_planning_msgs.hpp" +#include "mav_planning_msgs/msg/polynomial_segment.hpp" +#include "mav_planning_msgs/msg/polynomial_trajectory.hpp" + +// deprecated +#include "mav_planning_msgs/conversions_deprecated.hpp" + +namespace mav_planning_msgs { + +/// Converts a PolynomialSegment double array to an Eigen::VectorXd. +inline void vectorFromMsgArray(const msg::PolynomialSegment::_x_type& array, + Eigen::VectorXd* x) { + *x = Eigen::Map(&(array[0]), array.size()); +} + +/// Converts an Eigen::VectorXd to a PolynomialSegment double array. +inline void msgArrayFromVector(const Eigen::VectorXd& x, + msg::PolynomialSegment::_x_type* array) { + array->resize(x.size()); + Eigen::Map map = + Eigen::Map(&((*array)[0]), array->size()); + map = x; +} + +/// Converts a PolynomialSegment message to an EigenPolynomialSegment structure. +inline void eigenPolynomialSegmentFromMsg(const msg::PolynomialSegment& msg, + EigenPolynomialSegment* segment) { + assert(segment != NULL); + + vectorFromMsgArray(msg.x, &(segment->x)); + vectorFromMsgArray(msg.y, &(segment->y)); + vectorFromMsgArray(msg.z, &(segment->z)); + vectorFromMsgArray(msg.yaw, &(segment->yaw)); + vectorFromMsgArray(msg.rx, &(segment->rx)); + vectorFromMsgArray(msg.ry, &(segment->ry)); + vectorFromMsgArray(msg.rz, &(segment->rz)); + + segment->segment_time_ns = msg.segment_time.toNSec(); + segment->num_coeffs = msg.num_coeffs; +} + +/// Converts a PolynomialTrajectory message to a EigenPolynomialTrajectory +inline void eigenPolynomialTrajectoryFromMsg( + const msg::PolynomialTrajectory& msg, + EigenPolynomialTrajectory* eigen_trajectory) { + assert(eigen_trajectory != NULL); + eigen_trajectory->clear(); + eigen_trajectory->reserve(msg.segments.size()); + for (msg::PolynomialTrajectory::_segments_type::const_iterator it = + msg.segments.begin(); + it != msg.segments.end(); ++it) { + EigenPolynomialSegment segment; + eigenPolynomialSegmentFromMsg(*it, &segment); + eigen_trajectory->push_back(segment); + } +} + +/// Converts an EigenPolynomialSegment to a PolynomialSegment message. Does NOT +/// set the header! +inline void polynomialSegmentMsgFromEigen(const EigenPolynomialSegment& segment, + msg::PolynomialSegment* msg) { + assert(msg != NULL); + msgArrayFromVector(segment.x, &(msg->x)); + msgArrayFromVector(segment.y, &(msg->y)); + msgArrayFromVector(segment.z, &(msg->z)); + msgArrayFromVector(segment.yaw, &(msg->yaw)); + msgArrayFromVector(segment.rx, &(msg->rx)); + msgArrayFromVector(segment.ry, &(msg->ry)); + msgArrayFromVector(segment.rz, &(msg->rz)); + + msg->segment_time.fromNSec(segment.segment_time_ns); + msg->num_coeffs = segment.num_coeffs; +} + +/// Converts an EigenPolynomialTrajectory to a PolynomialTrajectory message. +/// Does NOT set the header! +inline void polynomialTrajectoryMsgFromEigen( + const EigenPolynomialTrajectory& eigen_trajectory, + msg::PolynomialTrajectory* msg) { + assert(msg != NULL); + msg->segments.reserve(eigen_trajectory.size()); + for (EigenPolynomialTrajectory::const_iterator it = eigen_trajectory.begin(); + it != eigen_trajectory.end(); ++it) { + msg::PolynomialSegment segment; + polynomialSegmentMsgFromEigen(*it, &segment); + msg->segments.push_back(segment); + } +} + +} // namespace mav_planning_msgs + +#endif // MAV_PLANNING_MSGS_CONVERSIONS_H diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/include/mav_planning_msgs/conversions_deprecated.hpp b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/include/mav_planning_msgs/conversions_deprecated.hpp new file mode 100644 index 00000000..ff6f5d9d --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/include/mav_planning_msgs/conversions_deprecated.hpp @@ -0,0 +1,101 @@ +/* + * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland + * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland + * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef MAV_PLANNING_MSGS_CONVERSIONS_DEPRECATED_H +#define MAV_PLANNING_MSGS_CONVERSIONS_DEPRECATED_H + +#include +#include +#include + +#include "mav_planning_msgs/eigen_planning_msgs.hpp" +#include "mav_planning_msgs/msg/polynomial_segment4_d.hpp" +#include "mav_planning_msgs/msg/polynomial_trajectory4_d.hpp" + +namespace mav_planning_msgs { + +/// Converts a PolynomialSegment double array to an Eigen::VectorXd. +inline void vectorFromMsgArray(const msg::PolynomialSegment4D::_x_type& array, + Eigen::VectorXd* x); + +/// Converts an Eigen::VectorXd to a PolynomialSegment double array. +inline void msgArrayFromVector(const Eigen::VectorXd& x, + msg::PolynomialSegment4D::_x_type* array); + +/// Converts a PolynomialSegment message to an EigenPolynomialSegment structure. +inline void eigenPolynomialSegmentFromMsg(const msg::PolynomialSegment4D& msg, + EigenPolynomialSegment* segment) { + assert(segment != NULL); + + vectorFromMsgArray(msg.x, &(segment->x)); + vectorFromMsgArray(msg.y, &(segment->y)); + vectorFromMsgArray(msg.z, &(segment->z)); + vectorFromMsgArray(msg.yaw, &(segment->yaw)); + + segment->segment_time_ns = msg.segment_time.toNSec(); + segment->num_coeffs = msg.num_coeffs; +} + +/// Converts a PolynomialTrajectory message to a EigenPolynomialTrajectory +inline void eigenPolynomialTrajectoryFromMsg( + const msg::PolynomialTrajectory4D& msg, + EigenPolynomialTrajectory* eigen_trajectory) { + assert(eigen_trajectory != NULL); + eigen_trajectory->clear(); + eigen_trajectory->reserve(msg.segments.size()); + for (msg::PolynomialTrajectory4D::_segments_type::const_iterator it = + msg.segments.begin(); + it != msg.segments.end(); ++it) { + EigenPolynomialSegment segment; + eigenPolynomialSegmentFromMsg(*it, &segment); + eigen_trajectory->push_back(segment); + } +} + +/// Converts an EigenPolynomialSegment to a PolynomialSegment message. Does NOT +/// set the header! +inline void polynomialSegmentMsgFromEigen(const EigenPolynomialSegment& segment, + msg::PolynomialSegment4D* msg) { + assert(msg != NULL); + msgArrayFromVector(segment.x, &(msg->x)); + msgArrayFromVector(segment.y, &(msg->y)); + msgArrayFromVector(segment.z, &(msg->z)); + msgArrayFromVector(segment.yaw, &(msg->yaw)); + + msg->segment_time.fromNSec(segment.segment_time_ns); + msg->num_coeffs = segment.num_coeffs; +} + +/// Converts an EigenPolynomialTrajectory to a PolynomialTrajectory message. +/// Does NOT set the header! +inline void polynomialTrajectoryMsgFromEigen( + const EigenPolynomialTrajectory& eigen_trajectory, + msg::PolynomialTrajectory4D* msg) { + assert(msg != NULL); + msg->segments.reserve(eigen_trajectory.size()); + for (EigenPolynomialTrajectory::const_iterator it = eigen_trajectory.begin(); + it != eigen_trajectory.end(); ++it) { + msg::PolynomialSegment4D segment; + polynomialSegmentMsgFromEigen(*it, &segment); + msg->segments.push_back(segment); + } +} + +} // namespace mav_planning_msgs + +#endif // MAV_PLANNING_MSGS_CONVERSIONS_DEPRECATED_H diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/include/mav_planning_msgs/eigen_planning_msgs.hpp b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/include/mav_planning_msgs/eigen_planning_msgs.hpp new file mode 100644 index 00000000..8d6e64bc --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/include/mav_planning_msgs/eigen_planning_msgs.hpp @@ -0,0 +1,45 @@ +/* + * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland + * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland + * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef MAV_PLANNING_MSGS_EIGEN_MAV_MSGS_H +#define MAV_PLANNING_MSGS_EIGEN_MAV_MSGS_H + +#include +#include + +namespace mav_planning_msgs { + +struct EigenPolynomialSegment { + EigenPolynomialSegment() : segment_time_ns(0), num_coeffs(0){}; + + Eigen::VectorXd x; + Eigen::VectorXd y; + Eigen::VectorXd z; + Eigen::VectorXd yaw; + Eigen::VectorXd rx; + Eigen::VectorXd ry; + Eigen::VectorXd rz; + uint64_t segment_time_ns; + int num_coeffs; +}; + +typedef std::vector EigenPolynomialTrajectory; + +} // namespace mav_planning_msgs + +#endif // MAV_PLANNING_MSGS_EIGEN_MAV_MSGS_H diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/msg/Point2D.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/msg/Point2D.msg new file mode 100644 index 00000000..9075de78 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/msg/Point2D.msg @@ -0,0 +1,3 @@ +# This contains the position of a 2D point. +float64 x +float64 y diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/msg/PointCloudWithPose.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/msg/PointCloudWithPose.msg new file mode 100644 index 00000000..9189d07a --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/msg/PointCloudWithPose.msg @@ -0,0 +1,3 @@ +std_msgs/Header header +geometry_msgs/TransformStamped sensor_pose +sensor_msgs/PointCloud2 cloud_in_sensor_frame \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/msg/Polygon2D.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/msg/Polygon2D.msg new file mode 100644 index 00000000..9c11ac19 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/msg/Polygon2D.msg @@ -0,0 +1,2 @@ +# A specification of a 2D polygon where the first and last points are assumed to be connected. +mav_planning_msgs/Point2D[] points diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/msg/PolygonWithHoles.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/msg/PolygonWithHoles.msg new file mode 100644 index 00000000..ecb65ac3 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/msg/PolygonWithHoles.msg @@ -0,0 +1,3 @@ +# A message to define a 2D polygon with holes. +mav_planning_msgs/Polygon2D hull +mav_planning_msgs/Polygon2D[] holes diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/msg/PolygonWithHolesStamped.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/msg/PolygonWithHolesStamped.msg new file mode 100644 index 00000000..8a00fc4e --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/msg/PolygonWithHolesStamped.msg @@ -0,0 +1,4 @@ +# A message to define a 2D polygon with holes, stamp, and altitude above ground. +std_msgs/Header header +float64 altitude +mav_planning_msgs/PolygonWithHoles polygon diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/msg/PolynomialSegment.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/msg/PolynomialSegment.msg new file mode 100644 index 00000000..347b7d4c --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/msg/PolynomialSegment.msg @@ -0,0 +1,12 @@ +std_msgs/Header header +int32 num_coeffs # order of the polynomial + 1, should match size of x[] +builtin_interfaces/Duration segment_time # duration of the segment +float64[] x # coefficients for the x-axis, INCREASING order +float64[] y # coefficients for the y-axis, INCREASING order +float64[] z # coefficients for the z-axis, INCREASING order +float64[] rx # coefficients for the rotation x-vector, INCREASING order +float64[] ry # coefficients for the rotation y-vector, INCREASING order +float64[] rz # coefficients for the rotation z-vector, INCREASING order +## For backwards compatibility with underactuated (4DOF) commands): +float64[] yaw # coefficients for the yaw, INCREASING order + diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/msg/PolynomialSegment4D.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/msg/PolynomialSegment4D.msg new file mode 100644 index 00000000..796bad2e --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/msg/PolynomialSegment4D.msg @@ -0,0 +1,7 @@ +std_msgs/Header header +int32 num_coeffs # order of the polynomial + 1, should match size of x[] +builtin_interfaces/Duration segment_time # duration of the segment +float64[] x # coefficients for the x-axis, INCREASING order +float64[] y # coefficients for the y-axis, INCREASING order +float64[] z # coefficients for the z-axis, INCREASING order +float64[] yaw # coefficients for the yaw, INCREASING order diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/msg/PolynomialTrajectory.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/msg/PolynomialTrajectory.msg new file mode 100644 index 00000000..6d6f03f3 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/msg/PolynomialTrajectory.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +PolynomialSegment[] segments diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/msg/PolynomialTrajectory4D.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/msg/PolynomialTrajectory4D.msg new file mode 100644 index 00000000..ba046802 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/msg/PolynomialTrajectory4D.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +PolynomialSegment4D[] segments diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/package.xml b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/package.xml new file mode 100644 index 00000000..2c477271 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/package.xml @@ -0,0 +1,47 @@ + + + + mav_planning_msgs + 3.3.3 + + Messages specific to MAV planning, especially polynomial planning. + + + Helen Oleynikova + + Simon Lynen + Markus Achtelik + Pascal Gohl + Sammy Omari + Michael Burri + Fadri Furrer + Helen Oleynikova + Karen Bodie + Rik Bähnemann + + ASL 2.0 + + https://github.com/ethz-asl/mav_comm + https://github.com/ethz-asl/mav_comm/issues + + ament_cmake + rosidl_default_generators + + eigen + rclcpp + + builtin_interfaces + geometry_msgs + mav_msgs + sensor_msgs + std_msgs + trajectory_msgs + + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/srv/ChangeNameService.srv b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/srv/ChangeNameService.srv new file mode 100644 index 00000000..bbdc5615 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/srv/ChangeNameService.srv @@ -0,0 +1,6 @@ +#request fields +string name +--- +# True on success, false on failure +bool success +string message diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/srv/PlannerService.srv b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/srv/PlannerService.srv new file mode 100644 index 00000000..903820ca --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/srv/PlannerService.srv @@ -0,0 +1,16 @@ +#request fields +geometry_msgs/PoseStamped start_pose #start pose for the planner +geometry_msgs/Vector3 start_velocity +geometry_msgs/PoseStamped goal_pose #start pose for the planner +geometry_msgs/Vector3 goal_velocity +geometry_msgs/Vector3 bounding_box +--- +# True on success, false on planning failure +bool success +# Either contains a polynomial trajectory: +mav_planning_msgs/PolynomialTrajectory polynomial_plan +mav_planning_msgs/PolynomialTrajectory4D polynomial_plan_4d +# or a MultiDOFJointTrajectory containing a sampled path (or straight-line +# waypoints, depending on the planner). +# Only one of these should be non-empty. +trajectory_msgs/MultiDOFJointTrajectory sampled_plan diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/srv/PolygonService.srv b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/srv/PolygonService.srv new file mode 100644 index 00000000..ed68217d --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_planning_msgs/srv/PolygonService.srv @@ -0,0 +1,6 @@ +# A service to set a new polygon with holes. +# Request fields: +mav_planning_msgs/PolygonWithHolesStamped polygon # The new polygon. +--- +# Response fields: +bool success # True on success, false on polygon error. diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_state_machine_msgs/CMakeLists.txt b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_state_machine_msgs/CMakeLists.txt new file mode 100644 index 00000000..53c520f7 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_state_machine_msgs/CMakeLists.txt @@ -0,0 +1,20 @@ +cmake_minimum_required(VERSION 3.14.4...3.27) +project(mav_state_machine_msgs) + +if(${CMAKE_VERSION} VERSION_GREATER 3.27) + cmake_policy(SET CMP0148 OLD) +endif() + +find_package(ament_cmake REQUIRED) + +find_package(std_msgs REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + msg/StartStopTask.msg + srv/RunTaskService.srv + DEPENDENCIES std_msgs +) + +ament_export_dependencies(rosidl_default_runtime) + +ament_package() diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_state_machine_msgs/msg/StartStopTask.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_state_machine_msgs/msg/StartStopTask.msg new file mode 100644 index 00000000..47084b0b --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_state_machine_msgs/msg/StartStopTask.msg @@ -0,0 +1,4 @@ +std_msgs/Header header + +string task_name +bool start \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_state_machine_msgs/package.xml b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_state_machine_msgs/package.xml new file mode 100644 index 00000000..099abe78 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_state_machine_msgs/package.xml @@ -0,0 +1,26 @@ + + + + mav_state_machine_msgs + 0.0.0 + + Messages specific to MAV state machine. + + + Christian Lanegger + + TBD + + ament_cmake + rosidl_default_generators + + std_msgs + + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_state_machine_msgs/srv/RunTaskService.srv b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_state_machine_msgs/srv/RunTaskService.srv new file mode 100644 index 00000000..a2c083f5 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_state_machine_msgs/srv/RunTaskService.srv @@ -0,0 +1,8 @@ +#request fields +# Name of task +string task_name +# True to start task, False to stop/abort +bool start +--- +# True on success, false on failure to start task +bool success diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_system_msgs/CMakeLists.txt b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_system_msgs/CMakeLists.txt new file mode 100644 index 00000000..5c5b4b51 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_system_msgs/CMakeLists.txt @@ -0,0 +1,18 @@ +cmake_minimum_required(VERSION 3.14.4...3.27) +project(mav_system_msgs) + +if(${CMAKE_VERSION} VERSION_GREATER 3.27) + cmake_policy(SET CMP0148 OLD) +endif() + +find_package(std_msgs REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + msg/CpuInfo.msg + msg/ProcessInfo.msg + DEPENDENCIES std_msgs +) + +ament_export_dependencies(rosidl_default_runtime) + +ament_package() diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_system_msgs/msg/CpuInfo.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_system_msgs/msg/CpuInfo.msg new file mode 100644 index 00000000..be6b3119 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_system_msgs/msg/CpuInfo.msg @@ -0,0 +1,7 @@ +# Miscellaneous information about CPU status. +# Written by Marco Tranzatto (see: https://github.com/ethz-asl/mav_eurathlon/blob/master/mav_eurathlon_msgs/msg/CpuInfo.msg) + +std_msgs/Header header + +float32[] cpu_percent # Current system-wide CPU utilization as a percentage, per CPU. +ProcessInfo[] heaviest_processes # Info about heaviest running processes. \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_system_msgs/msg/ProcessInfo.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_system_msgs/msg/ProcessInfo.msg new file mode 100644 index 00000000..0da2f2f0 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_system_msgs/msg/ProcessInfo.msg @@ -0,0 +1,7 @@ +# Miscellaneous information about process status. +# Written by Marco Tranzatto (see: https://github.com/ethz-asl/mav_eurathlon/blob/master/mav_eurathlon_msgs/msg/ProcessInfo.msg) + +uint32 pid # Process PID +string name # Process name +string username # Name of the user that owns the process +float32 cpu_percent # Process CPU utilization as a percentage diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_system_msgs/package.xml b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_system_msgs/package.xml new file mode 100644 index 00000000..a5574879 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/mav_system_msgs/package.xml @@ -0,0 +1,26 @@ + + + + mav_system_msgs + 0.0.0 + + Messages specific to MAV utils scripts. + + + Christian Lanegger + + TBD + + ament_cmake + rosidl_default_generators + + std_msgs + + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/CMakeLists.txt b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/CMakeLists.txt new file mode 100644 index 00000000..0fc6edc2 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/CMakeLists.txt @@ -0,0 +1,37 @@ +cmake_minimum_required(VERSION 3.5) + +project(px4_msgs) + +list(INSERT CMAKE_MODULE_PATH 0 "${CMAKE_CURRENT_SOURCE_DIR}/cmake") + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra) +endif() + +find_package(ament_cmake REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +# ############################################################################## +# Generate ROS messages, ROS2 interfaces and IDL files # +# ############################################################################## + +# get all msg files +set(MSGS_DIR "${CMAKE_CURRENT_SOURCE_DIR}/msg") +file(GLOB PX4_MSGS RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}" "${MSGS_DIR}/*.msg") + +# get all srv files +set(SRVS_DIR "${CMAKE_CURRENT_SOURCE_DIR}/srv") +file(GLOB PX4_SRVS RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}" "${SRVS_DIR}/*.srv") + +# Generate introspection typesupport for C and C++ and IDL files +rosidl_generate_interfaces(${PROJECT_NAME} + ${PX4_MSGS} + ${PX4_SRVS} + DEPENDENCIES builtin_interfaces + ADD_LINTER_TESTS +) + +ament_export_dependencies(rosidl_default_runtime) + +ament_package() diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/CONTRIBUTING/index.html b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/CONTRIBUTING/index.html new file mode 100644 index 00000000..f667f8d2 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/CONTRIBUTING/index.html @@ -0,0 +1,2606 @@ + + + + + + + + + + + + + + + + + + + Contributing - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

Contributing

+

Do not commit changes directly to this repository that change the message definitions. All the message definitions are directly generated from the uORB msg definitions on the PX4 Firmware repository. Any fixes or improvements one finds suitable to apply to the message definitions should be directly done on the uORB message files. The deployment of these are taken care by a Jenkins CI/CD stage.

+

Contributing to the PX4 Firmware repository (or to this repository, not including message definitions)

+

Follow the Contributing guide from the PX4 Firmware repo.

+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ +
+ + + +
+
+
+ +
+ Copyright © 2024 - 2030 AirLab CMU +
+ + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + +
+ +
+
+
+ +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/LICENSE b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/LICENSE new file mode 100644 index 00000000..31f2eb48 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/LICENSE @@ -0,0 +1,29 @@ +BSD 3-Clause License + +Copyright (c) 2019, PX4 Development Team +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/index.html b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/index.html new file mode 100644 index 00000000..321557b4 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/index.html @@ -0,0 +1,2680 @@ + + + + + + + + + + + + + + + + + + + px4_msgs - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

px4_msgs

+

GitHub license Build package

+

Discord Shield

+

ROS 2 message definitions for the PX4 Autopilot project.

+

Building this package generates all the required interfaces to interface ROS 2 nodes with the PX4 internals.

+

Supported versions and compatibility

+

Depending on the PX4 and ROS versions you want to use, you need to checkout the appropriate branch of this package:

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
PX4ROS 2Ubuntubranch
v1.13FoxyUbuntu 20.04release/1.13
v1.14FoxyUbuntu 20.04release/1.14
v1.14HumbleUbuntu 22.04release/1.14
v1.14RollingUbuntu 22.04release/1.14
mainFoxyUbuntu 22.04main
mainHumbleUbuntu 22.04main
mainRollingUbuntu 22.04main
+

Messages Sync from PX4

+

When PX4 message definitions in the main branch of PX4 Autopilot change, a CI/CD pipeline automatically copies and pushes updated ROS message definitions to this repository. This ensures that this repository main branch and the PX4-Autopilot main branch are always up to date. +However, if you are using a custom PX4 version and you modified existing messages or created new one, then you have to manually synchronize them in this repository:

+

Manual Message Sync

+
    +
  • Checkout the correct branch associated to the PX4 version from which you detached you custom version.
    • +
  • Delete all *.msg files in msg/ and copy all *.msg files from PX4-Autopilot/msg/ in it. Assuming that this repository and the PX4-Autopilot repository are placed in your home folder, you can run: + 
    rm -f ~/px4_msgs/msg/*.msg
+cp ~/PX4-Autopilot/msg/*.msg ~/px4_msgs/msg/
+
    • +
+

Install, build and usage

+

Check Using colcon to build packages to understand how this can be built inside a workspace. Check the PX4 ROS 2 User Guide section on the PX4 documentation for further details on how this integrates PX4 and how to exchange messages with the autopilot.

+

Bug tracking and feature requests

+

Use the Issues section to create a new issue. Report your issue or feature request here.

+

Questions and troubleshooting

+

Reach the PX4 development team on the PX4 Discord Server.

+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ +
+ + + +
+
+
+ +
+ Copyright © 2024 - 2030 AirLab CMU +
+ + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + +
+ +
+
+
+ +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ActionRequest.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ActionRequest.msg new file mode 100644 index 00000000..888814e0 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ActionRequest.msg @@ -0,0 +1,19 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 action # what action is requested +uint8 ACTION_DISARM = 0 +uint8 ACTION_ARM = 1 +uint8 ACTION_TOGGLE_ARMING = 2 +uint8 ACTION_UNKILL = 3 +uint8 ACTION_KILL = 4 +uint8 ACTION_SWITCH_MODE = 5 +uint8 ACTION_VTOL_TRANSITION_TO_MULTICOPTER = 6 +uint8 ACTION_VTOL_TRANSITION_TO_FIXEDWING = 7 + +uint8 source # how the request was triggered +uint8 SOURCE_RC_STICK_GESTURE = 0 +uint8 SOURCE_RC_SWITCH = 1 +uint8 SOURCE_RC_BUTTON = 2 +uint8 SOURCE_RC_MODE_SLOT = 3 + +uint8 mode # for ACTION_SWITCH_MODE what mode is requested according to vehicle_status_s::NAVIGATION_STATE_* diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ActuatorArmed.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ActuatorArmed.msg new file mode 100644 index 00000000..6867adf2 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ActuatorArmed.msg @@ -0,0 +1,9 @@ +uint64 timestamp # time since system start (microseconds) + +bool armed # Set to true if system is armed +bool prearmed # Set to true if the actuator safety is disabled but motors are not armed +bool ready_to_arm # Set to true if system is ready to be armed +bool lockdown # Set to true if actuators are forced to being disabled (due to emergency or HIL) +bool manual_lockdown # Set to true if manual throttle kill switch is engaged +bool force_failsafe # Set to true if the actuators are forced to the failsafe position +bool in_esc_calibration_mode # IO/FMU should ignore messages from the actuator controls topics diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ActuatorControlsStatus.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ActuatorControlsStatus.msg new file mode 100644 index 00000000..c89f669e --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ActuatorControlsStatus.msg @@ -0,0 +1,5 @@ +uint64 timestamp # time since system start (microseconds) + +float32[3] control_power + +# TOPICS actuator_controls_status_0 actuator_controls_status_1 diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ActuatorMotors.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ActuatorMotors.msg new file mode 100644 index 00000000..e74566f1 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ActuatorMotors.msg @@ -0,0 +1,12 @@ +# Motor control message +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp the data this control response is based on was sampled + +uint16 reversible_flags # bitset which motors are configured to be reversible + +uint8 ACTUATOR_FUNCTION_MOTOR1 = 101 + +uint8 NUM_CONTROLS = 12 +float32[12] control # range: [-1, 1], where 1 means maximum positive thrust, + # -1 maximum negative (if not supported by the output, <0 maps to NaN), + # and NaN maps to disarmed (stop the motors) diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ActuatorOutputs.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ActuatorOutputs.msg new file mode 100644 index 00000000..3209e54e --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ActuatorOutputs.msg @@ -0,0 +1,8 @@ +uint64 timestamp # time since system start (microseconds) +uint8 NUM_ACTUATOR_OUTPUTS = 16 +uint8 NUM_ACTUATOR_OUTPUT_GROUPS = 4 # for sanity checking +uint32 noutputs # valid outputs +float32[16] output # output data, in natural output units + +# actuator_outputs_sim is used for SITL, HITL & SIH (with an output range of [-1, 1]) +# TOPICS actuator_outputs actuator_outputs_sim actuator_outputs_debug diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ActuatorServos.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ActuatorServos.msg new file mode 100644 index 00000000..2c7900e8 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ActuatorServos.msg @@ -0,0 +1,8 @@ +# Servo control message +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp the data this control response is based on was sampled + +uint8 NUM_CONTROLS = 8 +float32[8] control # range: [-1, 1], where 1 means maximum positive position, + # -1 maximum negative, + # and NaN maps to disarmed diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ActuatorServosTrim.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ActuatorServosTrim.msg new file mode 100644 index 00000000..30953e7a --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ActuatorServosTrim.msg @@ -0,0 +1,5 @@ +# Servo trims, added as offset to servo outputs +uint64 timestamp # time since system start (microseconds) + +uint8 NUM_CONTROLS = 8 +float32[8] trim # range: [-1, 1] diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ActuatorTest.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ActuatorTest.msg new file mode 100644 index 00000000..221258f9 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ActuatorTest.msg @@ -0,0 +1,21 @@ +uint64 timestamp # time since system start (microseconds) + +# Topic to test individual actuator output functions + +uint8 ACTION_RELEASE_CONTROL = 0 # exit test mode for the given function +uint8 ACTION_DO_CONTROL = 1 # enable actuator test mode + +uint8 FUNCTION_MOTOR1 = 101 +uint8 MAX_NUM_MOTORS = 12 +uint8 FUNCTION_SERVO1 = 201 +uint8 MAX_NUM_SERVOS = 8 + +uint8 action # one of ACTION_* +uint16 function # actuator output function +float32 value # range: [-1, 1], where 1 means maximum positive output, + # 0 to center servos or minimum motor thrust, + # -1 maximum negative (if not supported by the motors, <0 maps to NaN), + # and NaN maps to disarmed (stop the motors) +uint32 timeout_ms # timeout in ms after which to exit test mode (if 0, do not time out) + +uint8 ORB_QUEUE_LENGTH = 16 # >= MAX_NUM_MOTORS to support code in esc_calibration diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/AdcReport.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/AdcReport.msg new file mode 100644 index 00000000..1ae72b6d --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/AdcReport.msg @@ -0,0 +1,6 @@ +uint64 timestamp # time since system start (microseconds) +uint32 device_id # unique device ID for the sensor that does not change between power cycles +int16[12] channel_id # ADC channel IDs, negative for non-existent, TODO: should be kept same as array index +int32[12] raw_data # ADC channel raw value, accept negative value, valid if channel ID is positive +uint32 resolution # ADC channel resolution +float32 v_ref # ADC channel voltage reference, use to calculate LSB voltage(lsb=scale/resolution) diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/Airspeed.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/Airspeed.msg new file mode 100644 index 00000000..aaed7b72 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/Airspeed.msg @@ -0,0 +1,10 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample + +float32 indicated_airspeed_m_s # indicated airspeed in m/s + +float32 true_airspeed_m_s # true filtered airspeed in m/s + +float32 air_temperature_celsius # air temperature in degrees Celsius, -1000 if unknown + +float32 confidence # confidence value from 0 to 1 for this sensor diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/AirspeedValidated.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/AirspeedValidated.msg new file mode 100644 index 00000000..06731cc4 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/AirspeedValidated.msg @@ -0,0 +1,12 @@ +uint64 timestamp # time since system start (microseconds) + +float32 indicated_airspeed_m_s # indicated airspeed in m/s (IAS), set to NAN if invalid +float32 calibrated_airspeed_m_s # calibrated airspeed in m/s (CAS, accounts for instrumentation errors), set to NAN if invalid +float32 true_airspeed_m_s # true filtered airspeed in m/s (TAS), set to NAN if invalid + +float32 calibrated_ground_minus_wind_m_s # CAS calculated from groundspeed - windspeed, where windspeed is estimated based on a zero-sideslip assumption, set to NAN if invalid +float32 true_ground_minus_wind_m_s # TAS calculated from groundspeed - windspeed, where windspeed is estimated based on a zero-sideslip assumption, set to NAN if invalid + +bool airspeed_sensor_measurement_valid # True if data from at least one airspeed sensor is declared valid. + +int8 selected_airspeed_index # 1-3: airspeed sensor index, 0: groundspeed-windspeed, -1: airspeed invalid diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/AirspeedWind.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/AirspeedWind.msg new file mode 100644 index 00000000..6ca513a3 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/AirspeedWind.msg @@ -0,0 +1,26 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +float32 windspeed_north # Wind component in north / X direction (m/sec) +float32 windspeed_east # Wind component in east / Y direction (m/sec) + +float32 variance_north # Wind estimate error variance in north / X direction (m/sec)**2 - set to zero (no uncertainty) if not estimated +float32 variance_east # Wind estimate error variance in east / Y direction (m/sec)**2 - set to zero (no uncertainty) if not estimated + +float32 tas_innov # True airspeed innovation +float32 tas_innov_var # True airspeed innovation variance + +float32 tas_scale_raw # Estimated true airspeed scale factor (not validated) +float32 tas_scale_raw_var # True airspeed scale factor variance + +float32 tas_scale_validated # Estimated true airspeed scale factor after validation + +float32 beta_innov # Sideslip measurement innovation +float32 beta_innov_var # Sideslip measurement innovation variance + +uint8 source # source of wind estimate + +uint8 SOURCE_AS_BETA_ONLY = 0 # wind estimate only based on synthetic sideslip fusion +uint8 SOURCE_AS_SENSOR_1 = 1 # combined synthetic sideslip and airspeed fusion (data from first airspeed sensor) +uint8 SOURCE_AS_SENSOR_2 = 2 # combined synthetic sideslip and airspeed fusion (data from second airspeed sensor) +uint8 SOURCE_AS_SENSOR_3 = 3 # combined synthetic sideslip and airspeed fusion (data from third airspeed sensor) diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ArmingCheckReply.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ArmingCheckReply.msg new file mode 100644 index 00000000..589ad1b1 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ArmingCheckReply.msg @@ -0,0 +1,33 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 request_id +uint8 registration_id + +uint8 HEALTH_COMPONENT_INDEX_NONE = 0 +uint8 HEALTH_COMPONENT_INDEX_AVOIDANCE = 19 + +uint8 health_component_index # HEALTH_COMPONENT_INDEX_* +bool health_component_is_present +bool health_component_warning +bool health_component_error + +bool can_arm_and_run # whether arming is possible, and if it's a navigation mode, if it can run + +uint8 num_events + +Event[5] events + +# Mode requirements +bool mode_req_angular_velocity +bool mode_req_attitude +bool mode_req_local_alt +bool mode_req_local_position +bool mode_req_local_position_relaxed +bool mode_req_global_position +bool mode_req_mission +bool mode_req_home_position +bool mode_req_prevent_arming +bool mode_req_manual_control + + +uint8 ORB_QUEUE_LENGTH = 4 diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ArmingCheckRequest.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ArmingCheckRequest.msg new file mode 100644 index 00000000..69e7e85f --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ArmingCheckRequest.msg @@ -0,0 +1,5 @@ +uint64 timestamp # time since system start (microseconds) + +# broadcast message to request all registered arming checks to be reported + +uint8 request_id diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/AutotuneAttitudeControlStatus.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/AutotuneAttitudeControlStatus.msg new file mode 100644 index 00000000..021a8c34 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/AutotuneAttitudeControlStatus.msg @@ -0,0 +1,35 @@ +uint64 timestamp # time since system start (microseconds) + +float32[5] coeff # coefficients of the identified discrete-time model +float32[5] coeff_var # coefficients' variance of the identified discrete-time model +float32 fitness # fitness of the parameter estimate +float32 innov +float32 dt_model + +float32 kc +float32 ki +float32 kd +float32 kff +float32 att_p + +float32[3] rate_sp + +float32 u_filt +float32 y_filt + +uint8 STATE_IDLE = 0 +uint8 STATE_INIT = 1 +uint8 STATE_ROLL = 2 +uint8 STATE_ROLL_PAUSE = 3 +uint8 STATE_PITCH = 4 +uint8 STATE_PITCH_PAUSE = 5 +uint8 STATE_YAW = 6 +uint8 STATE_YAW_PAUSE = 7 +uint8 STATE_VERIFICATION = 8 +uint8 STATE_APPLY = 9 +uint8 STATE_TEST = 10 +uint8 STATE_COMPLETE = 11 +uint8 STATE_FAIL = 12 +uint8 STATE_WAIT_FOR_DISARM = 13 + +uint8 state diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/BatteryStatus.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/BatteryStatus.msg new file mode 100644 index 00000000..66fcffa0 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/BatteryStatus.msg @@ -0,0 +1,78 @@ +uint64 timestamp # time since system start (microseconds) +bool connected # Whether or not a battery is connected, based on a voltage threshold +float32 voltage_v # Battery voltage in volts, 0 if unknown +float32 voltage_filtered_v # Battery voltage in volts, filtered, 0 if unknown +float32 current_a # Battery current in amperes, -1 if unknown +float32 current_filtered_a # Battery current in amperes, filtered, 0 if unknown +float32 current_average_a # Battery current average in amperes (for FW average in level flight), -1 if unknown +float32 discharged_mah # Discharged amount in mAh, -1 if unknown +float32 remaining # From 1 to 0, -1 if unknown +float32 scale # Power scaling factor, >= 1, or -1 if unknown +float32 time_remaining_s # predicted time in seconds remaining until battery is empty under previous averaged load, NAN if unknown +float32 temperature # temperature of the battery. NaN if unknown +uint8 cell_count # Number of cells, 0 if unknown + +uint8 BATTERY_SOURCE_POWER_MODULE = 0 +uint8 BATTERY_SOURCE_EXTERNAL = 1 +uint8 BATTERY_SOURCE_ESCS = 2 +uint8 source # Battery source +uint8 priority # Zero based priority is the connection on the Power Controller V1..Vn AKA BrickN-1 +uint16 capacity # actual capacity of the battery +uint16 cycle_count # number of discharge cycles the battery has experienced +uint16 average_time_to_empty # predicted remaining battery capacity based on the average rate of discharge in min +uint16 serial_number # serial number of the battery pack +uint16 manufacture_date # manufacture date, part of serial number of the battery pack. Formatted as: Day + Month×32 + (Year–1980)×512 +uint16 state_of_health # state of health. FullChargeCapacity/DesignCapacity, 0-100%. +uint16 max_error # max error, expected margin of error in % in the state-of-charge calculation with a range of 1 to 100% +uint8 id # ID number of a battery. Should be unique and consistent for the lifetime of a vehicle. 1-indexed. +uint16 interface_error # interface error counter + +float32[14] voltage_cell_v # Battery individual cell voltages, 0 if unknown +float32 max_cell_voltage_delta # Max difference between individual cell voltages + +bool is_powering_off # Power off event imminent indication, false if unknown +bool is_required # Set if the battery is explicitly required before arming + + +uint8 BATTERY_WARNING_NONE = 0 # no battery low voltage warning active +uint8 BATTERY_WARNING_LOW = 1 # warning of low voltage +uint8 BATTERY_WARNING_CRITICAL = 2 # critical voltage, return / abort immediately +uint8 BATTERY_WARNING_EMERGENCY = 3 # immediate landing required +uint8 BATTERY_WARNING_FAILED = 4 # the battery has failed completely +uint8 BATTERY_STATE_UNHEALTHY = 6 # Battery is diagnosed to be defective or an error occurred, usage is discouraged / prohibited. Possible causes (faults) are listed in faults field. +uint8 BATTERY_STATE_CHARGING = 7 # Battery is charging + +uint8 BATTERY_FAULT_DEEP_DISCHARGE = 0 # Battery has deep discharged +uint8 BATTERY_FAULT_SPIKES = 1 # Voltage spikes +uint8 BATTERY_FAULT_CELL_FAIL= 2 # One or more cells have failed +uint8 BATTERY_FAULT_OVER_CURRENT = 3 # Over-current +uint8 BATTERY_FAULT_OVER_TEMPERATURE = 4 # Over-temperature +uint8 BATTERY_FAULT_UNDER_TEMPERATURE = 5 # Under-temperature fault +uint8 BATTERY_FAULT_INCOMPATIBLE_VOLTAGE = 6 # Vehicle voltage is not compatible with battery one +uint8 BATTERY_FAULT_INCOMPATIBLE_FIRMWARE = 7 # Battery firmware is not compatible with current autopilot firmware +uint8 BATTERY_FAULT_INCOMPATIBLE_MODEL = 8 # Battery model is not supported by the system +uint8 BATTERY_FAULT_HARDWARE_FAILURE = 9 # hardware problem +uint8 BATTERY_WARNING_OVER_TEMPERATURE = 10 # Over-temperature +uint8 BATTERY_FAULT_COUNT = 11 # Counter - keep it as last element! + +uint16 faults # Smart battery supply status/fault flags (bitmask) for health indication. +uint32 custom_faults # Bitmask indicating smart battery internal manufacturer faults, those are not user actionable. +uint8 warning # Current battery warning +uint8 mode # Battery mode. Note, the normal operation mode + +uint8 BATTERY_MODE_UNKNOWN = 0 # Battery does not support a mode, or if it does, is operational +uint8 BATTERY_MODE_AUTO_DISCHARGING = 1 # Battery is auto discharging (towards storage level) +uint8 BATTERY_MODE_HOT_SWAP = 2 # Battery in hot-swap mode +uint8 BATTERY_MODE_COUNT = 3 # Counter - keep it as last element (once we're fully migrated to events interface we can just comment this)! + + +uint8 MAX_INSTANCES = 4 + +float32 average_power # The average power of the current discharge +float32 available_energy # The predicted charge or energy remaining in the battery +float32 full_charge_capacity_wh # The compensated battery capacity +float32 remaining_capacity_wh # The compensated battery capacity remaining +float32 design_capacity # The design capacity of the battery +uint16 average_time_to_full # The predicted remaining time until the battery reaches full charge, in minutes +uint16 over_discharge_count # Number of battery overdischarge +float32 nominal_voltage # Nominal voltage of the battery pack diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/Buffer128.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/Buffer128.msg new file mode 100644 index 00000000..342aa83d --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/Buffer128.msg @@ -0,0 +1,9 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 len # length of data +uint32 MAX_BUFLEN = 128 + +uint8[128] data # data + +# TOPICS voxl2_io_data + diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ButtonEvent.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ButtonEvent.msg new file mode 100644 index 00000000..bbca356a --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ButtonEvent.msg @@ -0,0 +1,6 @@ +uint64 timestamp # time since system start (microseconds) +bool triggered # Set to true if the event is triggered + +# TOPICS button_event safety_button + +uint8 ORB_QUEUE_LENGTH = 2 diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/CameraCapture.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/CameraCapture.msg new file mode 100644 index 00000000..141bb2eb --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/CameraCapture.msg @@ -0,0 +1,9 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_utc # Capture time in UTC / GPS time +uint32 seq # Image sequence number +float64 lat # Latitude in degrees (WGS84) +float64 lon # Longitude in degrees (WGS84) +float32 alt # Altitude (AMSL) +float32 ground_distance # Altitude above ground (meters) +float32[4] q # Attitude of the camera relative to NED earth-fixed frame when using a gimbal, otherwise vehicle attitude +int8 result # 1 for success, 0 for failure, -1 if camera does not provide feedback diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/CameraStatus.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/CameraStatus.msg new file mode 100644 index 00000000..c83be897 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/CameraStatus.msg @@ -0,0 +1,4 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 active_sys_id # mavlink system id of the currently active camera +uint8 active_comp_id # mavlink component id of currently active camera diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/CameraTrigger.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/CameraTrigger.msg new file mode 100644 index 00000000..abfdac6d --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/CameraTrigger.msg @@ -0,0 +1,7 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_utc # UTC timestamp + +uint32 seq # Image sequence number +bool feedback # Trigger feedback from camera + +uint32 ORB_QUEUE_LENGTH = 2 diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/CanInterfaceStatus.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/CanInterfaceStatus.msg new file mode 100644 index 00000000..4129c8d5 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/CanInterfaceStatus.msg @@ -0,0 +1,6 @@ +uint64 timestamp # time since system start (microseconds) +uint8 interface + +uint64 io_errors +uint64 frames_tx +uint64 frames_rx diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/CellularStatus.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/CellularStatus.msg new file mode 100644 index 00000000..5a1c8bac --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/CellularStatus.msg @@ -0,0 +1,28 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 CELLULAR_STATUS_FLAG_UNKNOWN=0 # State unknown or not reportable +uint8 CELLULAR_STATUS_FLAG_FAILED=1 # velocity setpoint +uint8 CELLULAR_STATUS_FLAG_INITIALIZING=2 # Modem is being initialized +uint8 CELLULAR_STATUS_FLAG_LOCKED=3 # Modem is locked +uint8 CELLULAR_STATUS_FLAG_DISABLED=4 # Modem is not enabled and is powered down +uint8 CELLULAR_STATUS_FLAG_DISABLING=5 # Modem is currently transitioning to the CELLULAR_STATUS_FLAG_DISABLED state +uint8 CELLULAR_STATUS_FLAG_ENABLING=6 # Modem is currently transitioning to the CELLULAR_STATUS_FLAG_ENABLED state +uint8 CELLULAR_STATUS_FLAG_ENABLED=7 # Modem is enabled and powered on but not registered with a network provider and not available for data connections +uint8 CELLULAR_STATUS_FLAG_SEARCHING=8 # Modem is searching for a network provider to register +uint8 CELLULAR_STATUS_FLAG_REGISTERED=9 # Modem is registered with a network provider, and data connections and messaging may be available for use +uint8 CELLULAR_STATUS_FLAG_DISCONNECTING=10 # Modem is disconnecting and deactivating the last active packet data bearer. This state will not be entered if more than one packet data bearer is active and one of the active bearers is deactivated +uint8 CELLULAR_STATUS_FLAG_CONNECTING=11 # Modem is activating and connecting the first packet data bearer. Subsequent bearer activations when another bearer is already active do not cause this state to be entered +uint8 CELLULAR_STATUS_FLAG_CONNECTED=12 # One or more packet data bearers is active and connected + +uint8 CELLULAR_NETWORK_FAILED_REASON_NONE=0 # No error +uint8 CELLULAR_NETWORK_FAILED_REASON_UNKNOWN=1 # Error state is unknown +uint8 CELLULAR_NETWORK_FAILED_REASON_SIM_MISSING=2 # SIM is required for the modem but missing +uint8 CELLULAR_NETWORK_FAILED_REASON_SIM_ERROR=3 # SIM is available, but not usable for connection + +uint16 status # Status bitmap 1: Roaming is active +uint8 failure_reason #Failure reason when status in in CELLUAR_STATUS_FAILED +uint8 type # Cellular network radio type 0: none 1: gsm 2: cdma 3: wcdma 4: lte +uint8 quality # Cellular network RSSI/RSRP in dBm, absolute value +uint16 mcc # Mobile country code. If unknown, set to: UINT16_MAX +uint16 mnc # Mobile network code. If unknown, set to: UINT16_MAX +uint16 lac # Location area code. If unknown, set to: 0 diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/CollisionConstraints.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/CollisionConstraints.msg new file mode 100644 index 00000000..40f67e29 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/CollisionConstraints.msg @@ -0,0 +1,7 @@ +# Local setpoint constraints in NED frame +# setting something to NaN means that no limit is provided + +uint64 timestamp # time since system start (microseconds) + +float32[2] original_setpoint # velocities demanded +float32[2] adapted_setpoint # velocities allowed diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/CollisionReport.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/CollisionReport.msg new file mode 100644 index 00000000..1ad7ce72 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/CollisionReport.msg @@ -0,0 +1,8 @@ +uint64 timestamp # time since system start (microseconds) +uint8 src +uint32 id +uint8 action +uint8 threat_level +float32 time_to_minimum_delta +float32 altitude_minimum_delta +float32 horizontal_minimum_delta diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ConfigOverrides.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ConfigOverrides.msg new file mode 100644 index 00000000..09b87253 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ConfigOverrides.msg @@ -0,0 +1,18 @@ +# Configurable overrides by (external) modes or mode executors +uint64 timestamp # time since system start (microseconds) + +bool disable_auto_disarm # Prevent the drone from automatically disarming after landing (if configured) + +bool defer_failsafes # Defer all failsafes that can be deferred (until the flag is cleared) +int16 defer_failsafes_timeout_s # Maximum time a failsafe can be deferred. 0 = system default, -1 = no timeout + + +int8 SOURCE_TYPE_MODE = 0 +int8 SOURCE_TYPE_MODE_EXECUTOR = 1 +int8 source_type + +uint8 source_id # ID depending on source_type + +uint8 ORB_QUEUE_LENGTH = 4 + +# TOPICS config_overrides config_overrides_request diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ControlAllocatorStatus.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ControlAllocatorStatus.msg new file mode 100644 index 00000000..2d7b0883 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ControlAllocatorStatus.msg @@ -0,0 +1,21 @@ +uint64 timestamp # time since system start (microseconds) + +bool torque_setpoint_achieved # Boolean indicating whether the 3D torque setpoint was correctly allocated to actuators. 0 if not achieved, 1 if achieved. +float32[3] unallocated_torque # Unallocated torque. Equal to 0 if the setpoint was achieved. + # Computed as: unallocated_torque = torque_setpoint - allocated_torque + +bool thrust_setpoint_achieved # Boolean indicating whether the 3D thrust setpoint was correctly allocated to actuators. 0 if not achieved, 1 if achieved. +float32[3] unallocated_thrust # Unallocated thrust. Equal to 0 if the setpoint was achieved. + # Computed as: unallocated_thrust = thrust_setpoint - allocated_thrust + +int8 ACTUATOR_SATURATION_OK = 0 # The actuator is not saturated +int8 ACTUATOR_SATURATION_UPPER_DYN = 1 # The actuator is saturated (with a value <= the desired value) because it cannot increase its value faster +int8 ACTUATOR_SATURATION_UPPER = 2 # The actuator is saturated (with a value <= the desired value) because it has reached its maximum value +int8 ACTUATOR_SATURATION_LOWER_DYN = -1 # The actuator is saturated (with a value >= the desired value) because it cannot decrease its value faster +int8 ACTUATOR_SATURATION_LOWER = -2 # The actuator is saturated (with a value >= the desired value) because it has reached its minimum value + +int8[16] actuator_saturation # Indicates actuator saturation status. + # Note 1: actuator saturation does not necessarily imply that the thrust setpoint or the torque setpoint were not achieved. + # Note 2: an actuator with limited dynamics can be indicated as upper-saturated even if it as not reached its maximum value. + +uint16 handled_motor_failure_mask # Bitmask of failed motors that were removed from the allocation / effectiveness matrix. Not necessarily identical to the report from FailureDetector diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/Cpuload.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/Cpuload.msg new file mode 100644 index 00000000..efc2c4df --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/Cpuload.msg @@ -0,0 +1,3 @@ +uint64 timestamp # time since system start (microseconds) +float32 load # processor load from 0 to 1 +float32 ram_usage # RAM usage from 0 to 1 diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/DatamanRequest.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/DatamanRequest.msg new file mode 100644 index 00000000..f819771a --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/DatamanRequest.msg @@ -0,0 +1,8 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 client_id +uint8 request_type # id/read/write/clear +uint8 item # dm_item_t +uint32 index +uint8[56] data +uint32 data_length \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/DatamanResponse.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/DatamanResponse.msg new file mode 100644 index 00000000..ebf752db --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/DatamanResponse.msg @@ -0,0 +1,15 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 client_id +uint8 request_type # id/read/write/clear +uint8 item # dm_item_t +uint32 index +uint8[56] data + +uint8 STATUS_SUCCESS = 0 +uint8 STATUS_FAILURE_ID_ERR = 1 +uint8 STATUS_FAILURE_NO_DATA = 2 +uint8 STATUS_FAILURE_READ_FAILED = 3 +uint8 STATUS_FAILURE_WRITE_FAILED = 4 +uint8 STATUS_FAILURE_CLEAR_FAILED = 5 +uint8 status diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/DebugArray.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/DebugArray.msg new file mode 100644 index 00000000..4763a0f5 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/DebugArray.msg @@ -0,0 +1,5 @@ +uint8 ARRAY_SIZE = 58 +uint64 timestamp # time since system start (microseconds) +uint16 id # unique ID of debug array, used to discriminate between arrays +char[10] name # name of the debug array (max. 10 characters) +float32[58] data # data \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/DebugKeyValue.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/DebugKeyValue.msg new file mode 100644 index 00000000..6815811e --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/DebugKeyValue.msg @@ -0,0 +1,3 @@ +uint64 timestamp # time since system start (microseconds) +char[10] key # max. 10 characters as key / name +float32 value # the value to send as debug output diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/DebugValue.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/DebugValue.msg new file mode 100644 index 00000000..8be13124 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/DebugValue.msg @@ -0,0 +1,3 @@ +uint64 timestamp # time since system start (microseconds) +int8 ind # index of debug variable +float32 value # the value to send as debug output diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/DebugVect.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/DebugVect.msg new file mode 100644 index 00000000..9c22e1da --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/DebugVect.msg @@ -0,0 +1,5 @@ +uint64 timestamp # time since system start (microseconds) +char[10] name # max. 10 characters as key / name +float32 x # x value +float32 y # y value +float32 z # z value diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/DifferentialDriveSetpoint.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/DifferentialDriveSetpoint.msg new file mode 100644 index 00000000..f7e4c584 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/DifferentialDriveSetpoint.msg @@ -0,0 +1,8 @@ +uint64 timestamp # time since system start (microseconds) + +float32 speed # [m/s] collective roll-off speed in body x-axis +bool closed_loop_speed_control # true if speed is controlled using estimator feedback, false if direct feed-forward +float32 yaw_rate # [rad/s] yaw rate +bool closed_loop_yaw_rate_control # true if yaw rate is controlled using gyroscope feedback, false if direct feed-forward + +# TOPICS differential_drive_setpoint differential_drive_control_output diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/DifferentialPressure.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/DifferentialPressure.msg new file mode 100644 index 00000000..0cdf1e4c --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/DifferentialPressure.msg @@ -0,0 +1,10 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +float32 differential_pressure_pa # differential pressure reading in Pascals (may be negative) + +float32 temperature # Temperature provided by sensor in degrees Celsius, NAN if unknown + +uint32 error_count # Number of errors detected by driver diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/DistanceSensor.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/DistanceSensor.msg new file mode 100644 index 00000000..dd08e4b5 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/DistanceSensor.msg @@ -0,0 +1,42 @@ +# DISTANCE_SENSOR message data + +uint64 timestamp # time since system start (microseconds) + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +float32 min_distance # Minimum distance the sensor can measure (in m) +float32 max_distance # Maximum distance the sensor can measure (in m) +float32 current_distance # Current distance reading (in m) +float32 variance # Measurement variance (in m^2), 0 for unknown / invalid readings +int8 signal_quality # Signal quality in percent (0...100%), where 0 = invalid signal, 100 = perfect signal, and -1 = unknown signal quality. + +uint8 type # Type from MAV_DISTANCE_SENSOR enum +uint8 MAV_DISTANCE_SENSOR_LASER = 0 +uint8 MAV_DISTANCE_SENSOR_ULTRASOUND = 1 +uint8 MAV_DISTANCE_SENSOR_INFRARED = 2 +uint8 MAV_DISTANCE_SENSOR_RADAR = 3 + +float32 h_fov # Sensor horizontal field of view (rad) +float32 v_fov # Sensor vertical field of view (rad) +float32[4] q # Quaterion sensor orientation with respect to the vehicle body frame to specify the orientation ROTATION_CUSTOM + +uint8 orientation # Direction the sensor faces from MAV_SENSOR_ORIENTATION enum + +uint8 ROTATION_YAW_0 = 0 # MAV_SENSOR_ROTATION_NONE +uint8 ROTATION_YAW_45 = 1 # MAV_SENSOR_ROTATION_YAW_45 +uint8 ROTATION_YAW_90 = 2 # MAV_SENSOR_ROTATION_YAW_90 +uint8 ROTATION_YAW_135 = 3 # MAV_SENSOR_ROTATION_YAW_135 +uint8 ROTATION_YAW_180 = 4 # MAV_SENSOR_ROTATION_YAW_180 +uint8 ROTATION_YAW_225 = 5 # MAV_SENSOR_ROTATION_YAW_225 +uint8 ROTATION_YAW_270 = 6 # MAV_SENSOR_ROTATION_YAW_270 +uint8 ROTATION_YAW_315 = 7 # MAV_SENSOR_ROTATION_YAW_315 + +uint8 ROTATION_FORWARD_FACING = 0 # MAV_SENSOR_ROTATION_NONE +uint8 ROTATION_RIGHT_FACING = 2 # MAV_SENSOR_ROTATION_YAW_90 +uint8 ROTATION_BACKWARD_FACING = 4 # MAV_SENSOR_ROTATION_YAW_180 +uint8 ROTATION_LEFT_FACING = 6 # MAV_SENSOR_ROTATION_YAW_270 + +uint8 ROTATION_UPWARD_FACING = 24 # MAV_SENSOR_ROTATION_PITCH_90 +uint8 ROTATION_DOWNWARD_FACING = 25 # MAV_SENSOR_ROTATION_PITCH_270 + +uint8 ROTATION_CUSTOM = 100 # MAV_SENSOR_ROTATION_CUSTOM diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/Ekf2Timestamps.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/Ekf2Timestamps.msg new file mode 100644 index 00000000..ae3ac067 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/Ekf2Timestamps.msg @@ -0,0 +1,23 @@ +# this message contains the (relative) timestamps of the sensor inputs used by EKF2. +# It can be used for reproducible replay. + +# the timestamp field is the ekf2 reference time and matches the timestamp of +# the sensor_combined topic. + +uint64 timestamp # time since system start (microseconds) + +int16 RELATIVE_TIMESTAMP_INVALID = 32767 # (0x7fff) If one of the relative timestamps + # is set to this value, it means the associated sensor values did not update + +# timestamps are relative to the main timestamp and are in 0.1 ms (timestamp + +# *_timestamp_rel = absolute timestamp). For int16, this allows a maximum +# difference of +-3.2s to the sensor_combined topic. + +int16 airspeed_timestamp_rel +int16 distance_sensor_timestamp_rel +int16 optical_flow_timestamp_rel +int16 vehicle_air_data_timestamp_rel +int16 vehicle_magnetometer_timestamp_rel +int16 visual_odometry_timestamp_rel + +# Note: this is a high-rate logged topic, so it needs to be as small as possible diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/EscReport.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/EscReport.msg new file mode 100644 index 00000000..9a75c3d7 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/EscReport.msg @@ -0,0 +1,27 @@ +uint64 timestamp # time since system start (microseconds) +uint32 esc_errorcount # Number of reported errors by ESC - if supported +int32 esc_rpm # Motor RPM, negative for reverse rotation [RPM] - if supported +float32 esc_voltage # Voltage measured from current ESC [V] - if supported +float32 esc_current # Current measured from current ESC [A] - if supported +float32 esc_temperature # Temperature measured from current ESC [degC] - if supported +uint8 esc_address # Address of current ESC (in most cases 1-8 / must be set by driver) +uint8 esc_cmdcount # Counter of number of commands + +uint8 esc_state # State of ESC - depend on Vendor + +uint8 actuator_function # actuator output function (one of Motor1...MotorN) + +uint16 failures # Bitmask to indicate the internal ESC faults +int8 esc_power # Applied power 0-100 in % (negative values reserved) + +uint8 FAILURE_OVER_CURRENT = 0 # (1 << 0) +uint8 FAILURE_OVER_VOLTAGE = 1 # (1 << 1) +uint8 FAILURE_MOTOR_OVER_TEMPERATURE = 2 # (1 << 2) +uint8 FAILURE_OVER_RPM = 3 # (1 << 3) +uint8 FAILURE_INCONSISTENT_CMD = 4 # (1 << 4) Set if ESC received an inconsistent command (i.e out of boundaries) +uint8 FAILURE_MOTOR_STUCK = 5 # (1 << 5) +uint8 FAILURE_GENERIC = 6 # (1 << 6) +uint8 FAILURE_MOTOR_WARN_TEMPERATURE = 7 # (1 << 7) +uint8 FAILURE_WARN_ESC_TEMPERATURE = 8 # (1 << 8) +uint8 FAILURE_OVER_ESC_TEMPERATURE = 9 # (1 << 9) +uint8 ESC_FAILURE_COUNT = 10 # Counter - keep it as last element! diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/EscStatus.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/EscStatus.msg new file mode 100644 index 00000000..e5e220ce --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/EscStatus.msg @@ -0,0 +1,28 @@ +uint64 timestamp # time since system start (microseconds) +uint8 CONNECTED_ESC_MAX = 8 # The number of ESCs supported. Current (Q2/2013) we support 8 ESCs + +uint8 ESC_CONNECTION_TYPE_PPM = 0 # Traditional PPM ESC +uint8 ESC_CONNECTION_TYPE_SERIAL = 1 # Serial Bus connected ESC +uint8 ESC_CONNECTION_TYPE_ONESHOT = 2 # One Shot PPM +uint8 ESC_CONNECTION_TYPE_I2C = 3 # I2C +uint8 ESC_CONNECTION_TYPE_CAN = 4 # CAN-Bus +uint8 ESC_CONNECTION_TYPE_DSHOT = 5 # DShot + +uint16 counter # incremented by the writing thread everytime new data is stored + +uint8 esc_count # number of connected ESCs +uint8 esc_connectiontype # how ESCs connected to the system + +uint8 esc_online_flags # Bitmask indicating which ESC is online/offline +# esc_online_flags bit 0 : Set to 1 if ESC0 is online +# esc_online_flags bit 1 : Set to 1 if ESC1 is online +# esc_online_flags bit 2 : Set to 1 if ESC2 is online +# esc_online_flags bit 3 : Set to 1 if ESC3 is online +# esc_online_flags bit 4 : Set to 1 if ESC4 is online +# esc_online_flags bit 5 : Set to 1 if ESC5 is online +# esc_online_flags bit 6 : Set to 1 if ESC6 is online +# esc_online_flags bit 7 : Set to 1 if ESC7 is online + +uint8 esc_armed_flags # Bitmask indicating which ESC is armed. For ESC's where the arming state is not known (returned by the ESC), the arming bits should always be set. + +EscReport[8] esc diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorAidSource1d.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorAidSource1d.msg new file mode 100644 index 00000000..7bd8ea76 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorAidSource1d.msg @@ -0,0 +1,27 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +uint8 estimator_instance + +uint32 device_id + +uint64 time_last_fuse + +float32 observation +float32 observation_variance + +float32 innovation +float32 innovation_filtered + +float32 innovation_variance + +float32 test_ratio # normalized innovation squared +float32 test_ratio_filtered # signed filtered test ratio + +bool innovation_rejected # true if the observation has been rejected +bool fused # true if the sample was successfully fused + +# TOPICS estimator_aid_src_baro_hgt estimator_aid_src_ev_hgt estimator_aid_src_gnss_hgt estimator_aid_src_rng_hgt +# TOPICS estimator_aid_src_airspeed estimator_aid_src_sideslip +# TOPICS estimator_aid_src_fake_hgt +# TOPICS estimator_aid_src_gnss_yaw estimator_aid_src_ev_yaw diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorAidSource2d.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorAidSource2d.msg new file mode 100644 index 00000000..14e3ac3f --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorAidSource2d.msg @@ -0,0 +1,26 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +uint8 estimator_instance + +uint32 device_id + +uint64 time_last_fuse + +float32[2] observation +float32[2] observation_variance + +float32[2] innovation +float32[2] innovation_filtered + +float32[2] innovation_variance + +float32[2] test_ratio # normalized innovation squared +float32[2] test_ratio_filtered # signed filtered test ratio + +bool innovation_rejected # true if the observation has been rejected +bool fused # true if the sample was successfully fused + +# TOPICS estimator_aid_src_ev_pos estimator_aid_src_fake_pos estimator_aid_src_gnss_pos estimator_aid_src_aux_global_position +# TOPICS estimator_aid_src_aux_vel estimator_aid_src_optical_flow +# TOPICS estimator_aid_src_drag diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorAidSource3d.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorAidSource3d.msg new file mode 100644 index 00000000..b89add28 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorAidSource3d.msg @@ -0,0 +1,24 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +uint8 estimator_instance + +uint32 device_id + +uint64 time_last_fuse + +float32[3] observation +float32[3] observation_variance + +float32[3] innovation +float32[3] innovation_filtered + +float32[3] innovation_variance + +float32[3] test_ratio # normalized innovation squared +float32[3] test_ratio_filtered # signed filtered test ratio + +bool innovation_rejected # true if the observation has been rejected +bool fused # true if the sample was successfully fused + +# TOPICS estimator_aid_src_ev_vel estimator_aid_src_gnss_vel estimator_aid_src_gravity estimator_aid_src_mag diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorBias.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorBias.msg new file mode 100644 index 00000000..bb65e47b --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorBias.msg @@ -0,0 +1,12 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +uint32 device_id # unique device ID for the sensor that does not change between power cycles +float32 bias # estimated barometric altitude bias (m) +float32 bias_var # estimated barometric altitude bias variance (m^2) + +float32 innov # innovation of the last measurement fusion (m) +float32 innov_var # innovation variance of the last measurement fusion (m^2) +float32 innov_test_ratio # normalized innovation squared test ratio + +# TOPICS estimator_baro_bias estimator_gnss_hgt_bias diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorBias3d.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorBias3d.msg new file mode 100644 index 00000000..16b29372 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorBias3d.msg @@ -0,0 +1,14 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +float32[3] bias # estimated barometric altitude bias (m) +float32[3] bias_var # estimated barometric altitude bias variance (m^2) + +float32[3] innov # innovation of the last measurement fusion (m) +float32[3] innov_var # innovation variance of the last measurement fusion (m^2) +float32[3] innov_test_ratio # normalized innovation squared test ratio + +# TOPICS estimator_bias3d +# TOPICS estimator_ev_pos_bias diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorEventFlags.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorEventFlags.msg new file mode 100644 index 00000000..1a47e676 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorEventFlags.msg @@ -0,0 +1,37 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +# information events +uint32 information_event_changes # number of information event changes +bool gps_checks_passed # 0 - true when gps quality checks are passing passed +bool reset_vel_to_gps # 1 - true when the velocity states are reset to the gps measurement +bool reset_vel_to_flow # 2 - true when the velocity states are reset using the optical flow measurement +bool reset_vel_to_vision # 3 - true when the velocity states are reset to the vision system measurement +bool reset_vel_to_zero # 4 - true when the velocity states are reset to zero +bool reset_pos_to_last_known # 5 - true when the position states are reset to the last known position +bool reset_pos_to_gps # 6 - true when the position states are reset to the gps measurement +bool reset_pos_to_vision # 7 - true when the position states are reset to the vision system measurement +bool starting_gps_fusion # 8 - true when the filter starts using gps measurements to correct the state estimates +bool starting_vision_pos_fusion # 9 - true when the filter starts using vision system position measurements to correct the state estimates +bool starting_vision_vel_fusion # 10 - true when the filter starts using vision system velocity measurements to correct the state estimates +bool starting_vision_yaw_fusion # 11 - true when the filter starts using vision system yaw measurements to correct the state estimates +bool yaw_aligned_to_imu_gps # 12 - true when the filter resets the yaw to an estimate derived from IMU and GPS data +bool reset_hgt_to_baro # 13 - true when the vertical position state is reset to the baro measurement +bool reset_hgt_to_gps # 14 - true when the vertical position state is reset to the gps measurement +bool reset_hgt_to_rng # 15 - true when the vertical position state is reset to the rng measurement +bool reset_hgt_to_ev # 16 - true when the vertical position state is reset to the ev measurement + +# warning events +uint32 warning_event_changes # number of warning event changes +bool gps_quality_poor # 0 - true when the gps is failing quality checks +bool gps_fusion_timout # 1 - true when the gps data has not been used to correct the state estimates for a significant time period +bool gps_data_stopped # 2 - true when the gps data has stopped for a significant time period +bool gps_data_stopped_using_alternate # 3 - true when the gps data has stopped for a significant time period but the filter is able to use other sources of data to maintain navigation +bool height_sensor_timeout # 4 - true when the height sensor has not been used to correct the state estimates for a significant time period +bool stopping_navigation # 5 - true when the filter has insufficient data to estimate velocity and position and is falling back to an attitude, height and height rate mode of operation +bool invalid_accel_bias_cov_reset # 6 - true when the filter has detected bad acceerometer bias state esitmstes and has reset the corresponding covariance matrix elements +bool bad_yaw_using_gps_course # 7 - true when the filter has detected an invalid yaw estimate and has reset the yaw angle to the GPS ground course +bool stopping_mag_use # 8 - true when the filter has detected bad magnetometer data and is stopping further use of the magnetometer data +bool vision_data_stopped # 9 - true when the vision system data has stopped for a significant time period +bool emergency_yaw_reset_mag_stopped # 10 - true when the filter has detected bad magnetometer data, has reset the yaw to anothter source of data and has stopped further use of the magnetometer data +bool emergency_yaw_reset_gps_yaw_stopped # 11 - true when the filter has detected bad GNSS yaw data, has reset the yaw to anothter source of data and has stopped further use of the GNSS yaw data diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorGpsStatus.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorGpsStatus.msg new file mode 100644 index 00000000..2d2462ee --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorGpsStatus.msg @@ -0,0 +1,19 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +bool checks_passed + +bool check_fail_gps_fix # 0 : insufficient fix type (no 3D solution) +bool check_fail_min_sat_count # 1 : minimum required sat count fail +bool check_fail_max_pdop # 2 : maximum allowed PDOP fail +bool check_fail_max_horz_err # 3 : maximum allowed horizontal position error fail +bool check_fail_max_vert_err # 4 : maximum allowed vertical position error fail +bool check_fail_max_spd_err # 5 : maximum allowed speed error fail +bool check_fail_max_horz_drift # 6 : maximum allowed horizontal position drift fail - requires stationary vehicle +bool check_fail_max_vert_drift # 7 : maximum allowed vertical position drift fail - requires stationary vehicle +bool check_fail_max_horz_spd_err # 8 : maximum allowed horizontal speed fail - requires stationary vehicle +bool check_fail_max_vert_spd_err # 9 : maximum allowed vertical velocity discrepancy fail + +float32 position_drift_rate_horizontal_m_s # Horizontal position rate magnitude (m/s) +float32 position_drift_rate_vertical_m_s # Vertical position rate magnitude (m/s) +float32 filtered_horizontal_speed_m_s # Filtered horizontal velocity magnitude (m/s) diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorInnovations.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorInnovations.msg new file mode 100644 index 00000000..11cc6a58 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorInnovations.msg @@ -0,0 +1,39 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +# GPS +float32[2] gps_hvel # horizontal GPS velocity innovation (m/sec) and innovation variance ((m/sec)**2) +float32 gps_vvel # vertical GPS velocity innovation (m/sec) and innovation variance ((m/sec)**2) +float32[2] gps_hpos # horizontal GPS position innovation (m) and innovation variance (m**2) +float32 gps_vpos # vertical GPS position innovation (m) and innovation variance (m**2) + +# External Vision +float32[2] ev_hvel # horizontal external vision velocity innovation (m/sec) and innovation variance ((m/sec)**2) +float32 ev_vvel # vertical external vision velocity innovation (m/sec) and innovation variance ((m/sec)**2) +float32[2] ev_hpos # horizontal external vision position innovation (m) and innovation variance (m**2) +float32 ev_vpos # vertical external vision position innovation (m) and innovation variance (m**2) + +# Height sensors +float32 rng_vpos # range sensor height innovation (m) and innovation variance (m**2) +float32 baro_vpos # barometer height innovation (m) and innovation variance (m**2) + +# Auxiliary velocity +float32[2] aux_hvel # horizontal auxiliary velocity innovation from landing target measurement (m/sec) and innovation variance ((m/sec)**2) + +# Optical flow +float32[2] flow # flow innvoation (rad/sec) and innovation variance ((rad/sec)**2) + +# Various +float32 heading # heading innovation (rad) and innovation variance (rad**2) +float32[3] mag_field # earth magnetic field innovation (Gauss) and innovation variance (Gauss**2) +float32[3] gravity # gravity innovation from accelerometerr vector (m/s**2) +float32[2] drag # drag specific force innovation (m/sec**2) and innovation variance ((m/sec)**2) +float32 airspeed # airspeed innovation (m/sec) and innovation variance ((m/sec)**2) +float32 beta # synthetic sideslip innovation (rad) and innovation variance (rad**2) +float32 hagl # height of ground innovation (m) and innovation variance (m**2) +float32 hagl_rate # height of ground rate innovation (m/s) and innovation variance ((m/s)**2) + +# The innovation test ratios are scalar values. In case the field is a vector, +# the test ratio will be put in the first component of the vector. + +# TOPICS estimator_innovations estimator_innovation_variances estimator_innovation_test_ratios diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorSelectorStatus.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorSelectorStatus.msg new file mode 100644 index 00000000..52f80859 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorSelectorStatus.msg @@ -0,0 +1,22 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 primary_instance + +uint8 instances_available + +uint32 instance_changed_count +uint64 last_instance_change + +uint32 accel_device_id +uint32 baro_device_id +uint32 gyro_device_id +uint32 mag_device_id + +float32[9] combined_test_ratio +float32[9] relative_test_ratio +bool[9] healthy + +float32[4] accumulated_gyro_error +float32[4] accumulated_accel_error +bool gyro_fault_detected +bool accel_fault_detected diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorSensorBias.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorSensorBias.msg new file mode 100644 index 00000000..f42e1aa8 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorSensorBias.msg @@ -0,0 +1,30 @@ +# +# Sensor readings and in-run biases in SI-unit form. Sensor readings are compensated for static offsets, +# scale errors, in-run bias and thermal drift (if thermal compensation is enabled and available). +# + +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +# In-run bias estimates (subtract from uncorrected data) + +uint32 gyro_device_id # unique device ID for the sensor that does not change between power cycles +float32[3] gyro_bias # gyroscope in-run bias in body frame (rad/s) +float32 gyro_bias_limit # magnitude of maximum gyroscope in-run bias in body frame (rad/s) +float32[3] gyro_bias_variance +bool gyro_bias_valid +bool gyro_bias_stable # true when the gyro bias estimate is stable enough to use for calibration + +uint32 accel_device_id # unique device ID for the sensor that does not change between power cycles +float32[3] accel_bias # accelerometer in-run bias in body frame (m/s^2) +float32 accel_bias_limit # magnitude of maximum accelerometer in-run bias in body frame (m/s^2) +float32[3] accel_bias_variance +bool accel_bias_valid +bool accel_bias_stable # true when the accel bias estimate is stable enough to use for calibration + +uint32 mag_device_id # unique device ID for the sensor that does not change between power cycles +float32[3] mag_bias # magnetometer in-run bias in body frame (Gauss) +float32 mag_bias_limit # magnitude of maximum magnetometer in-run bias in body frame (Gauss) +float32[3] mag_bias_variance +bool mag_bias_valid +bool mag_bias_stable # true when the mag bias estimate is stable enough to use for calibration diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorStates.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorStates.msg new file mode 100644 index 00000000..885246d8 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorStates.msg @@ -0,0 +1,7 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +float32[25] states # Internal filter states +uint8 n_states # Number of states effectively used + +float32[24] covariances # Diagonal Elements of Covariance Matrix diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorStatus.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorStatus.msg new file mode 100644 index 00000000..ac13b59e --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorStatus.msg @@ -0,0 +1,121 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +float32[3] output_tracking_error # return a vector containing the output predictor angular, velocity and position tracking error magnitudes (rad), (m/s), (m) + +uint16 gps_check_fail_flags # Bitmask to indicate status of GPS checks - see definition below +# bits are true when corresponding test has failed +uint8 GPS_CHECK_FAIL_GPS_FIX = 0 # 0 : insufficient fix type (no 3D solution) +uint8 GPS_CHECK_FAIL_MIN_SAT_COUNT = 1 # 1 : minimum required sat count fail +uint8 GPS_CHECK_FAIL_MAX_PDOP = 2 # 2 : maximum allowed PDOP fail +uint8 GPS_CHECK_FAIL_MAX_HORZ_ERR = 3 # 3 : maximum allowed horizontal position error fail +uint8 GPS_CHECK_FAIL_MAX_VERT_ERR = 4 # 4 : maximum allowed vertical position error fail +uint8 GPS_CHECK_FAIL_MAX_SPD_ERR = 5 # 5 : maximum allowed speed error fail +uint8 GPS_CHECK_FAIL_MAX_HORZ_DRIFT = 6 # 6 : maximum allowed horizontal position drift fail - requires stationary vehicle +uint8 GPS_CHECK_FAIL_MAX_VERT_DRIFT = 7 # 7 : maximum allowed vertical position drift fail - requires stationary vehicle +uint8 GPS_CHECK_FAIL_MAX_HORZ_SPD_ERR = 8 # 8 : maximum allowed horizontal speed fail - requires stationary vehicle +uint8 GPS_CHECK_FAIL_MAX_VERT_SPD_ERR = 9 # 9 : maximum allowed vertical velocity discrepancy fail + +uint64 control_mode_flags # Bitmask to indicate EKF logic state +uint8 CS_TILT_ALIGN = 0 # 0 - true if the filter tilt alignment is complete +uint8 CS_YAW_ALIGN = 1 # 1 - true if the filter yaw alignment is complete +uint8 CS_GPS = 2 # 2 - true if GPS measurements are being fused +uint8 CS_OPT_FLOW = 3 # 3 - true if optical flow measurements are being fused +uint8 CS_MAG_HDG = 4 # 4 - true if a simple magnetic yaw heading is being fused +uint8 CS_MAG_3D = 5 # 5 - true if 3-axis magnetometer measurement are being fused +uint8 CS_MAG_DEC = 6 # 6 - true if synthetic magnetic declination measurements are being fused +uint8 CS_IN_AIR = 7 # 7 - true when thought to be airborne +uint8 CS_WIND = 8 # 8 - true when wind velocity is being estimated +uint8 CS_BARO_HGT = 9 # 9 - true when baro height is being fused as a primary height reference +uint8 CS_RNG_HGT = 10 # 10 - true when range finder height is being fused as a primary height reference +uint8 CS_GPS_HGT = 11 # 11 - true when GPS height is being fused as a primary height reference +uint8 CS_EV_POS = 12 # 12 - true when local position data from external vision is being fused +uint8 CS_EV_YAW = 13 # 13 - true when yaw data from external vision measurements is being fused +uint8 CS_EV_HGT = 14 # 14 - true when height data from external vision measurements is being fused +uint8 CS_BETA = 15 # 15 - true when synthetic sideslip measurements are being fused +uint8 CS_MAG_FIELD = 16 # 16 - true when only the magnetic field states are updated by the magnetometer +uint8 CS_FIXED_WING = 17 # 17 - true when thought to be operating as a fixed wing vehicle with constrained sideslip +uint8 CS_MAG_FAULT = 18 # 18 - true when the magnetometer has been declared faulty and is no longer being used +uint8 CS_ASPD = 19 # 19 - true when airspeed measurements are being fused +uint8 CS_GND_EFFECT = 20 # 20 - true when when protection from ground effect induced static pressure rise is active +uint8 CS_RNG_STUCK = 21 # 21 - true when a stuck range finder sensor has been detected +uint8 CS_GPS_YAW = 22 # 22 - true when yaw (not ground course) data from a GPS receiver is being fused +uint8 CS_MAG_ALIGNED = 23 # 23 - true when the in-flight mag field alignment has been completed +uint8 CS_EV_VEL = 24 # 24 - true when local frame velocity data fusion from external vision measurements is intended +uint8 CS_SYNTHETIC_MAG_Z = 25 # 25 - true when we are using a synthesized measurement for the magnetometer Z component +uint8 CS_VEHICLE_AT_REST = 26 # 26 - true when the vehicle is at rest +uint8 CS_GPS_YAW_FAULT = 27 # 27 - true when the GNSS heading has been declared faulty and is no longer being used +uint8 CS_RNG_FAULT = 28 # 28 - true when the range finder has been declared faulty and is no longer being used + +uint32 filter_fault_flags # Bitmask to indicate EKF internal faults +# 0 - true if the fusion of the magnetometer X-axis has encountered a numerical error +# 1 - true if the fusion of the magnetometer Y-axis has encountered a numerical error +# 2 - true if the fusion of the magnetometer Z-axis has encountered a numerical error +# 3 - true if the fusion of the magnetic heading has encountered a numerical error +# 4 - true if the fusion of the magnetic declination has encountered a numerical error +# 5 - true if fusion of the airspeed has encountered a numerical error +# 6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error +# 7 - true if fusion of the optical flow X axis has encountered a numerical error +# 8 - true if fusion of the optical flow Y axis has encountered a numerical error +# 9 - true if fusion of the North velocity has encountered a numerical error +# 10 - true if fusion of the East velocity has encountered a numerical error +# 11 - true if fusion of the Down velocity has encountered a numerical error +# 12 - true if fusion of the North position has encountered a numerical error +# 13 - true if fusion of the East position has encountered a numerical error +# 14 - true if fusion of the Down position has encountered a numerical error +# 15 - true if bad delta velocity bias estimates have been detected +# 16 - true if bad vertical accelerometer data has been detected +# 17 - true if delta velocity data contains clipping (asymmetric railing) + +float32 pos_horiz_accuracy # 1-Sigma estimated horizontal position accuracy relative to the estimators origin (m) +float32 pos_vert_accuracy # 1-Sigma estimated vertical position accuracy relative to the estimators origin (m) + +float32 mag_test_ratio # low-pass filtered ratio of the largest magnetometer innovation component to the innovation test limit +float32 vel_test_ratio # low-pass filtered ratio of the largest velocity innovation component to the innovation test limit +float32 pos_test_ratio # low-pass filtered ratio of the largest horizontal position innovation component to the innovation test limit +float32 hgt_test_ratio # low-pass filtered ratio of the vertical position innovation to the innovation test limit +float32 tas_test_ratio # low-pass filtered ratio of the true airspeed innovation to the innovation test limit +float32 hagl_test_ratio # low-pass filtered ratio of the height above ground innovation to the innovation test limit +float32 beta_test_ratio # low-pass filtered ratio of the synthetic sideslip innovation to the innovation test limit + +uint16 solution_status_flags # Bitmask indicating which filter kinematic state outputs are valid for flight control use. +# 0 - True if the attitude estimate is good +# 1 - True if the horizontal velocity estimate is good +# 2 - True if the vertical velocity estimate is good +# 3 - True if the horizontal position (relative) estimate is good +# 4 - True if the horizontal position (absolute) estimate is good +# 5 - True if the vertical position (absolute) estimate is good +# 6 - True if the vertical position (above ground) estimate is good +# 7 - True if the EKF is in a constant position mode and is not using external measurements (eg GPS or optical flow) +# 8 - True if the EKF has sufficient data to enter a mode that will provide a (relative) position estimate +# 9 - True if the EKF has sufficient data to enter a mode that will provide a (absolute) position estimate +# 10 - True if the EKF has detected a GPS glitch +# 11 - True if the EKF has detected bad accelerometer data + +uint8 reset_count_vel_ne # number of horizontal position reset events (allow to wrap if count exceeds 255) +uint8 reset_count_vel_d # number of vertical velocity reset events (allow to wrap if count exceeds 255) +uint8 reset_count_pos_ne # number of horizontal position reset events (allow to wrap if count exceeds 255) +uint8 reset_count_pod_d # number of vertical position reset events (allow to wrap if count exceeds 255) +uint8 reset_count_quat # number of quaternion reset events (allow to wrap if count exceeds 255) + +float32 time_slip # cumulative amount of time in seconds that the EKF inertial calculation has slipped relative to system time + +bool pre_flt_fail_innov_heading +bool pre_flt_fail_innov_vel_horiz +bool pre_flt_fail_innov_vel_vert +bool pre_flt_fail_innov_height +bool pre_flt_fail_mag_field_disturbed + +uint32 accel_device_id +uint32 gyro_device_id +uint32 baro_device_id +uint32 mag_device_id + +# legacy local position estimator (LPE) flags +uint8 health_flags # Bitmask to indicate sensor health states (vel, pos, hgt) +uint8 timeout_flags # Bitmask to indicate timeout flags (vel, pos, hgt) + +float32 mag_inclination_deg +float32 mag_inclination_ref_deg +float32 mag_strength_gs +float32 mag_strength_ref_gs diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorStatusFlags.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorStatusFlags.msg new file mode 100644 index 00000000..c6e0504f --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/EstimatorStatusFlags.msg @@ -0,0 +1,74 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + + +# filter control status +uint32 control_status_changes # number of filter control status (cs) changes +bool cs_tilt_align # 0 - true if the filter tilt alignment is complete +bool cs_yaw_align # 1 - true if the filter yaw alignment is complete +bool cs_gps # 2 - true if GPS measurement fusion is intended +bool cs_opt_flow # 3 - true if optical flow measurements fusion is intended +bool cs_mag_hdg # 4 - true if a simple magnetic yaw heading fusion is intended +bool cs_mag_3d # 5 - true if 3-axis magnetometer measurement fusion is intended +bool cs_mag_dec # 6 - true if synthetic magnetic declination measurements fusion is intended +bool cs_in_air # 7 - true when the vehicle is airborne +bool cs_wind # 8 - true when wind velocity is being estimated +bool cs_baro_hgt # 9 - true when baro height is being fused as a primary height reference +bool cs_rng_hgt # 10 - true when range finder height is being fused as a primary height reference +bool cs_gps_hgt # 11 - true when GPS height is being fused as a primary height reference +bool cs_ev_pos # 12 - true when local position data fusion from external vision is intended +bool cs_ev_yaw # 13 - true when yaw data from external vision measurements fusion is intended +bool cs_ev_hgt # 14 - true when height data from external vision measurements is being fused +bool cs_fuse_beta # 15 - true when synthetic sideslip measurements are being fused +bool cs_mag_field_disturbed # 16 - true when the mag field does not match the expected strength +bool cs_fixed_wing # 17 - true when the vehicle is operating as a fixed wing vehicle +bool cs_mag_fault # 18 - true when the magnetometer has been declared faulty and is no longer being used +bool cs_fuse_aspd # 19 - true when airspeed measurements are being fused +bool cs_gnd_effect # 20 - true when protection from ground effect induced static pressure rise is active +bool cs_rng_stuck # 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough +bool cs_gps_yaw # 22 - true when yaw (not ground course) data fusion from a GPS receiver is intended +bool cs_mag_aligned_in_flight # 23 - true when the in-flight mag field alignment has been completed +bool cs_ev_vel # 24 - true when local frame velocity data fusion from external vision measurements is intended +bool cs_synthetic_mag_z # 25 - true when we are using a synthesized measurement for the magnetometer Z component +bool cs_vehicle_at_rest # 26 - true when the vehicle is at rest +bool cs_gps_yaw_fault # 27 - true when the GNSS heading has been declared faulty and is no longer being used +bool cs_rng_fault # 28 - true when the range finder has been declared faulty and is no longer being used +bool cs_inertial_dead_reckoning # 29 - true if we are no longer fusing measurements that constrain horizontal velocity drift +bool cs_wind_dead_reckoning # 30 - true if we are navigationg reliant on wind relative measurements +bool cs_rng_kin_consistent # 31 - true when the range finder kinematic consistency check is passing +bool cs_fake_pos # 32 - true when fake position measurements are being fused +bool cs_fake_hgt # 33 - true when fake height measurements are being fused +bool cs_gravity_vector # 34 - true when gravity vector measurements are being fused +bool cs_mag # 35 - true if 3-axis magnetometer measurement fusion (mag states only) is intended +bool cs_ev_yaw_fault # 36 - true when the EV heading has been declared faulty and is no longer being used +bool cs_mag_heading_consistent # 37 - true when the heading obtained from mag data is declared consistent with the filter +bool cs_aux_gpos # 38 - true if auxiliary global position measurement fusion is intended + +# fault status +uint32 fault_status_changes # number of filter fault status (fs) changes +bool fs_bad_mag_x # 0 - true if the fusion of the magnetometer X-axis has encountered a numerical error +bool fs_bad_mag_y # 1 - true if the fusion of the magnetometer Y-axis has encountered a numerical error +bool fs_bad_mag_z # 2 - true if the fusion of the magnetometer Z-axis has encountered a numerical error +bool fs_bad_hdg # 3 - true if the fusion of the heading angle has encountered a numerical error +bool fs_bad_mag_decl # 4 - true if the fusion of the magnetic declination has encountered a numerical error +bool fs_bad_airspeed # 5 - true if fusion of the airspeed has encountered a numerical error +bool fs_bad_sideslip # 6 - true if fusion of the synthetic sideslip constraint has encountered a numerical error +bool fs_bad_optflow_x # 7 - true if fusion of the optical flow X axis has encountered a numerical error +bool fs_bad_optflow_y # 8 - true if fusion of the optical flow Y axis has encountered a numerical error +bool fs_bad_acc_bias # 9 - true if bad delta velocity bias estimates have been detected +bool fs_bad_acc_vertical # 10 - true if bad vertical accelerometer data has been detected +bool fs_bad_acc_clipping # 11 - true if delta velocity data contains clipping (asymmetric railing) + + +# innovation test failures +uint32 innovation_fault_status_changes # number of innovation fault status (reject) changes +bool reject_hor_vel # 0 - true if horizontal velocity observations have been rejected +bool reject_ver_vel # 1 - true if vertical velocity observations have been rejected +bool reject_hor_pos # 2 - true if horizontal position observations have been rejected +bool reject_ver_pos # 3 - true if vertical position observations have been rejected +bool reject_yaw # 7 - true if the yaw observation has been rejected +bool reject_airspeed # 8 - true if the airspeed observation has been rejected +bool reject_sideslip # 9 - true if the synthetic sideslip observation has been rejected +bool reject_hagl # 10 - true if the height above ground observation has been rejected +bool reject_optflow_x # 11 - true if the X optical flow observation has been rejected +bool reject_optflow_y # 12 - true if the Y optical flow observation has been rejected diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/Event.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/Event.msg new file mode 100644 index 00000000..df1dd4a9 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/Event.msg @@ -0,0 +1,10 @@ +# Events interface +uint64 timestamp # time since system start (microseconds) + +uint32 id # Event ID +uint16 event_sequence # Event sequence number +uint8[25] arguments # (optional) arguments, depend on event id + +uint8 log_levels # Log levels: 4 bits MSB: internal, 4 bits LSB: external + +uint8 ORB_QUEUE_LENGTH = 16 diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/FailsafeFlags.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/FailsafeFlags.msg new file mode 100644 index 00000000..44945afa --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/FailsafeFlags.msg @@ -0,0 +1,59 @@ +# Input flags for the failsafe state machine set by the arming & health checks. +# +# Flags must be named such that false == no failure (e.g. _invalid, _unhealthy, _lost) +# The flag comments are used as label for the failsafe state machine simulation + +uint64 timestamp # time since system start (microseconds) + +# Per-mode requirements +uint32 mode_req_angular_velocity +uint32 mode_req_attitude +uint32 mode_req_local_alt +uint32 mode_req_local_position +uint32 mode_req_local_position_relaxed +uint32 mode_req_global_position +uint32 mode_req_mission +uint32 mode_req_offboard_signal +uint32 mode_req_home_position +uint32 mode_req_wind_and_flight_time_compliance # if set, mode cannot be entered if wind or flight time limit exceeded +uint32 mode_req_prevent_arming # if set, cannot arm while in this mode +uint32 mode_req_manual_control +uint32 mode_req_other # other requirements, not covered above (for external modes) + + +# Mode requirements +bool angular_velocity_invalid # Angular velocity invalid +bool attitude_invalid # Attitude invalid +bool local_altitude_invalid # Local altitude invalid +bool local_position_invalid # Local position estimate invalid +bool local_position_invalid_relaxed # Local position with reduced accuracy requirements invalid (e.g. flying with optical flow) +bool local_velocity_invalid # Local velocity estimate invalid +bool global_position_invalid # Global position estimate invalid +bool auto_mission_missing # No mission available +bool offboard_control_signal_lost # Offboard signal lost +bool home_position_invalid # No home position available + +# Control links +bool manual_control_signal_lost # Manual control (RC) signal lost +bool gcs_connection_lost # GCS connection lost + +# Battery +uint8 battery_warning # Battery warning level +bool battery_low_remaining_time # Low battery based on remaining flight time +bool battery_unhealthy # Battery unhealthy + +# Other +bool geofence_breached # Geofence breached (one or multiple) +bool mission_failure # Mission failure +bool vtol_fixed_wing_system_failure # vehicle in fixed-wing system failure failsafe mode (after quad-chute) +bool wind_limit_exceeded # Wind limit exceeded +bool flight_time_limit_exceeded # Maximum flight time exceeded +bool local_position_accuracy_low # Local position estimate has dropped below threshold, but is currently still declared valid + +# Failure detector +bool fd_critical_failure # Critical failure (attitude/altitude limit exceeded, or external ATS) +bool fd_esc_arming_failure # ESC failed to arm +bool fd_imbalanced_prop # Imbalanced propeller detected +bool fd_motor_failure # Motor failure + + diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/FailureDetectorStatus.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/FailureDetectorStatus.msg new file mode 100644 index 00000000..923ceb36 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/FailureDetectorStatus.msg @@ -0,0 +1,14 @@ +uint64 timestamp # time since system start (microseconds) + +# FailureDetector status +bool fd_roll +bool fd_pitch +bool fd_alt +bool fd_ext +bool fd_arm_escs +bool fd_battery +bool fd_imbalanced_prop +bool fd_motor + +float32 imbalanced_prop_metric # Metric of the imbalanced propeller check (low-passed) +uint16 motor_failure_mask # Bit-mask with motor indices, indicating critical motor failures diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/FigureEightStatus.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/FigureEightStatus.msg new file mode 100644 index 00000000..e14d8f0d --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/FigureEightStatus.msg @@ -0,0 +1,8 @@ +uint64 timestamp # time since system start (microseconds) +float32 major_radius # Major axis radius of the figure eight [m]. Positive values orbit clockwise, negative values orbit counter-clockwise. +float32 minor_radius # Minor axis radius of the figure eight [m]. +float32 orientation # Orientation of the major axis of the figure eight [rad]. +uint8 frame # The coordinate system of the fields: x, y, z. +int32 x # X coordinate of center point. Coordinate system depends on frame field: local = x position in meters * 1e4, global = latitude in degrees * 1e7. +int32 y # Y coordinate of center point. Coordinate system depends on frame field: local = y position in meters * 1e4, global = latitude in degrees * 1e7. +float32 z # Altitude of center point. Coordinate system depends on frame field. diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/FlightPhaseEstimation.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/FlightPhaseEstimation.msg new file mode 100644 index 00000000..e05b3291 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/FlightPhaseEstimation.msg @@ -0,0 +1,8 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 flight_phase # Estimate of current flight phase + +uint8 FLIGHT_PHASE_UNKNOWN = 0 # vehicle flight phase is unknown +uint8 FLIGHT_PHASE_LEVEL = 1 # Vehicle is in level flight +uint8 FLIGHT_PHASE_DESCEND = 2 # vehicle is in descend +uint8 FLIGHT_PHASE_CLIMB = 3 # vehicle is climbing diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/FollowTarget.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/FollowTarget.msg new file mode 100644 index 00000000..e88c2460 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/FollowTarget.msg @@ -0,0 +1,11 @@ +uint64 timestamp # time since system start (microseconds) + +float64 lat # target position (deg * 1e7) +float64 lon # target position (deg * 1e7) +float32 alt # target position + +float32 vy # target vel in y +float32 vx # target vel in x +float32 vz # target vel in z + +uint8 est_cap # target reporting capabilities diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/FollowTargetEstimator.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/FollowTargetEstimator.msg new file mode 100644 index 00000000..9d3df9f6 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/FollowTargetEstimator.msg @@ -0,0 +1,16 @@ +uint64 timestamp # time since system start (microseconds) +uint64 last_filter_reset_timestamp # time of last filter reset (microseconds) + +bool valid # True if estimator states are okay to be used +bool stale # True if estimator stopped receiving follow_target messages for some time. The estimate can still be valid, though it might be inaccurate. + +float64 lat_est # Estimated target latitude +float64 lon_est # Estimated target longitude +float32 alt_est # Estimated target altitude + +float32[3] pos_est # Estimated target NED position (m) +float32[3] vel_est # Estimated target NED velocity (m/s) +float32[3] acc_est # Estimated target NED acceleration (m^2/s) + +uint64 prediction_count +uint64 fusion_count diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/FollowTargetStatus.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/FollowTargetStatus.msg new file mode 100644 index 00000000..713a7dcd --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/FollowTargetStatus.msg @@ -0,0 +1,12 @@ +uint64 timestamp # [microseconds] time since system start + +float32 tracked_target_course # [rad] Tracked target course in NED local frame (North is course zero) +float32 follow_angle # [rad] Current follow angle setting + +float32 orbit_angle_setpoint # [rad] Current orbit angle setpoint from the smooth trajectory generator +float32 angular_rate_setpoint # [rad/s] Angular rate commanded from Jerk-limited Orbit Angle trajectory for Orbit Angle + +float32[3] desired_position_raw # [m] Raw 'idealistic' desired drone position if a drone could teleport from place to places + +bool in_emergency_ascent # [bool] True when doing emergency ascent (when distance to ground is below safety altitude) +float32 gimbal_pitch # [rad] Gimbal pitch commanded to track target in the center of the frame diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/FuelTankStatus.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/FuelTankStatus.msg new file mode 100644 index 00000000..22d21e4a --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/FuelTankStatus.msg @@ -0,0 +1,17 @@ +uint64 timestamp # time since system start (microseconds) + +float32 maximum_fuel_capacity # maximum fuel capacity. Must always be provided, either from the driver or a parameter +float32 consumed_fuel # consumed fuel, NaN if not measured. Should not be inferred from the max fuel capacity +float32 fuel_consumption_rate # fuel consumption rate, NaN if not measured + +uint8 percent_remaining # percentage of remaining fuel, UINT8_MAX if not provided +float32 remaining_fuel # remaining fuel, NaN if not measured. Should not be inferred from the max fuel capacity + +uint8 fuel_tank_id # identifier for the fuel tank. Must match ID of other messages for same fuel system. 0 by default when only a single tank exists + +uint32 fuel_type # type of fuel based on MAV_FUEL_TYPE enum. Set to MAV_FUEL_TYPE_UNKNOWN if unknown or it does not fit the provided types +uint8 MAV_FUEL_TYPE_UNKNOWN = 0 # fuel type not specified. Fuel levels are normalized (i.e., maximum is 1, and other levels are relative to 1). +uint8 MAV_FUEL_TYPE_LIQUID = 1 # represents generic liquid fuels, such as gasoline or diesel. Fuel levels are measured in millilitres (ml), and flow rates in millilitres per second (ml/s). +uint8 MAV_FUEL_TYPE_GAS = 2 # represents a gas fuel, such as hydrogen, methane, or propane. Fuel levels are in kilo-Pascal (kPa), and flow rates are in milliliters per second (ml/s). + +float32 temperature # fuel temperature in Kelvin, NaN if not measured diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GeneratorStatus.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GeneratorStatus.msg new file mode 100644 index 00000000..7ba9b402 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GeneratorStatus.msg @@ -0,0 +1,44 @@ +uint64 timestamp # time since system start (microseconds) + + +uint64 STATUS_FLAG_OFF = 1 # Generator is off. +uint64 STATUS_FLAG_READY = 2 # Generator is ready to start generating power. +uint64 STATUS_FLAG_GENERATING = 4 # Generator is generating power. +uint64 STATUS_FLAG_CHARGING = 8 # Generator is charging the batteries (generating enough power to charge and provide the load). +uint64 STATUS_FLAG_REDUCED_POWER = 16 # Generator is operating at a reduced maximum power. +uint64 STATUS_FLAG_MAXPOWER = 32 # Generator is providing the maximum output. +uint64 STATUS_FLAG_OVERTEMP_WARNING = 64 # Generator is near the maximum operating temperature, cooling is insufficient. +uint64 STATUS_FLAG_OVERTEMP_FAULT = 128 # Generator hit the maximum operating temperature and shutdown. +uint64 STATUS_FLAG_ELECTRONICS_OVERTEMP_WARNING = 256 # Power electronics are near the maximum operating temperature, cooling is insufficient. +uint64 STATUS_FLAG_ELECTRONICS_OVERTEMP_FAULT = 512 # Power electronics hit the maximum operating temperature and shutdown. +uint64 STATUS_FLAG_ELECTRONICS_FAULT = 1024 # Power electronics experienced a fault and shutdown. +uint64 STATUS_FLAG_POWERSOURCE_FAULT = 2048 # The power source supplying the generator failed e.g. mechanical generator stopped, tether is no longer providing power, solar cell is in shade, hydrogen reaction no longer happening. +uint64 STATUS_FLAG_COMMUNICATION_WARNING = 4096 # Generator controller having communication problems. +uint64 STATUS_FLAG_COOLING_WARNING = 8192 # Power electronic or generator cooling system error. +uint64 STATUS_FLAG_POWER_RAIL_FAULT = 16384 # Generator controller power rail experienced a fault. +uint64 STATUS_FLAG_OVERCURRENT_FAULT = 32768 # Generator controller exceeded the overcurrent threshold and shutdown to prevent damage. +uint64 STATUS_FLAG_BATTERY_OVERCHARGE_CURRENT_FAULT = 65536 # Generator controller detected a high current going into the batteries and shutdown to prevent battery damage. | +uint64 STATUS_FLAG_OVERVOLTAGE_FAULT = 131072 # Generator controller exceeded it's overvoltage threshold and shutdown to prevent it exceeding the voltage rating. +uint64 STATUS_FLAG_BATTERY_UNDERVOLT_FAULT = 262144 # Batteries are under voltage (generator will not start). +uint64 STATUS_FLAG_START_INHIBITED = 524288 # Generator start is inhibited by e.g. a safety switch. +uint64 STATUS_FLAG_MAINTENANCE_REQUIRED = 1048576 # Generator requires maintenance. +uint64 STATUS_FLAG_WARMING_UP = 2097152 # Generator is not ready to generate yet. +uint64 STATUS_FLAG_IDLE = 4194304 # Generator is idle. + +uint64 status # Status flags + + +float32 battery_current # [A] Current into/out of battery. Positive for out. Negative for in. NaN: field not provided. +float32 load_current # [A] Current going to the UAV. If battery current not available this is the DC current from the generator. Positive for out. Negative for in. NaN: field not provided +float32 power_generated # [W] The power being generated. NaN: field not provided +float32 bus_voltage # [V] Voltage of the bus seen at the generator, or battery bus if battery bus is controlled by generator and at a different voltage to main bus. +float32 bat_current_setpoint # [A] The target battery current. Positive for out. Negative for in. NaN: field not provided + +uint32 runtime # [s] Seconds this generator has run since it was rebooted. UINT32_MAX: field not provided. + +int32 time_until_maintenance # [s] Seconds until this generator requires maintenance. A negative value indicates maintenance is past-due. INT32_MAX: field not provided. + +uint16 generator_speed # [rpm] Speed of electrical generator or alternator. UINT16_MAX: field not provided. + +int16 rectifier_temperature # [degC] The temperature of the rectifier or power converter. INT16_MAX: field not provided. +int16 generator_temperature # [degC] The temperature of the mechanical motor, fuel cell core or generator. INT16_MAX: field not provided. diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GeofenceResult.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GeofenceResult.msg new file mode 100644 index 00000000..7782d1d6 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GeofenceResult.msg @@ -0,0 +1,13 @@ +uint64 timestamp # time since system start (microseconds) +uint8 GF_ACTION_NONE = 0 # no action on geofence violation +uint8 GF_ACTION_WARN = 1 # critical mavlink message +uint8 GF_ACTION_LOITER = 2 # switch to AUTO|LOITER +uint8 GF_ACTION_RTL = 3 # switch to AUTO|RTL +uint8 GF_ACTION_TERMINATE = 4 # flight termination +uint8 GF_ACTION_LAND = 5 # switch to AUTO|LAND + +bool geofence_max_dist_triggered # true the check for max distance from Home is triggered +bool geofence_max_alt_triggered # true the check for max altitude above Home is triggered +bool geofence_custom_fence_triggered # true the check for custom inclusion/exclusion geofence(s) is triggered + +uint8 geofence_action # action to take when the geofence is breached diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GeofenceStatus.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GeofenceStatus.msg new file mode 100644 index 00000000..d32b9010 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GeofenceStatus.msg @@ -0,0 +1,7 @@ +uint64 timestamp # time since system start (microseconds) + +uint32 geofence_id # loaded geofence id +uint8 status # Current geofence status + +uint8 GF_STATUS_LOADING = 0 +uint8 GF_STATUS_READY = 1 diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GimbalControls.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GimbalControls.msg new file mode 100644 index 00000000..3e1c5a9d --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GimbalControls.msg @@ -0,0 +1,7 @@ +uint64 timestamp # time since system start (microseconds) +uint8 INDEX_ROLL = 0 +uint8 INDEX_PITCH = 1 +uint8 INDEX_YAW = 2 + +uint64 timestamp_sample # the timestamp the data this control response is based on was sampled +float32[3] control diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GimbalDeviceAttitudeStatus.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GimbalDeviceAttitudeStatus.msg new file mode 100644 index 00000000..0be66bab --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GimbalDeviceAttitudeStatus.msg @@ -0,0 +1,20 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 target_system +uint8 target_component +uint16 device_flags + +uint16 DEVICE_FLAGS_RETRACT = 1 +uint16 DEVICE_FLAGS_NEUTRAL = 2 +uint16 DEVICE_FLAGS_ROLL_LOCK = 4 +uint16 DEVICE_FLAGS_PITCH_LOCK = 8 +uint16 DEVICE_FLAGS_YAW_LOCK = 16 + +float32[4] q +float32 angular_velocity_x +float32 angular_velocity_y +float32 angular_velocity_z + +uint32 failure_flags + +bool received_from_mavlink diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GimbalDeviceInformation.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GimbalDeviceInformation.msg new file mode 100644 index 00000000..8f7a4164 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GimbalDeviceInformation.msg @@ -0,0 +1,36 @@ +uint64 timestamp # time since system start (microseconds) + +uint8[32] vendor_name +uint8[32] model_name +uint8[32] custom_name +uint32 firmware_version +uint32 hardware_version +uint64 uid + +uint16 cap_flags + +uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_RETRACT = 1 +uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_NEUTRAL = 2 +uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_AXIS = 4 +uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_FOLLOW = 8 +uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_ROLL_LOCK = 16 +uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_AXIS = 32 +uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_FOLLOW = 64 +uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_PITCH_LOCK = 128 +uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_AXIS = 256 +uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_FOLLOW = 512 +uint32 GIMBAL_DEVICE_CAP_FLAGS_HAS_YAW_LOCK = 1024 +uint32 GIMBAL_DEVICE_CAP_FLAGS_SUPPORTS_INFINITE_YAW = 2048 + +uint16 custom_cap_flags + +float32 roll_min # [rad] +float32 roll_max # [rad] + +float32 pitch_min # [rad] +float32 pitch_max # [rad] + +float32 yaw_min # [rad] +float32 yaw_max # [rad] + +uint8 gimbal_device_compid diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GimbalDeviceSetAttitude.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GimbalDeviceSetAttitude.msg new file mode 100644 index 00000000..f224a234 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GimbalDeviceSetAttitude.msg @@ -0,0 +1,17 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 target_system +uint8 target_component + +uint16 flags +uint32 GIMBAL_DEVICE_FLAGS_RETRACT = 1 +uint32 GIMBAL_DEVICE_FLAGS_NEUTRAL = 2 +uint32 GIMBAL_DEVICE_FLAGS_ROLL_LOCK = 4 +uint32 GIMBAL_DEVICE_FLAGS_PITCH_LOCK = 8 +uint32 GIMBAL_DEVICE_FLAGS_YAW_LOCK = 16 + +float32[4] q + +float32 angular_velocity_x +float32 angular_velocity_y +float32 angular_velocity_z diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GimbalManagerInformation.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GimbalManagerInformation.msg new file mode 100644 index 00000000..28db68a4 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GimbalManagerInformation.msg @@ -0,0 +1,29 @@ +uint64 timestamp # time since system start (microseconds) + +uint32 cap_flags + +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_RETRACT = 1 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_NEUTRAL = 2 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_ROLL_AXIS = 4 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_ROLL_FOLLOW = 8 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_ROLL_LOCK = 16 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_AXIS = 32 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_FOLLOW = 64 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_PITCH_LOCK = 128 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_AXIS = 256 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_FOLLOW = 512 +uint32 GIMBAL_MANAGER_CAP_FLAGS_HAS_YAW_LOCK = 1024 +uint32 GIMBAL_MANAGER_CAP_FLAGS_SUPPORTS_INFINITE_YAW = 2048 +uint32 GIMBAL_MANAGER_CAP_FLAGS_CAN_POINT_LOCATION_LOCAL = 65536 +uint32 GIMBAL_MANAGER_CAP_FLAGS_CAN_POINT_LOCATION_GLOBAL = 131072 + +uint8 gimbal_device_id + +float32 roll_min # [rad] +float32 roll_max # [rad] + +float32 pitch_min # [rad] +float32 pitch_max # [rad] + +float32 yaw_min # [rad] +float32 yaw_max # [rad] diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GimbalManagerSetAttitude.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GimbalManagerSetAttitude.msg new file mode 100644 index 00000000..d88acca8 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GimbalManagerSetAttitude.msg @@ -0,0 +1,22 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 origin_sysid +uint8 origin_compid + +uint8 target_system +uint8 target_component + +uint32 GIMBAL_MANAGER_FLAGS_RETRACT = 1 +uint32 GIMBAL_MANAGER_FLAGS_NEUTRAL = 2 +uint32 GIMBAL_MANAGER_FLAGS_ROLL_LOCK = 4 +uint32 GIMBAL_MANAGER_FLAGS_PITCH_LOCK = 8 +uint32 GIMBAL_MANAGER_FLAGS_YAW_LOCK = 16 + +uint32 flags +uint8 gimbal_device_id + +float32[4] q + +float32 angular_velocity_x +float32 angular_velocity_y +float32 angular_velocity_z diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GimbalManagerSetManualControl.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GimbalManagerSetManualControl.msg new file mode 100644 index 00000000..4061438f --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GimbalManagerSetManualControl.msg @@ -0,0 +1,21 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 origin_sysid +uint8 origin_compid + +uint8 target_system +uint8 target_component + +uint32 GIMBAL_MANAGER_FLAGS_RETRACT = 1 +uint32 GIMBAL_MANAGER_FLAGS_NEUTRAL = 2 +uint32 GIMBAL_MANAGER_FLAGS_ROLL_LOCK = 4 +uint32 GIMBAL_MANAGER_FLAGS_PITCH_LOCK = 8 +uint32 GIMBAL_MANAGER_FLAGS_YAW_LOCK = 16 + +uint32 flags +uint8 gimbal_device_id + +float32 pitch # unitless -1..1, can be NAN +float32 yaw # unitless -1..1, can be NAN +float32 pitch_rate # unitless -1..1, can be NAN +float32 yaw_rate # unitless -1..1, can be NAN diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GimbalManagerStatus.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GimbalManagerStatus.msg new file mode 100644 index 00000000..002e5c90 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GimbalManagerStatus.msg @@ -0,0 +1,8 @@ +uint64 timestamp # time since system start (microseconds) + +uint32 flags +uint8 gimbal_device_id +uint8 primary_control_sysid +uint8 primary_control_compid +uint8 secondary_control_sysid +uint8 secondary_control_compid diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GotoSetpoint.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GotoSetpoint.msg new file mode 100644 index 00000000..5fe3ab8a --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GotoSetpoint.msg @@ -0,0 +1,24 @@ +# Position and (optional) heading setpoints with corresponding speed constraints +# Setpoints are intended as inputs to position and heading smoothers, respectively +# Setpoints do not need to be kinematically consistent +# Optional heading setpoints may be specified as controlled by the respective flag +# Unset optional setpoints are not controlled +# Unset optional constraints default to vehicle specifications + +uint64 timestamp # time since system start (microseconds) + +# setpoints +float32[3] position # [m] NED local world frame + +bool flag_control_heading # true if heading is to be controlled +float32 heading # (optional) [rad] [-pi,pi] from North + +# constraints +bool flag_set_max_horizontal_speed # true if setting a non-default horizontal speed limit +float32 max_horizontal_speed # (optional) [m/s] maximum speed (absolute) in the NE-plane + +bool flag_set_max_vertical_speed # true if setting a non-default vertical speed limit +float32 max_vertical_speed # (optional) [m/s] maximum speed (absolute) in the D-axis + +bool flag_set_max_heading_rate # true if setting a non-default heading rate limit +float32 max_heading_rate # (optional) [rad/s] maximum heading rate (absolute) diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GpioConfig.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GpioConfig.msg new file mode 100644 index 00000000..0ff393ec --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GpioConfig.msg @@ -0,0 +1,28 @@ +# GPIO configuration + +uint64 timestamp # time since system start (microseconds) +uint32 device_id # Device id + +uint32 mask # Pin mask +uint32 state # Initial pin output state + +# Configuration Mask +# Bit 0-3: Direction: 0=Input, 1=Output +# Bit 4-7: Input Config: 0=Floating, 1=PullUp, 2=PullDown +# Bit 8-12: Output Config: 0=PushPull, 1=OpenDrain +# Bit 13-31: Reserved +uint32 INPUT = 0 # 0x0000 +uint32 OUTPUT = 1 # 0x0001 +uint32 PULLUP = 16 # 0x0010 +uint32 PULLDOWN = 32 # 0x0020 +uint32 OPENDRAIN = 256 # 0x0100 + +uint32 INPUT_FLOATING = 0 # 0x0000 +uint32 INPUT_PULLUP = 16 # 0x0010 +uint32 INPUT_PULLDOWN = 32 # 0x0020 + +uint32 OUTPUT_PUSHPULL = 0 # 0x0000 +uint32 OUTPUT_OPENDRAIN = 256 # 0x0100 +uint32 OUTPUT_OPENDRAIN_PULLUP = 272 # 0x0110 + +uint32 config diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GpioIn.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GpioIn.msg new file mode 100644 index 00000000..0482a218 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GpioIn.msg @@ -0,0 +1,6 @@ +# GPIO mask and state + +uint64 timestamp # time since system start (microseconds) +uint32 device_id # Device id + +uint32 state # pin state mask diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GpioOut.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GpioOut.msg new file mode 100644 index 00000000..3865bbf2 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GpioOut.msg @@ -0,0 +1,7 @@ +# GPIO mask and state + +uint64 timestamp # time since system start (microseconds) +uint32 device_id # Device id + +uint32 mask # pin mask +uint32 state # pin state mask diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GpioRequest.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GpioRequest.msg new file mode 100644 index 00000000..3328b001 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GpioRequest.msg @@ -0,0 +1,4 @@ +# Request GPIO mask to be read + +uint64 timestamp # time since system start (microseconds) +uint32 device_id # Device id diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GpsDump.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GpsDump.msg new file mode 100644 index 00000000..2477bcfa --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GpsDump.msg @@ -0,0 +1,10 @@ +# This message is used to dump the raw gps communication to the log. + +uint64 timestamp # time since system start (microseconds) + +uint8 instance # Instance of GNSS receiver +uint8 len # length of data, MSB bit set = message to the gps device, + # clear = message from the device +uint8[79] data # data to write to the log + +uint8 ORB_QUEUE_LENGTH = 8 diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GpsInjectData.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GpsInjectData.msg new file mode 100644 index 00000000..516d5cb5 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/GpsInjectData.msg @@ -0,0 +1,11 @@ +uint64 timestamp # time since system start (microseconds) + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +uint16 len # length of data +uint8 flags # LSB: 1=fragmented +uint8[300] data # data to write to GPS device (RTCM message) + +uint8 ORB_QUEUE_LENGTH = 8 + +uint8 MAX_INSTANCES = 2 diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/Gripper.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/Gripper.msg new file mode 100644 index 00000000..4f1445cb --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/Gripper.msg @@ -0,0 +1,7 @@ +## Used to command an actuation in the gripper, which is mapped to a specific output in the control allocation module + +uint64 timestamp + +int8 command # Commanded state for the gripper +int8 COMMAND_GRAB = 0 +int8 COMMAND_RELEASE = 1 diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/HealthReport.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/HealthReport.msg new file mode 100644 index 00000000..18951805 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/HealthReport.msg @@ -0,0 +1,12 @@ +uint64 timestamp # time since system start (microseconds) + +uint64 can_arm_mode_flags # bitfield for each flight mode (NAVIGATION_STATE_*) if arming is possible +uint64 can_run_mode_flags # bitfield for each flight mode if it can run + +uint64 health_is_present_flags # flags for each health_component_t +uint64 health_warning_flags +uint64 health_error_flags +# A component is required but missing, if present==0 and error==1 + +uint64 arming_check_warning_flags +uint64 arming_check_error_flags diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/HeaterStatus.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/HeaterStatus.msg new file mode 100644 index 00000000..44207a39 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/HeaterStatus.msg @@ -0,0 +1,20 @@ +uint64 timestamp # time since system start (microseconds) + +uint32 device_id + +bool heater_on +bool temperature_target_met + +float32 temperature_sensor +float32 temperature_target + +uint32 controller_period_usec +uint32 controller_time_on_usec + +float32 proportional_value +float32 integrator_value +float32 feed_forward_value + +uint8 MODE_GPIO = 1 +uint8 MODE_PX4IO = 2 +uint8 mode diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/HomePosition.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/HomePosition.msg new file mode 100644 index 00000000..e6a51728 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/HomePosition.msg @@ -0,0 +1,21 @@ +# GPS home position in WGS84 coordinates. + +uint64 timestamp # time since system start (microseconds) + +float64 lat # Latitude in degrees +float64 lon # Longitude in degrees +float32 alt # Altitude in meters (AMSL) + +float32 x # X coordinate in meters +float32 y # Y coordinate in meters +float32 z # Z coordinate in meters + +float32 yaw # Yaw angle in radians + +bool valid_alt # true when the altitude has been set +bool valid_hpos # true when the latitude and longitude have been set +bool valid_lpos # true when the local position (xyz) has been set + +bool manual_home # true when home position was set manually + +uint32 update_count # update counter of the home position diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/HoverThrustEstimate.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/HoverThrustEstimate.msg new file mode 100644 index 00000000..a38d9042 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/HoverThrustEstimate.msg @@ -0,0 +1,13 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # time of corresponding sensor data last used for this estimate + +float32 hover_thrust # estimated hover thrust [0.1, 0.9] +float32 hover_thrust_var # estimated hover thrust variance + +float32 accel_innov # innovation of the last acceleration fusion +float32 accel_innov_var # innovation variance of the last acceleration fusion +float32 accel_innov_test_ratio # normalized innovation squared test ratio + +float32 accel_noise_var # vertical acceleration noise variance estimated form innovation residual + +bool valid diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/InputRc.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/InputRc.msg new file mode 100644 index 00000000..db4b3de2 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/InputRc.msg @@ -0,0 +1,40 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 RC_INPUT_SOURCE_UNKNOWN = 0 +uint8 RC_INPUT_SOURCE_PX4FMU_PPM = 1 +uint8 RC_INPUT_SOURCE_PX4IO_PPM = 2 +uint8 RC_INPUT_SOURCE_PX4IO_SPEKTRUM = 3 +uint8 RC_INPUT_SOURCE_PX4IO_SBUS = 4 +uint8 RC_INPUT_SOURCE_PX4IO_ST24 = 5 +uint8 RC_INPUT_SOURCE_MAVLINK = 6 +uint8 RC_INPUT_SOURCE_QURT = 7 +uint8 RC_INPUT_SOURCE_PX4FMU_SPEKTRUM = 8 +uint8 RC_INPUT_SOURCE_PX4FMU_SBUS = 9 +uint8 RC_INPUT_SOURCE_PX4FMU_ST24 = 10 +uint8 RC_INPUT_SOURCE_PX4FMU_SUMD = 11 +uint8 RC_INPUT_SOURCE_PX4FMU_DSM = 12 +uint8 RC_INPUT_SOURCE_PX4IO_SUMD = 13 +uint8 RC_INPUT_SOURCE_PX4FMU_CRSF = 14 +uint8 RC_INPUT_SOURCE_PX4FMU_GHST = 15 + +uint8 RC_INPUT_MAX_CHANNELS = 18 # Maximum number of R/C input channels in the system. S.Bus has up to 18 channels. + +uint64 timestamp_last_signal # last valid reception time + +uint8 channel_count # number of channels actually being seen + +int8 RSSI_MAX = 100 +int32 rssi # receive signal strength indicator (RSSI): < 0: Undefined, 0: no signal, 100: full reception + +bool rc_failsafe # explicit failsafe flag: true on TX failure or TX out of range , false otherwise. Only the true state is reliable, as there are some (PPM) receivers on the market going into failsafe without telling us explicitly. +bool rc_lost # RC receiver connection status: True,if no frame has arrived in the expected time, false otherwise. True usually means that the receiver has been disconnected, but can also indicate a radio link loss on "stupid" systems. Will remain false, if a RX with failsafe option continues to transmit frames after a link loss. + +uint16 rc_lost_frame_count # Number of lost RC frames. Note: intended purpose: observe the radio link quality if RSSI is not available. This value must not be used to trigger any failsafe-alike functionality. +uint16 rc_total_frame_count # Number of total RC frames. Note: intended purpose: observe the radio link quality if RSSI is not available. This value must not be used to trigger any failsafe-alike functionality. +uint16 rc_ppm_frame_length # Length of a single PPM frame. Zero for non-PPM systems + +uint8 input_source # Input source +uint16[18] values # measured pulse widths for each of the supported channels + +int8 link_quality # link quality. Percentage 0-100%. -1 = invalid +float32 rssi_dbm # Actual rssi in units of dBm. NaN = invalid \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/InternalCombustionEngineStatus.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/InternalCombustionEngineStatus.msg new file mode 100644 index 00000000..301eb92a --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/InternalCombustionEngineStatus.msg @@ -0,0 +1,64 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 STATE_STOPPED = 0 # The engine is not running. This is the default state. +uint8 STATE_STARTING = 1 # The engine is starting. This is a transient state. +uint8 STATE_RUNNING = 2 # The engine is running normally. +uint8 STATE_FAULT = 3 # The engine can no longer function. +uint8 state + +uint32 FLAG_GENERAL_ERROR = 1 # General error. + +uint32 FLAG_CRANKSHAFT_SENSOR_ERROR_SUPPORTED = 2 # Error of the crankshaft sensor. This flag is optional. +uint32 FLAG_CRANKSHAFT_SENSOR_ERROR = 4 + +uint32 FLAG_TEMPERATURE_SUPPORTED = 8 # Temperature levels. These flags are optional +uint32 FLAG_TEMPERATURE_BELOW_NOMINAL = 16 # Under-temperature warning +uint32 FLAG_TEMPERATURE_ABOVE_NOMINAL = 32 # Over-temperature warning +uint32 FLAG_TEMPERATURE_OVERHEATING = 64 # Critical overheating +uint32 FLAG_TEMPERATURE_EGT_ABOVE_NOMINAL = 128 # Exhaust gas over-temperature warning + +uint32 FLAG_FUEL_PRESSURE_SUPPORTED = 256 # Fuel pressure. These flags are optional +uint32 FLAG_FUEL_PRESSURE_BELOW_NOMINAL = 512 # Under-pressure warning +uint32 FLAG_FUEL_PRESSURE_ABOVE_NOMINAL = 1024 # Over-pressure warning + +uint32 FLAG_DETONATION_SUPPORTED = 2048 # Detonation warning. This flag is optional. +uint32 FLAG_DETONATION_OBSERVED = 4096 # Detonation condition observed warning + +uint32 FLAG_MISFIRE_SUPPORTED = 8192 # Misfire warning. This flag is optional. +uint32 FLAG_MISFIRE_OBSERVED = 16384 # Misfire condition observed warning + +uint32 FLAG_OIL_PRESSURE_SUPPORTED = 32768 # Oil pressure. These flags are optional +uint32 FLAG_OIL_PRESSURE_BELOW_NOMINAL = 65536 # Under-pressure warning +uint32 FLAG_OIL_PRESSURE_ABOVE_NOMINAL = 131072 # Over-pressure warning + +uint32 FLAG_DEBRIS_SUPPORTED = 262144 # Debris warning. This flag is optional +uint32 FLAG_DEBRIS_DETECTED = 524288 # Detection of debris warning +uint32 flags + +uint8 engine_load_percent # Engine load estimate, percent, [0, 127] +uint32 engine_speed_rpm # Engine speed, revolutions per minute +float32 spark_dwell_time_ms # Spark dwell time, millisecond +float32 atmospheric_pressure_kpa # Atmospheric (barometric) pressure, kilopascal +float32 intake_manifold_pressure_kpa # Engine intake manifold pressure, kilopascal +float32 intake_manifold_temperature # Engine intake manifold temperature, kelvin +float32 coolant_temperature # Engine coolant temperature, kelvin +float32 oil_pressure # Oil pressure, kilopascal +float32 oil_temperature # Oil temperature, kelvin +float32 fuel_pressure # Fuel pressure, kilopascal +float32 fuel_consumption_rate_cm3pm # Instant fuel consumption estimate, (centimeter^3)/minute +float32 estimated_consumed_fuel_volume_cm3 # Estimate of the consumed fuel since the start of the engine, centimeter^3 +uint8 throttle_position_percent # Throttle position, percent +uint8 ecu_index # The index of the publishing ECU + + +uint8 SPARK_PLUG_SINGLE = 0 +uint8 SPARK_PLUG_FIRST_ACTIVE = 1 +uint8 SPARK_PLUG_SECOND_ACTIVE = 2 +uint8 SPARK_PLUG_BOTH_ACTIVE = 3 +uint8 spark_plug_usage # Spark plug activity report. + +float32 ignition_timing_deg # Cylinder ignition timing, angular degrees of the crankshaft +float32 injection_time_ms # Fuel injection time, millisecond +float32 cylinder_head_temperature # Cylinder head temperature (CHT), kelvin +float32 exhaust_gas_temperature # Exhaust gas temperature (EGT), kelvin +float32 lambda_coefficient # Estimated lambda coefficient, dimensionless ratio diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/IridiumsbdStatus.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/IridiumsbdStatus.msg new file mode 100644 index 00000000..436654e4 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/IridiumsbdStatus.msg @@ -0,0 +1,15 @@ +uint64 timestamp # time since system start (microseconds) +uint64 last_at_ok_timestamp # timestamp of the last "OK" received after the "AT" command +uint16 tx_buf_write_index # current size of the tx buffer +uint16 rx_buf_read_index # the rx buffer is parsed up to that index +uint16 rx_buf_end_index # current size of the rx buffer +uint16 failed_sbd_sessions # number of failed sbd sessions +uint16 successful_sbd_sessions # number of successful sbd sessions +uint16 num_tx_buf_reset # number of times the tx buffer was reset +uint8 signal_quality # current signal quality, 0 is no signal, 5 the best +uint8 state # current state of the driver, see the satcom_state of IridiumSBD.h for the definition +bool ring_pending # indicates if a ring call is pending +bool tx_buf_write_pending # indicates if a tx buffer write is pending +bool tx_session_pending # indicates if a tx session is pending +bool rx_read_pending # indicates if a rx read is pending +bool rx_session_pending # indicates if a rx session is pending diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/IrlockReport.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/IrlockReport.msg new file mode 100644 index 00000000..9f23cbf3 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/IrlockReport.msg @@ -0,0 +1,11 @@ +# IRLOCK_REPORT message data + +uint64 timestamp # time since system start (microseconds) + +uint16 signature + +# When looking along the optical axis of the camera, x points right, y points down, and z points along the optical axis. +float32 pos_x # tan(theta), where theta is the angle between the target and the camera center of projection in camera x-axis +float32 pos_y # tan(theta), where theta is the angle between the target and the camera center of projection in camera y-axis +float32 size_x #/** size of target along camera x-axis in units of tan(theta) **/ +float32 size_y #/** size of target along camera y-axis in units of tan(theta) **/ diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/LandingGear.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/LandingGear.msg new file mode 100644 index 00000000..5ef9ee52 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/LandingGear.msg @@ -0,0 +1,7 @@ +uint64 timestamp # time since system start (microseconds) + +int8 GEAR_UP = 1 # landing gear up +int8 GEAR_DOWN = -1 # landing gear down +int8 GEAR_KEEP = 0 # keep the current state + +int8 landing_gear diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/LandingGearWheel.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/LandingGearWheel.msg new file mode 100644 index 00000000..2ff99fcc --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/LandingGearWheel.msg @@ -0,0 +1,3 @@ +uint64 timestamp # time since system start (microseconds) + +float32 normalized_wheel_setpoint # negative is turning left, positive turning right [-1, 1] diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/LandingTargetInnovations.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/LandingTargetInnovations.msg new file mode 100644 index 00000000..5dd892c5 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/LandingTargetInnovations.msg @@ -0,0 +1,8 @@ +uint64 timestamp # time since system start (microseconds) +# Innovation of landing target position estimator +float32 innov_x +float32 innov_y + +# Innovation covariance of landing target position estimator +float32 innov_cov_x +float32 innov_cov_y diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/LandingTargetPose.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/LandingTargetPose.msg new file mode 100644 index 00000000..875920f1 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/LandingTargetPose.msg @@ -0,0 +1,26 @@ +# Relative position of precision land target in navigation (body fixed, north aligned, NED) and inertial (world fixed, north aligned, NED) frames + +uint64 timestamp # time since system start (microseconds) + +bool is_static # Flag indicating whether the landing target is static or moving with respect to the ground + +bool rel_pos_valid # Flag showing whether relative position is valid +bool rel_vel_valid # Flag showing whether relative velocity is valid + +float32 x_rel # X/north position of target, relative to vehicle (navigation frame) [meters] +float32 y_rel # Y/east position of target, relative to vehicle (navigation frame) [meters] +float32 z_rel # Z/down position of target, relative to vehicle (navigation frame) [meters] + +float32 vx_rel # X/north velocity of target, relative to vehicle (navigation frame) [meters/second] +float32 vy_rel # Y/east velocity of target, relative to vehicle (navigation frame) [meters/second] + +float32 cov_x_rel # X/north position variance [meters^2] +float32 cov_y_rel # Y/east position variance [meters^2] + +float32 cov_vx_rel # X/north velocity variance [(meters/second)^2] +float32 cov_vy_rel # Y/east velocity variance [(meters/second)^2] + +bool abs_pos_valid # Flag showing whether absolute position is valid +float32 x_abs # X/north position of target, relative to origin (navigation frame) [meters] +float32 y_abs # Y/east position of target, relative to origin (navigation frame) [meters] +float32 z_abs # Z/down position of target, relative to origin (navigation frame) [meters] diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/LaunchDetectionStatus.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/LaunchDetectionStatus.msg new file mode 100644 index 00000000..6917f4bc --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/LaunchDetectionStatus.msg @@ -0,0 +1,9 @@ +# Status of the launch detection state machine (fixed-wing only) + +uint64 timestamp # time since system start (microseconds) + +uint8 STATE_WAITING_FOR_LAUNCH = 0 # waiting for launch +uint8 STATE_LAUNCH_DETECTED_DISABLED_MOTOR = 1 # launch detected, but keep motor(s) disabled (e.g. because it can't spin freely while on catapult) +uint8 STATE_FLYING = 2 # launch detected, use normal takeoff/flying configuration + +uint8 launch_detection_state diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/LedControl.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/LedControl.msg new file mode 100644 index 00000000..4be5cc1c --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/LedControl.msg @@ -0,0 +1,37 @@ +# LED control: control a single or multiple LED's. +# These are the externally visible LED's, not the board LED's + +uint64 timestamp # time since system start (microseconds) + +# colors +uint8 COLOR_OFF = 0 # this is only used in the drivers +uint8 COLOR_RED = 1 +uint8 COLOR_GREEN = 2 +uint8 COLOR_BLUE = 3 +uint8 COLOR_YELLOW = 4 +uint8 COLOR_PURPLE = 5 +uint8 COLOR_AMBER = 6 +uint8 COLOR_CYAN = 7 +uint8 COLOR_WHITE = 8 + +# LED modes definitions +uint8 MODE_OFF = 0 # turn LED off +uint8 MODE_ON = 1 # turn LED on +uint8 MODE_DISABLED = 2 # disable this priority (switch to lower priority setting) +uint8 MODE_BLINK_SLOW = 3 +uint8 MODE_BLINK_NORMAL = 4 +uint8 MODE_BLINK_FAST = 5 +uint8 MODE_BREATHE = 6 # continuously increase & decrease brightness (solid color if driver does not support it) +uint8 MODE_FLASH = 7 # two fast blinks (on/off) with timing as in MODE_BLINK_FAST and then off for a while + +uint8 MAX_PRIORITY = 2 # maximum priority (minimum is 0) + + +uint8 led_mask # bitmask which LED(s) to control, set to 0xff for all +uint8 color # see COLOR_* +uint8 mode # see MODE_* +uint8 num_blinks # how many times to blink (number of on-off cycles if mode is one of MODE_BLINK_*) . Set to 0 for infinite + # in MODE_FLASH it is the number of cycles. Max number of blinks: 122 and max number of flash cycles: 20 +uint8 priority # priority: higher priority events will override current lower priority events (see MAX_PRIORITY) + +uint8 ORB_QUEUE_LENGTH = 8 # needs to match BOARD_MAX_LEDS diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/LogMessage.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/LogMessage.msg new file mode 100644 index 00000000..afb690b1 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/LogMessage.msg @@ -0,0 +1,8 @@ +# A logging message, output with PX4_WARN, PX4_ERR, PX4_INFO + +uint64 timestamp # time since system start (microseconds) + +uint8 severity # log level (same as in the linux kernel, starting with 0) +char[127] text + +uint8 ORB_QUEUE_LENGTH = 4 diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/LoggerStatus.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/LoggerStatus.msg new file mode 100644 index 00000000..c67c8895 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/LoggerStatus.msg @@ -0,0 +1,23 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 LOGGER_TYPE_FULL = 0 # Normal, full size log +uint8 LOGGER_TYPE_MISSION = 1 # reduced mission log (e.g. for geotagging) +uint8 type + +uint8 BACKEND_FILE = 1 +uint8 BACKEND_MAVLINK = 2 +uint8 BACKEND_ALL = 3 +uint8 backend + +bool is_logging + +float32 total_written_kb # total written to log in kiloBytes +float32 write_rate_kb_s # write rate in kiloBytes/s + +uint32 dropouts # number of failed buffer writes due to buffer overflow +uint32 message_gaps # messages misssed + +uint32 buffer_used_bytes # current buffer fill in Bytes +uint32 buffer_size_bytes # total buffer size in Bytes + +uint8 num_messages diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/MagWorkerData.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/MagWorkerData.msg new file mode 100644 index 00000000..09626e8a --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/MagWorkerData.msg @@ -0,0 +1,13 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample + +uint8 MAX_MAGS = 4 + +uint32 done_count +uint32 calibration_points_perside +uint64 calibration_interval_perside_us +uint32[4] calibration_counter_total +bool[4] side_data_collected +float32[4] x +float32[4] y +float32[4] z diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/MagnetometerBiasEstimate.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/MagnetometerBiasEstimate.msg new file mode 100644 index 00000000..3c0c1136 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/MagnetometerBiasEstimate.msg @@ -0,0 +1,8 @@ +uint64 timestamp # time since system start (microseconds) + +float32[4] bias_x # estimated X-bias of all the sensors +float32[4] bias_y # estimated Y-bias of all the sensors +float32[4] bias_z # estimated Z-bias of all the sensors + +bool[4] valid # true if the estimator has converged +bool[4] stable diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ManualControlSetpoint.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ManualControlSetpoint.msg new file mode 100644 index 00000000..95fa6222 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ManualControlSetpoint.msg @@ -0,0 +1,46 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +bool valid + +uint8 SOURCE_UNKNOWN = 0 +uint8 SOURCE_RC = 1 # radio control (input_rc) +uint8 SOURCE_MAVLINK_0 = 2 # mavlink instance 0 +uint8 SOURCE_MAVLINK_1 = 3 # mavlink instance 1 +uint8 SOURCE_MAVLINK_2 = 4 # mavlink instance 2 +uint8 SOURCE_MAVLINK_3 = 5 # mavlink instance 3 +uint8 SOURCE_MAVLINK_4 = 6 # mavlink instance 4 +uint8 SOURCE_MAVLINK_5 = 7 # mavlink instance 5 + +uint8 data_source + +# Any of the channels may not be available and be set to NaN +# to indicate that it does not contain valid data. + +# Stick positions [-1,1] +# on a common RC mode 1/2/3/4 remote/joystick the stick deflection: -1 is down/left, 1 is up/right +# Note: QGC sends throttle/z in range [0,1000] - [0,1]. The MAVLink input conversion [0,1] to [-1,1] is at the moment kept backwards compatible. +# Positive values are generally used for: +float32 roll # move right, positive roll rotation, right side down +float32 pitch # move forward, negative pitch rotation, nose down +float32 yaw # positive yaw rotation, clockwise when seen top down +float32 throttle # move up, positive thrust, -1 is minimum available 0% or -100% +1 is 100% thrust + +float32 flaps # position of flaps switch/knob/lever [-1, 1] + +float32 aux1 +float32 aux2 +float32 aux3 +float32 aux4 +float32 aux5 +float32 aux6 + +bool sticks_moving + +uint16 buttons # From uint16 buttons field of Mavlink manual_control message + +# TOPICS manual_control_setpoint manual_control_input +# DEPRECATED: float32 x +# DEPRECATED: float32 y +# DEPRECATED: float32 z +# DEPRECATED: float32 r diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ManualControlSwitches.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ManualControlSwitches.msg new file mode 100644 index 00000000..4d1cbab2 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ManualControlSwitches.msg @@ -0,0 +1,34 @@ +uint64 timestamp # time since system start (microseconds) + +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +uint8 SWITCH_POS_NONE = 0 # switch is not mapped +uint8 SWITCH_POS_ON = 1 # switch activated (value = 1) +uint8 SWITCH_POS_MIDDLE = 2 # middle position (value = 0) +uint8 SWITCH_POS_OFF = 3 # switch not activated (value = -1) + +uint8 MODE_SLOT_NONE = 0 # no mode slot assigned +uint8 MODE_SLOT_1 = 1 # mode slot 1 selected +uint8 MODE_SLOT_2 = 2 # mode slot 2 selected +uint8 MODE_SLOT_3 = 3 # mode slot 3 selected +uint8 MODE_SLOT_4 = 4 # mode slot 4 selected +uint8 MODE_SLOT_5 = 5 # mode slot 5 selected +uint8 MODE_SLOT_6 = 6 # mode slot 6 selected +uint8 MODE_SLOT_NUM = 6 # number of slots + +uint8 mode_slot # the slot a specific model selector is in + +uint8 arm_switch # arm/disarm switch: _DISARMED_, ARMED +uint8 return_switch # return to launch 2 position switch (mandatory): _NORMAL_, RTL +uint8 loiter_switch # loiter 2 position switch (optional): _MISSION_, LOITER +uint8 offboard_switch # offboard 2 position switch (optional): _NORMAL_, OFFBOARD +uint8 kill_switch # throttle kill: _NORMAL_, KILL +uint8 gear_switch # landing gear switch: _DOWN_, UP +uint8 transition_switch # VTOL transition switch: _HOVER, FORWARD_FLIGHT + +uint8 photo_switch # Photo trigger switch +uint8 video_switch # Photo trigger switch + +uint8 engage_main_motor_switch # Engage the main motor (for helicopters) + +uint32 switch_changes # number of switch changes diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/MavlinkLog.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/MavlinkLog.msg new file mode 100644 index 00000000..8f52ec7d --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/MavlinkLog.msg @@ -0,0 +1,6 @@ +uint64 timestamp # time since system start (microseconds) + +char[127] text +uint8 severity # log level (same as in the linux kernel, starting with 0) + +uint8 ORB_QUEUE_LENGTH = 8 diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/MavlinkTunnel.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/MavlinkTunnel.msg new file mode 100644 index 00000000..16934a95 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/MavlinkTunnel.msg @@ -0,0 +1,20 @@ +# MAV_TUNNEL_PAYLOAD_TYPE enum + +uint8 MAV_TUNNEL_PAYLOAD_TYPE_UNKNOWN = 0 # Encoding of payload unknown +uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED0 = 200 # Registered for STorM32 gimbal controller +uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED1 = 201 # Registered for STorM32 gimbal controller +uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED2 = 202 # Registered for STorM32 gimbal controller +uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED3 = 203 # Registered for STorM32 gimbal controller +uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED4 = 204 # Registered for STorM32 gimbal controller +uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED5 = 205 # Registered for STorM32 gimbal controller +uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED6 = 206 # Registered for STorM32 gimbal controller +uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED7 = 207 # Registered for STorM32 gimbal controller +uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED8 = 208 # Registered for STorM32 gimbal controller +uint8 MAV_TUNNEL_PAYLOAD_TYPE_STORM32_RESERVED9 = 209 # Registered for STorM32 gimbal controller + +uint64 timestamp # Time since system start (microseconds) +uint16 payload_type # A code that identifies the content of the payload (0 for unknown, which is the default). If this code is less than 32768, it is a 'registered' payload type and the corresponding code should be added to the MAV_TUNNEL_PAYLOAD_TYPE enum. Software creators can register blocks of types as needed. Codes greater than 32767 are considered local experiments and should not be checked in to any widely distributed codebase. +uint8 target_system # System ID (can be 0 for broadcast, but this is discouraged) +uint8 target_component # Component ID (can be 0 for broadcast, but this is discouraged) +uint8 payload_length # Length of the data transported in payload +uint8[128] payload # Data itself diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/MessageFormatRequest.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/MessageFormatRequest.msg new file mode 100644 index 00000000..6ceb66d0 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/MessageFormatRequest.msg @@ -0,0 +1,9 @@ +uint64 timestamp # time since system start (microseconds) + +# Request to PX4 to get the hash of a message, to check for message compatibility + +uint16 LATEST_PROTOCOL_VERSION = 1 # Current version of this protocol. Increase this whenever the MessageFormatRequest or MessageFormatResponse changes. + +uint16 protocol_version # Must be set to LATEST_PROTOCOL_VERSION. Do not change this field, it must be the first field after the timestamp + +char[50] topic_name # E.g. /fmu/in/vehicle_command diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/MessageFormatResponse.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/MessageFormatResponse.msg new file mode 100644 index 00000000..41ee9627 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/MessageFormatResponse.msg @@ -0,0 +1,11 @@ +uint64 timestamp # time since system start (microseconds) + +# Response from PX4 with the format of a message + +uint16 protocol_version # Must be set to LATEST_PROTOCOL_VERSION. Do not change this field, it must be the first field after the timestamp + +char[50] topic_name # E.g. /fmu/in/vehicle_command + +bool success +uint32 message_hash # hash over all message fields + diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/Mission.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/Mission.msg new file mode 100644 index 00000000..a923193d --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/Mission.msg @@ -0,0 +1,14 @@ +uint64 timestamp # time since system start (microseconds) +uint8 mission_dataman_id # default 0, there are two offboard storage places in the dataman: 0 or 1 +uint8 fence_dataman_id # default 0, there are two offboard storage places in the dataman: 0 or 1 +uint8 safepoint_dataman_id # default 0, there are two offboard storage places in the dataman: 0 or 1 + +uint16 count # count of the missions stored in the dataman +int32 current_seq # default -1, start at the one changed latest + +int32 land_start_index # Index of the land start marker, if unavailable index of the land item, -1 otherwise +int32 land_index # Index of the land item, -1 otherwise + +uint32 mission_id # indicates updates to the mission, reload from dataman if changed +uint32 geofence_id # indicates updates to the geofence, reload from dataman if changed +uint32 safe_points_id # indicates updates to the safe points, reload from dataman if changed diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/MissionResult.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/MissionResult.msg new file mode 100644 index 00000000..f70326be --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/MissionResult.msg @@ -0,0 +1,20 @@ +uint64 timestamp # time since system start (microseconds) + +uint32 mission_id # Id for the mission for which the result was generated +uint32 geofence_id # Id for the corresponding geofence for which the result was generated (used for mission feasibility) +uint32 home_position_counter # Counter of the home position for which the result was generated (used for mission feasibility) + +int32 seq_reached # Sequence of the mission item which has been reached, default -1 +uint16 seq_current # Sequence of the current mission item +uint16 seq_total # Total number of mission items + +bool valid # true if mission is valid +bool warning # true if mission is valid, but has potentially problematic items leading to safety warnings +bool finished # true if mission has been completed +bool failure # true if the mission cannot continue or be completed for some reason + +bool item_do_jump_changed # true if the number of do jumps remaining has changed +uint16 item_changed_index # indicate which item has changed +uint16 item_do_jump_remaining # set to the number of do jumps remaining for that item + +uint8 execution_mode # indicates the mode in which the mission is executed diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ModeCompleted.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ModeCompleted.msg new file mode 100644 index 00000000..bacff4a9 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ModeCompleted.msg @@ -0,0 +1,15 @@ +# Mode completion result, published by an active mode. +# The possible values of nav_state are defined in the VehicleStatus msg. +# Note that this is not always published (e.g. when a user switches modes or on +# failsafe activation) +uint64 timestamp # time since system start (microseconds) + + +uint8 RESULT_SUCCESS = 0 +# [1-99]: reserved +uint8 RESULT_FAILURE_OTHER = 100 # Mode failed (generic error) + +uint8 result # One of RESULT_* + +uint8 nav_state # Source mode (values in VehicleStatus) + diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/MountOrientation.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/MountOrientation.msg new file mode 100644 index 00000000..7ae54a39 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/MountOrientation.msg @@ -0,0 +1,2 @@ +uint64 timestamp # time since system start (microseconds) +float32[3] attitude_euler_angle # Attitude/direction of the mount as euler angles in rad diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/NavigatorMissionItem.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/NavigatorMissionItem.msg new file mode 100644 index 00000000..64af762f --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/NavigatorMissionItem.msg @@ -0,0 +1,25 @@ +uint64 timestamp # time since system start (microseconds) + +uint32 instance_count # Instance count of this mission. Increments monotonically whenever the mission is modified + +uint16 sequence_current # Sequence of the current mission item + +uint16 nav_cmd + +float32 latitude +float32 longitude + +float32 time_inside # time that the MAV should stay inside the radius before advancing in seconds +float32 acceptance_radius # default radius in which the mission is accepted as reached in meters +float32 loiter_radius # loiter radius in meters, 0 for a VTOL to hover, negative for counter-clockwise +float32 yaw # in radians NED -PI..+PI, NAN means don't change yaw +float32 altitude # altitude in meters (AMSL) + +uint8 frame # mission frame +uint8 origin # mission item origin (onboard or mavlink) + +bool loiter_exit_xtrack # exit xtrack location: 0 for center of loiter wp, 1 for exit location +bool force_heading # heading needs to be reached +bool altitude_is_relative # true if altitude is relative from start point +bool autocontinue # true if next waypoint should follow after this one +bool vtol_back_transition # part of the vtol back transition sequence diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/NormalizedUnsignedSetpoint.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/NormalizedUnsignedSetpoint.msg new file mode 100644 index 00000000..10193b8c --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/NormalizedUnsignedSetpoint.msg @@ -0,0 +1,5 @@ +uint64 timestamp # time since system start (microseconds) + +float32 normalized_setpoint # [0, 1] + +# TOPICS flaps_setpoint spoilers_setpoint diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/NpfgStatus.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/NpfgStatus.msg new file mode 100644 index 00000000..132c1f7f --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/NpfgStatus.msg @@ -0,0 +1,17 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 wind_est_valid # (boolean) true = wind estimate is valid and/or being used by controller (also indicates if wind est usage is disabled despite being valid) +float32 lat_accel # resultant lateral acceleration reference [m/s^2] +float32 lat_accel_ff # lateral acceleration demand only for maintaining curvature [m/s^2] +float32 bearing_feas # bearing feasibility [0,1] +float32 bearing_feas_on_track # on-track bearing feasibility [0,1] +float32 signed_track_error # signed track error [m] +float32 track_error_bound # track error bound [m] +float32 airspeed_ref # (true) airspeed reference [m/s] +float32 bearing # bearing angle [rad] +float32 heading_ref # heading angle reference [rad] +float32 min_ground_speed_ref # minimum forward ground speed reference [m/s] +float32 adapted_period # adapted period (if auto-tuning enabled) [s] +float32 p_gain # controller proportional gain [rad/s] +float32 time_const # controller time constant [s] +float32 can_run_factor # estimate of certainty of the correct functionality of the npfg roll setpoint in [0, 1] diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ObstacleDistance.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ObstacleDistance.msg new file mode 100644 index 00000000..e3c4963a --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ObstacleDistance.msg @@ -0,0 +1,24 @@ +# Obstacle distances in front of the sensor. +uint64 timestamp # time since system start (microseconds) + +uint8 frame #Coordinate frame of reference for the yaw rotation and offset of the sensor data. Defaults to MAV_FRAME_GLOBAL, which is North aligned. For body-mounted sensors use MAV_FRAME_BODY_FRD, which is vehicle front aligned. +uint8 MAV_FRAME_GLOBAL = 0 +uint8 MAV_FRAME_LOCAL_NED = 1 +uint8 MAV_FRAME_BODY_FRD = 12 + +uint8 sensor_type # Type from MAV_DISTANCE_SENSOR enum. +uint8 MAV_DISTANCE_SENSOR_LASER = 0 +uint8 MAV_DISTANCE_SENSOR_ULTRASOUND = 1 +uint8 MAV_DISTANCE_SENSOR_INFRARED = 2 +uint8 MAV_DISTANCE_SENSOR_RADAR = 3 + +uint16[72] distances # Distance of obstacles around the UAV with index 0 corresponding to local North. A value of 0 means that the obstacle is right in front of the sensor. A value of max_distance +1 means no obstacle is present. A value of UINT16_MAX for unknown/not used. In a array element, one unit corresponds to 1cm. + +float32 increment # Angular width in degrees of each array element. + +uint16 min_distance # Minimum distance the sensor can measure in centimeters. +uint16 max_distance # Maximum distance the sensor can measure in centimeters. + +float32 angle_offset # Relative angle offset of the 0-index element in the distances array. Value of 0 corresponds to forward. Positive values are offsets to the right. + +# TOPICS obstacle_distance obstacle_distance_fused diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/OffboardControlMode.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/OffboardControlMode.msg new file mode 100644 index 00000000..885164a6 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/OffboardControlMode.msg @@ -0,0 +1,11 @@ +# Off-board control mode + +uint64 timestamp # time since system start (microseconds) + +bool position +bool velocity +bool acceleration +bool attitude +bool body_rate +bool thrust_and_torque +bool direct_actuator diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/OnboardComputerStatus.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/OnboardComputerStatus.msg new file mode 100644 index 00000000..736932bc --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/OnboardComputerStatus.msg @@ -0,0 +1,23 @@ +# ONBOARD_COMPUTER_STATUS message data +uint64 timestamp # [us] time since system start (microseconds) +uint32 uptime # [ms] time since system boot of the companion (milliseconds) + +uint8 type # type of onboard computer 0: Mission computer primary, 1: Mission computer backup 1, 2: Mission computer backup 2, 3: Compute node, 4-5: Compute spares, 6-9: Payload computers. + +uint8[8] cpu_cores # CPU usage on the component in percent +uint8[10] cpu_combined # Combined CPU usage as the last 10 slices of 100 MS +uint8[4] gpu_cores # GPU usage on the component in percent +uint8[10] gpu_combined # Combined GPU usage as the last 10 slices of 100 MS +int8 temperature_board # [degC] Temperature of the board +int8[8] temperature_core # [degC] Temperature of the CPU core +int16[4] fan_speed # [rpm] Fan speeds +uint32 ram_usage # [MB] Amount of used RAM on the component system +uint32 ram_total # [MB] Total amount of RAM on the component system +uint32[4] storage_type # Storage type: 0: HDD, 1: SSD, 2: EMMC, 3: SD card (non-removable), 4: SD card (removable) +uint32[4] storage_usage # [MB] Amount of used storage space on the component system +uint32[4] storage_total # [MB] Total amount of storage space on the component system +uint32[6] link_type # [Kb/s] Link type: 0-9: UART, 10-19: Wired network, 20-29: Wifi, 30-39: Point-to-point proprietary, 40-49: Mesh proprietary +uint32[6] link_tx_rate # [Kb/s] Network traffic from the component system +uint32[6] link_rx_rate # [Kb/s] Network traffic to the component system +uint32[6] link_tx_max # [Kb/s] Network capacity from the component system +uint32[6] link_rx_max # [Kb/s] Network capacity to the component system diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/OrbTest.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/OrbTest.msg new file mode 100644 index 00000000..bbbca412 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/OrbTest.msg @@ -0,0 +1,5 @@ +uint64 timestamp # time since system start (microseconds) + +int32 val + +# TOPICS orb_test orb_multitest diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/OrbTestLarge.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/OrbTestLarge.msg new file mode 100644 index 00000000..48d6a427 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/OrbTestLarge.msg @@ -0,0 +1,5 @@ +uint64 timestamp # time since system start (microseconds) + +int32 val + +uint8[512] junk diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/OrbTestMedium.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/OrbTestMedium.msg new file mode 100644 index 00000000..43109d49 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/OrbTestMedium.msg @@ -0,0 +1,9 @@ +uint64 timestamp # time since system start (microseconds) + +int32 val + +uint8[64] junk + +uint8 ORB_QUEUE_LENGTH = 16 + +# TOPICS orb_test_medium orb_test_medium_multi orb_test_medium_wrap_around orb_test_medium_queue orb_test_medium_queue_poll diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/OrbitStatus.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/OrbitStatus.msg new file mode 100644 index 00000000..a04265db --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/OrbitStatus.msg @@ -0,0 +1,14 @@ +# ORBIT_YAW_BEHAVIOUR +uint8 ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TO_CIRCLE_CENTER = 0 +uint8 ORBIT_YAW_BEHAVIOUR_HOLD_INITIAL_HEADING = 1 +uint8 ORBIT_YAW_BEHAVIOUR_UNCONTROLLED = 2 +uint8 ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TANGENT_TO_CIRCLE = 3 +uint8 ORBIT_YAW_BEHAVIOUR_RC_CONTROLLED = 4 + +uint64 timestamp # time since system start (microseconds) +float32 radius # Radius of the orbit circle. Positive values orbit clockwise, negative values orbit counter-clockwise. [m] +uint8 frame # The coordinate system of the fields: x, y, z. +float64 x # X coordinate of center point. Coordinate system depends on frame field: local = x position in meters * 1e4, global = latitude in degrees * 1e7. +float64 y # Y coordinate of center point. Coordinate system depends on frame field: local = y position in meters * 1e4, global = latitude in degrees * 1e7. +float32 z # Altitude of center point. Coordinate system depends on frame field. +uint8 yaw_behaviour diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ParameterResetRequest.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ParameterResetRequest.msg new file mode 100644 index 00000000..db08edb3 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ParameterResetRequest.msg @@ -0,0 +1,8 @@ +# ParameterResetRequest : Used by the primary to reset one or all parameter value(s) on the remote + +uint64 timestamp +uint16 parameter_index + +bool reset_all # If this is true then ignore parameter_index + +uint8 ORB_QUEUE_LENGTH = 4 diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ParameterSetUsedRequest.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ParameterSetUsedRequest.msg new file mode 100644 index 00000000..ca97f9e9 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ParameterSetUsedRequest.msg @@ -0,0 +1,6 @@ +# ParameterSetUsedRequest : Used by a remote to update the used flag for a parameter on the primary + +uint64 timestamp +uint16 parameter_index + +uint8 ORB_QUEUE_LENGTH = 64 diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ParameterSetValueRequest.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ParameterSetValueRequest.msg new file mode 100644 index 00000000..eaf650f2 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ParameterSetValueRequest.msg @@ -0,0 +1,11 @@ +# ParameterSetValueRequest : Used by a remote or primary to update the value for a parameter at the other end + +uint64 timestamp +uint16 parameter_index + +int32 int_value # Optional value for an integer parameter +float32 float_value # Optional value for a float parameter + +uint8 ORB_QUEUE_LENGTH = 32 + +# TOPICS parameter_set_value_request parameter_remote_set_value_request parameter_primary_set_value_request diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ParameterSetValueResponse.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ParameterSetValueResponse.msg new file mode 100644 index 00000000..09f8e308 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ParameterSetValueResponse.msg @@ -0,0 +1,9 @@ +# ParameterSetValueResponse : Response to a set value request by either primary or secondary + +uint64 timestamp +uint64 request_timestamp +uint16 parameter_index + +uint8 ORB_QUEUE_LENGTH = 4 + +# TOPICS parameter_set_value_response parameter_remote_set_value_response parameter_primary_set_value_response \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ParameterUpdate.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ParameterUpdate.msg new file mode 100644 index 00000000..bfb49937 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/ParameterUpdate.msg @@ -0,0 +1,14 @@ +# This message is used to notify the system about one or more parameter changes + +uint64 timestamp # time since system start (microseconds) + +uint32 instance # Instance count - constantly incrementing + +uint32 get_count +uint32 set_count +uint32 find_count +uint32 export_count + +uint16 active +uint16 changed +uint16 custom_default diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/Ping.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/Ping.msg new file mode 100644 index 00000000..498a3c73 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/Ping.msg @@ -0,0 +1,7 @@ +uint64 timestamp # time since system start (microseconds) +uint64 ping_time # Timestamp of the ping packet +uint32 ping_sequence # Sequence number of the ping packet +uint32 dropped_packets # Number of dropped ping packets +float32 rtt_ms # Round trip time (in ms) +uint8 system_id # System ID of the remote system +uint8 component_id # Component ID of the remote system diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/PositionControllerLandingStatus.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/PositionControllerLandingStatus.msg new file mode 100644 index 00000000..249529b4 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/PositionControllerLandingStatus.msg @@ -0,0 +1,16 @@ +uint64 timestamp # [us] time since system start +float32 lateral_touchdown_offset # [m] lateral touchdown position offset manually commanded during landing +bool flaring # true if the aircraft is flaring + +# abort status is: +# 0 if not aborted +# >0 if aborted, with the singular abort criterion which triggered the landing abort enumerated by the following abort reasons +uint8 abort_status + +# abort reasons +# after the manual operator abort, corresponds to individual bits of param FW_LND_ABORT +uint8 NOT_ABORTED = 0 +uint8 ABORTED_BY_OPERATOR = 1 +uint8 TERRAIN_NOT_FOUND = 2 # FW_LND_ABORT (1 << 0) +uint8 TERRAIN_TIMEOUT = 3 # FW_LND_ABORT (1 << 1) +uint8 UNKNOWN_ABORT_CRITERION = 4 diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/PositionControllerStatus.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/PositionControllerStatus.msg new file mode 100644 index 00000000..7237351f --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/PositionControllerStatus.msg @@ -0,0 +1,12 @@ +uint64 timestamp # time since system start (microseconds) + +float32 nav_roll # Roll setpoint [rad] +float32 nav_pitch # Pitch setpoint [rad] +float32 nav_bearing # Bearing angle[rad] +float32 target_bearing # Bearing angle from aircraft to current target [rad] +float32 xtrack_error # Signed track error [m] +float32 wp_dist # Distance to active (next) waypoint [m] +float32 acceptance_radius # Current horizontal acceptance radius [m] +float32 yaw_acceptance # Yaw acceptance error[rad] +float32 altitude_acceptance # Current vertical acceptance error [m] +uint8 type # Current (applied) position setpoint type (see PositionSetpoint.msg) diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/PositionSetpoint.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/PositionSetpoint.msg new file mode 100644 index 00000000..035d3520 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/PositionSetpoint.msg @@ -0,0 +1,37 @@ +# this file is only used in the position_setpoint triple as a dependency + +uint64 timestamp # time since system start (microseconds) + +uint8 SETPOINT_TYPE_POSITION=0 # position setpoint +uint8 SETPOINT_TYPE_VELOCITY=1 # velocity setpoint +uint8 SETPOINT_TYPE_LOITER=2 # loiter setpoint +uint8 SETPOINT_TYPE_TAKEOFF=3 # takeoff setpoint +uint8 SETPOINT_TYPE_LAND=4 # land setpoint, altitude must be ignored, descend until landing +uint8 SETPOINT_TYPE_IDLE=5 # do nothing, switch off motors or keep at idle speed (MC) + +uint8 LOITER_TYPE_ORBIT=0 # Circular pattern +uint8 LOITER_TYPE_FIGUREEIGHT=1 # Pattern resembling an 8 + +bool valid # true if setpoint is valid +uint8 type # setpoint type to adjust behavior of position controller + +float32 vx # local velocity setpoint in m/s in NED +float32 vy # local velocity setpoint in m/s in NED +float32 vz # local velocity setpoint in m/s in NED + +float64 lat # latitude, in deg +float64 lon # longitude, in deg +float32 alt # altitude AMSL, in m +float32 yaw # yaw (only in hover), in rad [-PI..PI), NaN = leave to flight task + +float32 loiter_radius # loiter major axis radius in m +float32 loiter_minor_radius # loiter minor axis radius (used for non-circular loiter shapes) in m +bool loiter_direction_counter_clockwise # loiter direction is clockwise by default and can be changed using this field +float32 loiter_orientation # Orientation of the major axis with respect to true north in rad [-pi,pi) +uint8 loiter_pattern # loitern pattern to follow + +float32 acceptance_radius # navigation acceptance_radius if we're doing waypoint navigation + +float32 cruising_speed # the generally desired cruising speed (not a hard constraint) +bool gliding_enabled # commands the vehicle to glide if the capability is available (fixed wing only) +float32 cruising_throttle # the generally desired cruising throttle (not a hard constraint), only has an effect for rover diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/PositionSetpointTriplet.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/PositionSetpointTriplet.msg new file mode 100644 index 00000000..6f9ac4d2 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/PositionSetpointTriplet.msg @@ -0,0 +1,8 @@ +# Global position setpoint triplet in WGS84 coordinates. +# This are the three next waypoints (or just the next two or one). + +uint64 timestamp # time since system start (microseconds) + +PositionSetpoint previous +PositionSetpoint current +PositionSetpoint next diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/PowerButtonState.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/PowerButtonState.msg new file mode 100644 index 00000000..15180666 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/PowerButtonState.msg @@ -0,0 +1,10 @@ +# power button state notification message + +uint64 timestamp # time since system start (microseconds) + +uint8 PWR_BUTTON_STATE_IDEL = 0 # Button went up without meeting shutdown button down time (delete event) +uint8 PWR_BUTTON_STATE_DOWN = 1 # Button went Down +uint8 PWR_BUTTON_STATE_UP = 2 # Button went Up +uint8 PWR_BUTTON_STATE_REQUEST_SHUTDOWN = 3 # Button went Up after meeting shutdown button down time + +uint8 event # one of PWR_BUTTON_STATE_* diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/PowerMonitor.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/PowerMonitor.msg new file mode 100644 index 00000000..a64585fa --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/PowerMonitor.msg @@ -0,0 +1,15 @@ +# power monitor message + +uint64 timestamp # Time since system start (microseconds) + +float32 voltage_v # Voltage in volts, 0 if unknown +float32 current_a # Current in amperes, -1 if unknown +float32 power_w # power in watts, -1 if unknown +int16 rconf +int16 rsv +int16 rbv +int16 rp +int16 rc +int16 rcal +int16 me +int16 al diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/PpsCapture.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/PpsCapture.msg new file mode 100644 index 00000000..c6fa2cb9 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/PpsCapture.msg @@ -0,0 +1,3 @@ +uint64 timestamp # time since system start (microseconds) at PPS capture event +uint64 rtc_timestamp # Corrected GPS UTC timestamp at PPS capture event +uint8 pps_rate_exceeded_counter # Increments when PPS dt < 50ms diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/PwmInput.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/PwmInput.msg new file mode 100644 index 00000000..fcc7dbe4 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/PwmInput.msg @@ -0,0 +1,4 @@ +uint64 timestamp # Time since system start (microseconds) +uint64 error_count # Timer overcapture error flag (AUX5 or MAIN5) +uint32 pulse_width # Pulse width, timer counts +uint32 period # Period, timer counts diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/Px4ioStatus.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/Px4ioStatus.msg new file mode 100644 index 00000000..295c8fba --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/Px4ioStatus.msg @@ -0,0 +1,43 @@ +uint64 timestamp # time since system start (microseconds) + +uint16 free_memory_bytes + +float32 voltage_v # Servo rail voltage in volts +float32 rssi_v # RSSI pin voltage in volts + +# PX4IO status flags (PX4IO_P_STATUS_FLAGS) +bool status_arm_sync +bool status_failsafe +bool status_fmu_initialized +bool status_fmu_ok +bool status_init_ok +bool status_outputs_armed +bool status_raw_pwm +bool status_rc_ok +bool status_rc_dsm +bool status_rc_ppm +bool status_rc_sbus +bool status_rc_st24 +bool status_rc_sumd +bool status_safety_button_event # px4io safety button was pressed for longer than 1 second + +# PX4IO alarms (PX4IO_P_STATUS_ALARMS) +bool alarm_pwm_error +bool alarm_rc_lost + +# PX4IO arming (PX4IO_P_SETUP_ARMING) +bool arming_failsafe_custom +bool arming_fmu_armed +bool arming_fmu_prearmed +bool arming_force_failsafe +bool arming_io_arm_ok +bool arming_lockdown +bool arming_termination_failsafe + +uint16[8] pwm +uint16[8] pwm_disarmed +uint16[8] pwm_failsafe + +uint16[8] pwm_rate_hz + +uint16[18] raw_inputs diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/QshellReq.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/QshellReq.msg new file mode 100644 index 00000000..d472e8dd --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/QshellReq.msg @@ -0,0 +1,5 @@ +uint64 timestamp # time since system start (microseconds) +char[100] cmd +uint32 MAX_STRLEN = 100 +uint32 strlen +uint32 request_sequence diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/QshellRetval.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/QshellRetval.msg new file mode 100644 index 00000000..d42a7715 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/QshellRetval.msg @@ -0,0 +1,3 @@ +uint64 timestamp # time since system start (microseconds) +int32 return_value +uint32 return_sequence diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/RadioStatus.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/RadioStatus.msg new file mode 100644 index 00000000..c57c82b5 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/RadioStatus.msg @@ -0,0 +1,13 @@ + +uint64 timestamp # time since system start (microseconds) + +uint8 rssi # local signal strength +uint8 remote_rssi # remote signal strength + +uint8 txbuf # how full the tx buffer is as a percentage +uint8 noise # background noise level + +uint8 remote_noise # remote background noise level +uint16 rxerrors # receive errors + +uint16 fix # count of error corrected packets diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/RateCtrlStatus.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/RateCtrlStatus.msg new file mode 100644 index 00000000..3f564469 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/RateCtrlStatus.msg @@ -0,0 +1,7 @@ +uint64 timestamp # time since system start (microseconds) + +# rate controller integrator status +float32 rollspeed_integ +float32 pitchspeed_integ +float32 yawspeed_integ +float32 wheel_rate_integ # FW only and optional diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/RcChannels.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/RcChannels.msg new file mode 100644 index 00000000..546755f6 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/RcChannels.msg @@ -0,0 +1,40 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 FUNCTION_THROTTLE = 0 +uint8 FUNCTION_ROLL = 1 +uint8 FUNCTION_PITCH = 2 +uint8 FUNCTION_YAW = 3 +uint8 FUNCTION_RETURN = 4 +uint8 FUNCTION_LOITER = 5 +uint8 FUNCTION_OFFBOARD = 6 +uint8 FUNCTION_FLAPS = 7 +uint8 FUNCTION_AUX_1 = 8 +uint8 FUNCTION_AUX_2 = 9 +uint8 FUNCTION_AUX_3 = 10 +uint8 FUNCTION_AUX_4 = 11 +uint8 FUNCTION_AUX_5 = 12 +uint8 FUNCTION_AUX_6 = 13 +uint8 FUNCTION_PARAM_1 = 14 +uint8 FUNCTION_PARAM_2 = 15 +uint8 FUNCTION_PARAM_3_5 = 16 +uint8 FUNCTION_KILLSWITCH = 17 +uint8 FUNCTION_TRANSITION = 18 +uint8 FUNCTION_GEAR = 19 +uint8 FUNCTION_ARMSWITCH = 20 +uint8 FUNCTION_FLTBTN_SLOT_1 = 21 +uint8 FUNCTION_FLTBTN_SLOT_2 = 22 +uint8 FUNCTION_FLTBTN_SLOT_3 = 23 +uint8 FUNCTION_FLTBTN_SLOT_4 = 24 +uint8 FUNCTION_FLTBTN_SLOT_5 = 25 +uint8 FUNCTION_FLTBTN_SLOT_6 = 26 +uint8 FUNCTION_ENGAGE_MAIN_MOTOR = 27 + +uint8 FUNCTION_FLTBTN_SLOT_COUNT = 6 + +uint64 timestamp_last_valid # Timestamp of last valid RC signal +float32[18] channels # Scaled to -1..1 (throttle: 0..1) +uint8 channel_count # Number of valid channels +int8[28] function # Functions mapping +uint8 rssi # Receive signal strength index +bool signal_lost # Control signal lost, should be checked together with topic timeout +uint32 frame_drop_count # Number of dropped frames diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/RcParameterMap.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/RcParameterMap.msg new file mode 100644 index 00000000..2a5df3e7 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/RcParameterMap.msg @@ -0,0 +1,11 @@ +uint64 timestamp # time since system start (microseconds) +uint8 RC_PARAM_MAP_NCHAN = 3 # This limit is also hardcoded in the enum RC_CHANNELS_FUNCTION in rc_channels.h +uint8 PARAM_ID_LEN = 16 # corresponds to MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + +bool[3] valid #true for RC-Param channels which are mapped to a param +int32[3] param_index # corresponding param index, this field is ignored if set to -1, in this case param_id will be used +char[51] param_id # MAP_NCHAN * (ID_LEN + 1) chars, corresponding param id, null terminated +float32[3] scale # scale to map the RC input [-1, 1] to a parameter value +float32[3] value0 # initial value around which the parameter value is changed +float32[3] value_min # minimal parameter value +float32[3] value_max # minimal parameter value diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/RegisterExtComponentReply.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/RegisterExtComponentReply.msg new file mode 100644 index 00000000..7cd7eef0 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/RegisterExtComponentReply.msg @@ -0,0 +1,13 @@ +uint64 timestamp # time since system start (microseconds) + +uint64 request_id # ID from the request +char[25] name # name from the request + +uint16 px4_ros2_api_version + +bool success +int8 arming_check_id # arming check registration ID (-1 if invalid) +int8 mode_id # assigned mode ID (-1 if invalid) +int8 mode_executor_id # assigned mode executor ID (-1 if invalid) + +uint8 ORB_QUEUE_LENGTH = 2 diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/RegisterExtComponentRequest.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/RegisterExtComponentRequest.msg new file mode 100644 index 00000000..46ab0cb0 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/RegisterExtComponentRequest.msg @@ -0,0 +1,21 @@ +# Request to register an external component +uint64 timestamp # time since system start (microseconds) + +uint64 request_id # ID, set this to a random value +char[25] name # either the requested mode name, or component name + +uint16 LATEST_PX4_ROS2_API_VERSION = 1 # API version compatibility. Increase this on a breaking semantic change. Changes to any message field are detected separately and do not require an API version change. + +uint16 px4_ros2_api_version # Set to LATEST_PX4_ROS2_API_VERSION + +# Components to be registered +bool register_arming_check +bool register_mode # registering a mode also requires arming_check to be set +bool register_mode_executor # registering an executor also requires a mode to be registered (which is the owned mode by the executor) + +bool enable_replace_internal_mode # set to true if an internal mode should be replaced +uint8 replace_internal_mode # vehicle_status::NAVIGATION_STATE_* +bool activate_mode_immediately # switch to the registered mode (can only be set in combination with an executor) + + +uint8 ORB_QUEUE_LENGTH = 2 diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/RoverAckermannGuidanceStatus.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/RoverAckermannGuidanceStatus.msg new file mode 100644 index 00000000..3c34b63c --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/RoverAckermannGuidanceStatus.msg @@ -0,0 +1,10 @@ +uint64 timestamp # time since system start (microseconds) + +float32 actual_speed # [m/s] Rover ground speed +float32 desired_speed # [m/s] Rover desired ground speed +float32 lookahead_distance # [m] Lookahead distance of pure the pursuit controller +float32 heading_error # [deg] Heading error of the pure pursuit controller +float32 pid_throttle_integral # [-1, 1] Integral of the PID for the normalized throttle to control the rover speed during missions +float32 crosstrack_error # [m] Shortest distance from the vehicle to the path + +# TOPICS rover_ackermann_guidance_status diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/Rpm.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/Rpm.msg new file mode 100644 index 00000000..baab7c6a --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/Rpm.msg @@ -0,0 +1,4 @@ +uint64 timestamp # time since system start (microseconds) + +float32 indicated_frequency_rpm # indicated rotor Frequency in Revolution per minute +float32 estimated_accurancy_rpm # estimated accuracy in Revolution per minute diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/RtlStatus.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/RtlStatus.msg new file mode 100644 index 00000000..f25b2224 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/RtlStatus.msg @@ -0,0 +1,15 @@ +uint64 timestamp # time since system start (microseconds) + +uint32 safe_points_id # unique ID of active set of safe_point_items +bool is_evaluation_pending # flag if the RTL point needs reevaluation (e.g. new safe points available, but need loading). + +bool has_vtol_approach # flag if approaches are defined for current RTL_TYPE parameter setting + +uint8 rtl_type # Type of RTL chosen +uint8 safe_point_index # index of the chosen safe point, if in RTL_STATUS_TYPE_DIRECT_SAFE_POINT mode + +uint8 RTL_STATUS_TYPE_NONE=0 # pending if evaluation can't pe performed currently e.g. when it is still loading the safe points +uint8 RTL_STATUS_TYPE_DIRECT_SAFE_POINT=1 # chosen to directly go to a safe point or home position +uint8 RTL_STATUS_TYPE_DIRECT_MISSION_LAND=2 # going straight to the beginning of the mission landing +uint8 RTL_STATUS_TYPE_FOLLOW_MISSION=3 # Following the mission from start index to mission landing. Start index is current WP if in Mission mode, and closest WP otherwise. +uint8 RTL_STATUS_TYPE_FOLLOW_MISSION_REVERSE=4 # Following the mission in reverse from start index to the beginning of the mission. Start index is previous WP if in Mission mode, and closest WP otherwise. diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/RtlTimeEstimate.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/RtlTimeEstimate.msg new file mode 100644 index 00000000..ee46888d --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/RtlTimeEstimate.msg @@ -0,0 +1,5 @@ +uint64 timestamp # time since system start (microseconds) + +bool valid # Flag indicating whether the time estiamtes are valid +float32 time_estimate # [s] Estimated time for RTL +float32 safe_time_estimate # [s] Same as time_estimate, but with safety factor and safety margin included (factor*t + margin) diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SatelliteInfo.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SatelliteInfo.msg new file mode 100644 index 00000000..2980ffcb --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SatelliteInfo.msg @@ -0,0 +1,10 @@ +uint64 timestamp # time since system start (microseconds) +uint8 SAT_INFO_MAX_SATELLITES = 20 + +uint8 count # Number of satellites visible to the receiver +uint8[20] svid # Space vehicle ID [1..255], see scheme below +uint8[20] used # 0: Satellite not used, 1: used for navigation +uint8[20] elevation # Elevation (0: right on top of receiver, 90: on the horizon) of satellite +uint8[20] azimuth # Direction of satellite, 0: 0 deg, 255: 360 deg. +uint8[20] snr # dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. +uint8[20] prn # Satellite PRN code assignment, (psuedorandom number SBAS, valid codes are 120-144) diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorAccel.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorAccel.msg new file mode 100644 index 00000000..e47d813a --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorAccel.msg @@ -0,0 +1,18 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +float32 x # acceleration in the FRD board frame X-axis in m/s^2 +float32 y # acceleration in the FRD board frame Y-axis in m/s^2 +float32 z # acceleration in the FRD board frame Z-axis in m/s^2 + +float32 temperature # temperature in degrees Celsius + +uint32 error_count + +uint8[3] clip_counter # clip count per axis in the sample period + +uint8 samples # number of raw samples that went into this message + +uint8 ORB_QUEUE_LENGTH = 8 diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorAccelFifo.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorAccelFifo.msg new file mode 100644 index 00000000..1eae5dd1 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorAccelFifo.msg @@ -0,0 +1,13 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +float32 dt # delta time between samples (microseconds) +float32 scale + +uint8 samples # number of valid samples + +int16[32] x # acceleration in the FRD board frame X-axis in m/s^2 +int16[32] y # acceleration in the FRD board frame Y-axis in m/s^2 +int16[32] z # acceleration in the FRD board frame Z-axis in m/s^2 diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorAirflow.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorAirflow.msg new file mode 100644 index 00000000..dd55ad0b --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorAirflow.msg @@ -0,0 +1,5 @@ +uint64 timestamp # time since system start (microseconds) +uint32 device_id # unique device ID for the sensor that does not change between power cycles +float32 speed # the speed being reported by the wind / airflow sensor +float32 direction # the direction bein report by the wind / airflow sensor +uint8 status # Status code from the sensor diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorBaro.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorBaro.msg new file mode 100644 index 00000000..7c4154e1 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorBaro.msg @@ -0,0 +1,12 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +float32 pressure # static pressure measurement in Pascals + +float32 temperature # temperature in degrees Celsius + +uint32 error_count + +uint8 ORB_QUEUE_LENGTH = 4 diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorCombined.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorCombined.msg new file mode 100644 index 00000000..837c7a1f --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorCombined.msg @@ -0,0 +1,25 @@ +# Sensor readings in SI-unit form. +# These fields are scaled and offset-compensated where possible and do not +# change with board revisions and sensor updates. + +uint64 timestamp # time since system start (microseconds) + +int32 RELATIVE_TIMESTAMP_INVALID = 2147483647 # (0x7fffffff) If one of the relative timestamps is set to this value, it means the associated sensor values are invalid + +# gyro timstamp is equal to the timestamp of the message +float32[3] gyro_rad # average angular rate measured in the FRD body frame XYZ-axis in rad/s over the last gyro sampling period +uint32 gyro_integral_dt # gyro measurement sampling period in microseconds + +int32 accelerometer_timestamp_relative # timestamp + accelerometer_timestamp_relative = Accelerometer timestamp +float32[3] accelerometer_m_s2 # average value acceleration measured in the FRD body frame XYZ-axis in m/s^2 over the last accelerometer sampling period +uint32 accelerometer_integral_dt # accelerometer measurement sampling period in microseconds + +uint8 CLIPPING_X = 1 +uint8 CLIPPING_Y = 2 +uint8 CLIPPING_Z = 4 + +uint8 accelerometer_clipping # bitfield indicating if there was any accelerometer clipping (per axis) during the integration time frame +uint8 gyro_clipping # bitfield indicating if there was any gyro clipping (per axis) during the integration time frame + +uint8 accel_calibration_count # Calibration changed counter. Monotonically increases whenever accelermeter calibration changes. +uint8 gyro_calibration_count # Calibration changed counter. Monotonically increases whenever rate gyro calibration changes. diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorCorrection.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorCorrection.msg new file mode 100644 index 00000000..bfbc8e2e --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorCorrection.msg @@ -0,0 +1,41 @@ +# +# Sensor corrections in SI-unit form for the voted sensor +# + +uint64 timestamp # time since system start (microseconds) + +# Corrections for acceleromter acceleration outputs where corrected_accel = raw_accel * accel_scale + accel_offset +# Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame +uint32[4] accel_device_ids +float32[4] accel_temperature +float32[3] accel_offset_0 # accelerometer 0 offsets in the FRD board frame XYZ-axis in m/s^s +float32[3] accel_offset_1 # accelerometer 1 offsets in the FRD board frame XYZ-axis in m/s^s +float32[3] accel_offset_2 # accelerometer 2 offsets in the FRD board frame XYZ-axis in m/s^s +float32[3] accel_offset_3 # accelerometer 3 offsets in the FRD board frame XYZ-axis in m/s^s + +# Corrections for gyro angular rate outputs where corrected_rate = raw_rate * gyro_scale + gyro_offset +# Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame +uint32[4] gyro_device_ids +float32[4] gyro_temperature +float32[3] gyro_offset_0 # gyro 0 XYZ offsets in the sensor frame in rad/s +float32[3] gyro_offset_1 # gyro 1 XYZ offsets in the sensor frame in rad/s +float32[3] gyro_offset_2 # gyro 2 XYZ offsets in the sensor frame in rad/s +float32[3] gyro_offset_3 # gyro 3 XYZ offsets in the sensor frame in rad/s + +# Corrections for magnetometer measurement outputs where corrected_mag = raw_mag * mag_scale + mag_offset +# Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame +uint32[4] mag_device_ids +float32[4] mag_temperature +float32[3] mag_offset_0 # magnetometer 0 offsets in the FRD board frame XYZ-axis in m/s^s +float32[3] mag_offset_1 # magnetometer 1 offsets in the FRD board frame XYZ-axis in m/s^s +float32[3] mag_offset_2 # magnetometer 2 offsets in the FRD board frame XYZ-axis in m/s^s +float32[3] mag_offset_3 # magnetometer 3 offsets in the FRD board frame XYZ-axis in m/s^s + +# Corrections for barometric pressure outputs where corrected_pressure = raw_pressure * pressure_scale + pressure_offset +# Note the corrections are in the sensor frame and must be applied before the sensor data is rotated into body frame +uint32[4] baro_device_ids +float32[4] baro_temperature +float32 baro_offset_0 # barometric pressure 0 offsets in the sensor frame in Pascals +float32 baro_offset_1 # barometric pressure 1 offsets in the sensor frame in Pascals +float32 baro_offset_2 # barometric pressure 2 offsets in the sensor frame in Pascals +float32 baro_offset_3 # barometric pressure 3 offsets in the sensor frame in Pascals diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorGnssRelative.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorGnssRelative.msg new file mode 100644 index 00000000..6d87a344 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorGnssRelative.msg @@ -0,0 +1,30 @@ +# GNSS relative positioning information in NED frame. The NED frame is defined as the local topological system at the reference station. + +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # time since system start (microseconds) + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +uint64 time_utc_usec # Timestamp (microseconds, UTC), this is the timestamp which comes from the gps module. It might be unavailable right after cold start, indicated by a value of 0 + +uint16 reference_station_id # Reference Station ID + +float32[3] position # GPS NED relative position vector (m) +float32[3] position_accuracy # Accuracy of relative position (m) + +float32 heading # Heading of the relative position vector (radians) +float32 heading_accuracy # Accuracy of heading of the relative position vector (radians) + +float32 position_length # Length of the position vector (m) +float32 accuracy_length # Accuracy of the position length (m) + +bool gnss_fix_ok # GNSS valid fix (i.e within DOP & accuracy masks) +bool differential_solution # differential corrections were applied +bool relative_position_valid +bool carrier_solution_floating # carrier phase range solution with floating ambiguities +bool carrier_solution_fixed # carrier phase range solution with fixed ambiguities +bool moving_base_mode # if the receiver is operating in moving base mode +bool reference_position_miss # extrapolated reference position was used to compute moving base solution this epoch +bool reference_observations_miss # extrapolated reference observations were used to compute moving base solution this epoch +bool heading_valid +bool relative_position_normalized # the components of the relative position vector (including the high-precision parts) are normalized diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorGps.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorGps.msg new file mode 100644 index 00000000..ce2bfad4 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorGps.msg @@ -0,0 +1,72 @@ +# GPS position in WGS84 coordinates. +# the field 'timestamp' is for the position & velocity (microseconds) +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +float64 latitude_deg # Latitude in degrees, allows centimeter level RTK precision +float64 longitude_deg # Longitude in degrees, allows centimeter level RTK precision +float64 altitude_msl_m # Altitude above MSL, meters +float64 altitude_ellipsoid_m # Altitude above Ellipsoid, meters + +float32 s_variance_m_s # GPS speed accuracy estimate, (metres/sec) +float32 c_variance_rad # GPS course accuracy estimate, (radians) +uint8 FIX_TYPE_NONE = 1 # Value 0 is also valid to represent no fix. +uint8 FIX_TYPE_2D = 2 +uint8 FIX_TYPE_3D = 3 +uint8 FIX_TYPE_RTCM_CODE_DIFFERENTIAL = 4 +uint8 FIX_TYPE_RTK_FLOAT = 5 +uint8 FIX_TYPE_RTK_FIXED = 6 +uint8 FIX_TYPE_EXTRAPOLATED = 8 +uint8 fix_type # Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + +float32 eph # GPS horizontal position accuracy (metres) +float32 epv # GPS vertical position accuracy (metres) + +float32 hdop # Horizontal dilution of precision +float32 vdop # Vertical dilution of precision + +int32 noise_per_ms # GPS noise per millisecond +uint16 automatic_gain_control # Automatic gain control monitor + +uint8 JAMMING_STATE_UNKNOWN = 0 +uint8 JAMMING_STATE_OK = 1 +uint8 JAMMING_STATE_WARNING = 2 +uint8 JAMMING_STATE_CRITICAL = 3 +uint8 jamming_state # indicates whether jamming has been detected or suspected by the receivers. O: Unknown, 1: OK, 2: Warning, 3: Critical +int32 jamming_indicator # indicates jamming is occurring + +uint8 SPOOFING_STATE_UNKNOWN = 0 +uint8 SPOOFING_STATE_NONE = 1 +uint8 SPOOFING_STATE_INDICATED = 2 +uint8 SPOOFING_STATE_MULTIPLE = 3 +uint8 spoofing_state # indicates whether spoofing has been detected or suspected by the receivers. O: Unknown, 1: OK, 2: Warning, 3: Critical + +float32 vel_m_s # GPS ground speed, (metres/sec) +float32 vel_n_m_s # GPS North velocity, (metres/sec) +float32 vel_e_m_s # GPS East velocity, (metres/sec) +float32 vel_d_m_s # GPS Down velocity, (metres/sec) +float32 cog_rad # Course over ground (NOT heading, but direction of movement), -PI..PI, (radians) +bool vel_ned_valid # True if NED velocity is valid + +int32 timestamp_time_relative # timestamp + timestamp_time_relative = Time of the UTC timestamp since system start, (microseconds) +uint64 time_utc_usec # Timestamp (microseconds, UTC), this is the timestamp which comes from the gps module. It might be unavailable right after cold start, indicated by a value of 0 + +uint8 satellites_used # Number of satellites used + +float32 heading # heading angle of XYZ body frame rel to NED. Set to NaN if not available and updated (used for dual antenna GPS), (rad, [-PI, PI]) +float32 heading_offset # heading offset of dual antenna array in body frame. Set to NaN if not applicable. (rad, [-PI, PI]) +float32 heading_accuracy # heading accuracy (rad, [0, 2PI]) + +float32 rtcm_injection_rate # RTCM message injection rate Hz +uint8 selected_rtcm_instance # uorb instance that is being used for RTCM corrections + +bool rtcm_crc_failed # RTCM message CRC failure detected + +uint8 RTCM_MSG_USED_UNKNOWN = 0 +uint8 RTCM_MSG_USED_NOT_USED = 1 +uint8 RTCM_MSG_USED_USED = 2 +uint8 rtcm_msg_used # Indicates if the RTCM message was used successfully by the receiver + +# TOPICS sensor_gps vehicle_gps_position diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorGyro.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorGyro.msg new file mode 100644 index 00000000..b906127b --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorGyro.msg @@ -0,0 +1,18 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +float32 x # angular velocity in the FRD board frame X-axis in rad/s +float32 y # angular velocity in the FRD board frame Y-axis in rad/s +float32 z # angular velocity in the FRD board frame Z-axis in rad/s + +float32 temperature # temperature in degrees Celsius + +uint32 error_count + +uint8[3] clip_counter # clip count per axis in the sample period + +uint8 samples # number of raw samples that went into this message + +uint8 ORB_QUEUE_LENGTH = 8 diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorGyroFft.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorGyroFft.msg new file mode 100644 index 00000000..ed84d0a0 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorGyroFft.msg @@ -0,0 +1,15 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +float32 sensor_sample_rate_hz +float32 resolution_hz + +float32[3] peak_frequencies_x # x axis peak frequencies +float32[3] peak_frequencies_y # y axis peak frequencies +float32[3] peak_frequencies_z # z axis peak frequencies + +float32[3] peak_snr_x # x axis peak SNR +float32[3] peak_snr_y # y axis peak SNR +float32[3] peak_snr_z # z axis peak SNR diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorGyroFifo.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorGyroFifo.msg new file mode 100644 index 00000000..2e77ef07 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorGyroFifo.msg @@ -0,0 +1,15 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +float32 dt # delta time between samples (microseconds) +float32 scale + +uint8 samples # number of valid samples + +int16[32] x # angular velocity in the FRD board frame X-axis in rad/s +int16[32] y # angular velocity in the FRD board frame Y-axis in rad/s +int16[32] z # angular velocity in the FRD board frame Z-axis in rad/s + +uint8 ORB_QUEUE_LENGTH = 4 diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorHygrometer.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorHygrometer.msg new file mode 100644 index 00000000..490a7402 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorHygrometer.msg @@ -0,0 +1,8 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +float32 temperature # Temperature provided by sensor (Celsius) + +float32 humidity # Humidity provided by sensor diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorMag.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorMag.msg new file mode 100644 index 00000000..1b5ba487 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorMag.msg @@ -0,0 +1,14 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +float32 x # magnetic field in the FRD board frame X-axis in Gauss +float32 y # magnetic field in the FRD board frame Y-axis in Gauss +float32 z # magnetic field in the FRD board frame Z-axis in Gauss + +float32 temperature # temperature in degrees Celsius + +uint32 error_count + +uint8 ORB_QUEUE_LENGTH = 4 diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorOpticalFlow.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorOpticalFlow.msg new file mode 100644 index 00000000..ce7e8bf0 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorOpticalFlow.msg @@ -0,0 +1,30 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +float32[2] pixel_flow # (radians) optical flow in radians where a positive value is produced by a RH rotation about the body axis + +float32[3] delta_angle # (radians) accumulated gyro radians where a positive value is produced by a RH rotation about the body axis. Set to NaN if flow sensor does not have 3-axis gyro data. +bool delta_angle_available + +float32 distance_m # (meters) Distance to the center of the flow field +bool distance_available + +uint32 integration_timespan_us # (microseconds) accumulation timespan in microseconds + +uint8 quality # quality, 0: bad quality, 255: maximum quality + +uint32 error_count + +float32 max_flow_rate # (radians/s) Magnitude of maximum angular which the optical flow sensor can measure reliably + +float32 min_ground_distance # (meters) Minimum distance from ground at which the optical flow sensor operates reliably +float32 max_ground_distance # (meters) Maximum distance from ground at which the optical flow sensor operates reliably + +uint8 MODE_UNKNOWN = 0 +uint8 MODE_BRIGHT = 1 +uint8 MODE_LOWLIGHT = 2 +uint8 MODE_SUPER_LOWLIGHT = 3 + +uint8 mode diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorPreflightMag.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorPreflightMag.msg new file mode 100644 index 00000000..2b5333c4 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorPreflightMag.msg @@ -0,0 +1,7 @@ +# +# Pre-flight sensor check metrics. +# The topic will not be updated when the vehicle is armed +# +uint64 timestamp # time since system start (microseconds) + +float32 mag_inconsistency_angle # maximum angle between magnetometer instance field vectors in radians. diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorSelection.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorSelection.msg new file mode 100644 index 00000000..799ccf18 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorSelection.msg @@ -0,0 +1,7 @@ +# +# Sensor ID's for the voted sensors output on the sensor_combined topic. +# Will be updated on startup of the sensor module and when sensor selection changes +# +uint64 timestamp # time since system start (microseconds) +uint32 accel_device_id # unique device ID for the selected accelerometers +uint32 gyro_device_id # unique device ID for the selected rate gyros diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorUwb.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorUwb.msg new file mode 100644 index 00000000..ae889a8b --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorUwb.msg @@ -0,0 +1,34 @@ +# UWB distance contains the distance information measured by an ultra-wideband positioning system, +# such as Pozyx or NXP Rddrone. + +uint64 timestamp # time since system start (microseconds) + +uint32 sessionid # UWB SessionID +uint32 time_offset # Time between Ranging Rounds in ms +uint32 counter # Number of Ranges since last Start of Ranging +uint16 mac # MAC adress of Initiator (controller) + +uint16 mac_dest # MAC adress of Responder (Controlee) +uint16 status # status feedback # +uint8 nlos # None line of site condition y/n +float32 distance # distance in m to the UWB receiver + + +#Angle of arrival, Angle in Degree -60..+60; FOV in both axis is 120 degrees +float32 aoa_azimuth_dev # Angle of arrival of first incomming RX msg +float32 aoa_elevation_dev # Angle of arrival of first incomming RX msg +float32 aoa_azimuth_resp # Angle of arrival of first incomming RX msg at the responder +float32 aoa_elevation_resp # Angle of arrival of first incomming RX msg at the responder + +# Figure of merit for the angle measurements +uint8 aoa_azimuth_fom # AOA Azimuth FOM +uint8 aoa_elevation_fom # AOA Elevation FOM +uint8 aoa_dest_azimuth_fom # AOA Azimuth FOM +uint8 aoa_dest_elevation_fom # AOA Elevation FOM + +# Initiator physical configuration +uint8 orientation # Direction the sensor faces from MAV_SENSOR_ORIENTATION enum + # Standard configuration is Antennas facing down and azimuth aligened in forward direction +float32 offset_x # UWB initiator offset in X axis (NED drone frame) +float32 offset_y # UWB initiator offset in Y axis (NED drone frame) +float32 offset_z # UWB initiator offset in Z axis (NED drone frame) diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorsStatus.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorsStatus.msg new file mode 100644 index 00000000..c16bf1c6 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorsStatus.msg @@ -0,0 +1,15 @@ +# +# Sensor check metrics. This will be zero for a sensor that's primary or unpopulated. +# +uint64 timestamp # time since system start (microseconds) + +uint32 device_id_primary # current primary device id for reference + +uint32[4] device_ids +float32[4] inconsistency # magnitude of difference between sensor instance and mean +bool[4] healthy # sensor healthy +uint8[4] priority +bool[4] enabled +bool[4] external + +# TOPICS sensors_status_baro sensors_status_mag diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorsStatusImu.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorsStatusImu.msg new file mode 100644 index 00000000..cfad3419 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SensorsStatusImu.msg @@ -0,0 +1,18 @@ +# +# Sensor check metrics. This will be zero for a sensor that's primary or unpopulated. +# +uint64 timestamp # time since system start (microseconds) + +uint32 accel_device_id_primary # current primary accel device id for reference + +uint32[4] accel_device_ids +float32[4] accel_inconsistency_m_s_s # magnitude of acceleration difference between IMU instance and mean in m/s^2. +bool[4] accel_healthy +uint8[4] accel_priority + +uint32 gyro_device_id_primary # current primary gyro device id for reference + +uint32[4] gyro_device_ids +float32[4] gyro_inconsistency_rad_s # magnitude of angular rate difference between IMU instance and mean in (rad/s). +bool[4] gyro_healthy +uint8[4] gyro_priority diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SystemPower.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SystemPower.msg new file mode 100644 index 00000000..21b35ba1 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/SystemPower.msg @@ -0,0 +1,21 @@ +uint64 timestamp # time since system start (microseconds) +float32 voltage5v_v # peripheral 5V rail voltage +float32[4] sensors3v3 # Sensors 3V3 rail voltage +uint8 sensors3v3_valid # Sensors 3V3 rail voltage was read (bitfield). +uint8 usb_connected # USB is connected when 1 +uint8 brick_valid # brick bits power is good when bit 1 +uint8 usb_valid # USB is valid when 1 +uint8 servo_valid # servo power is good when 1 +uint8 periph_5v_oc # peripheral overcurrent when 1 +uint8 hipower_5v_oc # high power peripheral overcurrent when 1 +uint8 comp_5v_valid # 5V to companion valid +uint8 can1_gps1_5v_valid # 5V for CAN1/GPS1 valid + +uint8 BRICK1_VALID_SHIFTS=0 +uint8 BRICK1_VALID_MASK=1 +uint8 BRICK2_VALID_SHIFTS=1 +uint8 BRICK2_VALID_MASK=2 +uint8 BRICK3_VALID_SHIFTS=2 +uint8 BRICK3_VALID_MASK=4 +uint8 BRICK4_VALID_SHIFTS=3 +uint8 BRICK4_VALID_MASK=8 diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/TakeoffStatus.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/TakeoffStatus.msg new file mode 100644 index 00000000..4cc49d50 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/TakeoffStatus.msg @@ -0,0 +1,14 @@ +# Status of the takeoff state machine currently just available for multicopters + +uint64 timestamp # time since system start (microseconds) + +uint8 TAKEOFF_STATE_UNINITIALIZED = 0 +uint8 TAKEOFF_STATE_DISARMED = 1 +uint8 TAKEOFF_STATE_SPOOLUP = 2 +uint8 TAKEOFF_STATE_READY_FOR_TAKEOFF = 3 +uint8 TAKEOFF_STATE_RAMPUP = 4 +uint8 TAKEOFF_STATE_FLIGHT = 5 + +uint8 takeoff_state + +float32 tilt_limit # limited tilt feasibility during takeoff, contains maximum tilt otherwise diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/TaskStackInfo.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/TaskStackInfo.msg new file mode 100644 index 00000000..bb69bf1a --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/TaskStackInfo.msg @@ -0,0 +1,8 @@ +# stack information for a single running process + +uint64 timestamp # time since system start (microseconds) + +uint16 stack_free +char[24] task_name + +uint8 ORB_QUEUE_LENGTH = 2 diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/TecsStatus.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/TecsStatus.msg new file mode 100644 index 00000000..ae6835dc --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/TecsStatus.msg @@ -0,0 +1,29 @@ +uint64 timestamp # time since system start (microseconds) + +float32 altitude_sp # Altitude setpoint AMSL [m] +float32 altitude_reference # Altitude setpoint reference AMSL [m] +float32 height_rate_reference # Height rate setpoint reference [m/s] +float32 height_rate_direct # Direct height rate setpoint from velocity reference generator [m/s] +float32 height_rate_setpoint # Height rate setpoint [m/s] +float32 height_rate # Height rate [m/s] +float32 equivalent_airspeed_sp # Equivalent airspeed setpoint [m/s] +float32 true_airspeed_sp # True airspeed setpoint [m/s] +float32 true_airspeed_filtered # True airspeed filtered [m/s] +float32 true_airspeed_derivative_sp # True airspeed derivative setpoint [m/s^2] +float32 true_airspeed_derivative # True airspeed derivative [m/s^2] +float32 true_airspeed_derivative_raw # True airspeed derivative raw [m/s^2] + +float32 total_energy_rate_sp # Total energy rate setpoint [m^2/s^3] +float32 total_energy_rate # Total energy rate estimate [m^2/s^3] + +float32 total_energy_balance_rate_sp # Energy balance rate setpoint [m^2/s^3] +float32 total_energy_balance_rate # Energy balance rate estimate [m^2/s^3] + +float32 throttle_integ # Throttle integrator value [-] +float32 pitch_integ # Pitch integrator value [rad] + +float32 throttle_sp # Current throttle setpoint [-] +float32 pitch_sp_rad # Current pitch setpoint [rad] +float32 throttle_trim # estimated throttle value [0,1] required to fly level at equivalent_airspeed_sp in the current atmospheric conditions + +float32 underspeed_ratio # 0: no underspeed, 1: maximal underspeed. Controller takes measures to avoid stall proportional to ratio if >0. diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/TelemetryStatus.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/TelemetryStatus.msg new file mode 100644 index 00000000..48d85f93 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/TelemetryStatus.msg @@ -0,0 +1,63 @@ +uint8 LINK_TYPE_GENERIC = 0 +uint8 LINK_TYPE_UBIQUITY_BULLET = 1 +uint8 LINK_TYPE_WIRE = 2 +uint8 LINK_TYPE_USB = 3 +uint8 LINK_TYPE_IRIDIUM = 4 + +uint64 timestamp # time since system start (microseconds) + +uint8 type # type of the radio hardware (LINK_TYPE_*) + +uint8 mode + +bool flow_control +bool forwarding +bool mavlink_v2 +bool ftp + +uint8 streams + +float32 data_rate # configured maximum data rate (Bytes/s) + +float32 rate_multiplier + +float32 tx_rate_avg # transmit rate average (Bytes/s) +float32 tx_error_rate_avg # transmit error rate average (Bytes/s) +uint32 tx_message_count # total message sent count +uint32 tx_buffer_overruns # number of TX buffer overruns + +float32 rx_rate_avg # transmit rate average (Bytes/s) +uint32 rx_message_count # count of total messages received +uint32 rx_message_lost_count +uint32 rx_buffer_overruns # number of RX buffer overruns +uint32 rx_parse_errors # number of parse errors +uint32 rx_packet_drop_count # number of packet drops +float32 rx_message_lost_rate + + +uint64 HEARTBEAT_TIMEOUT_US = 2500000 # Heartbeat timeout (tolerate missing 1 + jitter) + +# Heartbeats per type +bool heartbeat_type_antenna_tracker # MAV_TYPE_ANTENNA_TRACKER +bool heartbeat_type_gcs # MAV_TYPE_GCS +bool heartbeat_type_onboard_controller # MAV_TYPE_ONBOARD_CONTROLLER +bool heartbeat_type_gimbal # MAV_TYPE_GIMBAL +bool heartbeat_type_adsb # MAV_TYPE_ADSB +bool heartbeat_type_camera # MAV_TYPE_CAMERA +bool heartbeat_type_parachute # MAV_TYPE_PARACHUTE +bool heartbeat_type_open_drone_id # MAV_TYPE_ODID + +# Heartbeats per component +bool heartbeat_component_telemetry_radio # MAV_COMP_ID_TELEMETRY_RADIO +bool heartbeat_component_log # MAV_COMP_ID_LOG +bool heartbeat_component_osd # MAV_COMP_ID_OSD +bool heartbeat_component_obstacle_avoidance # MAV_COMP_ID_OBSTACLE_AVOIDANCE +bool heartbeat_component_vio # MAV_COMP_ID_VISUAL_INERTIAL_ODOMETRY +bool heartbeat_component_pairing_manager # MAV_COMP_ID_PAIRING_MANAGER +bool heartbeat_component_udp_bridge # MAV_COMP_ID_UDP_BRIDGE +bool heartbeat_component_uart_bridge # MAV_COMP_ID_UART_BRIDGE + +# Misc component health +bool avoidance_system_healthy +bool open_drone_id_system_healthy +bool parachute_system_healthy diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/TiltrotorExtraControls.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/TiltrotorExtraControls.msg new file mode 100644 index 00000000..20af316c --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/TiltrotorExtraControls.msg @@ -0,0 +1,4 @@ +uint64 timestamp # time since system start (microseconds) + +float32 collective_tilt_normalized_setpoint # Collective tilt angle of motors of tiltrotor, 0: vertical, 1: horizontal [0, 1] +float32 collective_thrust_normalized_setpoint # Collective thrust setpoint [0, 1] diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/TimesyncStatus.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/TimesyncStatus.msg new file mode 100644 index 00000000..71e84e85 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/TimesyncStatus.msg @@ -0,0 +1,11 @@ +uint64 timestamp # time since system start (microseconds) + +uint8 SOURCE_PROTOCOL_UNKNOWN = 0 +uint8 SOURCE_PROTOCOL_MAVLINK = 1 +uint8 SOURCE_PROTOCOL_DDS = 2 +uint8 source_protocol # timesync source + +uint64 remote_timestamp # remote system timestamp (microseconds) +int64 observed_offset # raw time offset directly observed from this timesync packet (microseconds) +int64 estimated_offset # smoothed time offset between companion system and PX4 (microseconds) +uint32 round_trip_time # round trip time of this timesync packet (microseconds) diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/TrajectoryBezier.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/TrajectoryBezier.msg new file mode 100644 index 00000000..e3d9d4e0 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/TrajectoryBezier.msg @@ -0,0 +1,8 @@ +# Bezier Trajectory description. See also Mavlink TRAJECTORY msg +# The topic trajectory_bezier describe each waypoint defined in vehicle_trajectory_bezier + +uint64 timestamp # time since system start (microseconds) + +float32[3] position # local position x,y,z (metres) +float32 yaw # yaw angle (rad) +float32 delta # time it should take to get to this waypoint, if this is the final waypoint (seconds) diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/TrajectorySetpoint.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/TrajectorySetpoint.msg new file mode 100644 index 00000000..4a88c867 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/TrajectorySetpoint.msg @@ -0,0 +1,15 @@ +# Trajectory setpoint in NED frame +# Input to PID position controller. +# Needs to be kinematically consistent and feasible for smooth flight. +# setting a value to NaN means the state should not be controlled + +uint64 timestamp # time since system start (microseconds) + +# NED local world frame +float32[3] position # in meters +float32[3] velocity # in meters/second +float32[3] acceleration # in meters/second^2 +float32[3] jerk # in meters/second^3 (for logging only) + +float32 yaw # euler angle of desired attitude in radians -PI..+PI +float32 yawspeed # angular velocity around NED frame z-axis in radians/second diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/TrajectoryWaypoint.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/TrajectoryWaypoint.msg new file mode 100644 index 00000000..6ea9bae4 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/TrajectoryWaypoint.msg @@ -0,0 +1,13 @@ +# Waypoint Trajectory description. See also Mavlink TRAJECTORY msg +# The topic trajectory_waypoint describe each waypoint defined in vehicle_trajectory_waypoint + +uint64 timestamp # time since system start (microseconds) + +float32[3] position +float32[3] velocity +float32[3] acceleration +float32 yaw +float32 yaw_speed + +bool point_valid +uint8 type diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/TransponderReport.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/TransponderReport.msg new file mode 100644 index 00000000..d5171cf3 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/TransponderReport.msg @@ -0,0 +1,50 @@ +uint64 timestamp # time since system start (microseconds) +uint32 icao_address # ICAO address +float64 lat # Latitude, expressed as degrees +float64 lon # Longitude, expressed as degrees +uint8 altitude_type # Type from ADSB_ALTITUDE_TYPE enum +float32 altitude # Altitude(ASL) in meters +float32 heading # Course over ground in radians, -pi to +pi, 0 is north +float32 hor_velocity # The horizontal velocity in m/s +float32 ver_velocity # The vertical velocity in m/s, positive is up +char[9] callsign # The callsign, 8+null +uint8 emitter_type # Type from ADSB_EMITTER_TYPE enum +uint8 tslc # Time since last communication in seconds +uint16 flags # Flags to indicate various statuses including valid data fields +uint16 squawk # Squawk code +uint8[18] uas_id # Unique UAS ID + +# ADSB flags +uint16 PX4_ADSB_FLAGS_VALID_COORDS = 1 +uint16 PX4_ADSB_FLAGS_VALID_ALTITUDE = 2 +uint16 PX4_ADSB_FLAGS_VALID_HEADING = 4 +uint16 PX4_ADSB_FLAGS_VALID_VELOCITY = 8 +uint16 PX4_ADSB_FLAGS_VALID_CALLSIGN = 16 +uint16 PX4_ADSB_FLAGS_VALID_SQUAWK = 32 +uint16 PX4_ADSB_FLAGS_RETRANSLATE = 256 + +#ADSB Emitter Data: +#from mavlink/v2.0/common/common.h +uint16 ADSB_EMITTER_TYPE_NO_INFO=0 +uint16 ADSB_EMITTER_TYPE_LIGHT=1 +uint16 ADSB_EMITTER_TYPE_SMALL=2 +uint16 ADSB_EMITTER_TYPE_LARGE=3 +uint16 ADSB_EMITTER_TYPE_HIGH_VORTEX_LARGE=4 +uint16 ADSB_EMITTER_TYPE_HEAVY=5 +uint16 ADSB_EMITTER_TYPE_HIGHLY_MANUV=6 +uint16 ADSB_EMITTER_TYPE_ROTOCRAFT=7 +uint16 ADSB_EMITTER_TYPE_UNASSIGNED=8 +uint16 ADSB_EMITTER_TYPE_GLIDER=9 +uint16 ADSB_EMITTER_TYPE_LIGHTER_AIR=10 +uint16 ADSB_EMITTER_TYPE_PARACHUTE=11 +uint16 ADSB_EMITTER_TYPE_ULTRA_LIGHT=12 +uint16 ADSB_EMITTER_TYPE_UNASSIGNED2=13 +uint16 ADSB_EMITTER_TYPE_UAV=14 +uint16 ADSB_EMITTER_TYPE_SPACE=15 +uint16 ADSB_EMITTER_TYPE_UNASSGINED3=16 +uint16 ADSB_EMITTER_TYPE_EMERGENCY_SURFACE=17 +uint16 ADSB_EMITTER_TYPE_SERVICE_SURFACE=18 +uint16 ADSB_EMITTER_TYPE_POINT_OBSTACLE=19 +uint16 ADSB_EMITTER_TYPE_ENUM_END=20 + +uint8 ORB_QUEUE_LENGTH = 16 diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/TuneControl.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/TuneControl.msg new file mode 100644 index 00000000..96d70af1 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/TuneControl.msg @@ -0,0 +1,39 @@ +# This message is used to control the tunes, when the tune_id is set to CUSTOM +# then the frequency, duration are used otherwise those values are ignored. + +uint64 timestamp # time since system start (microseconds) + +uint8 TUNE_ID_STOP = 0 +uint8 TUNE_ID_STARTUP = 1 +uint8 TUNE_ID_ERROR = 2 +uint8 TUNE_ID_NOTIFY_POSITIVE = 3 +uint8 TUNE_ID_NOTIFY_NEUTRAL = 4 +uint8 TUNE_ID_NOTIFY_NEGATIVE = 5 +uint8 TUNE_ID_ARMING_WARNING = 6 +uint8 TUNE_ID_BATTERY_WARNING_SLOW = 7 +uint8 TUNE_ID_BATTERY_WARNING_FAST = 8 +uint8 TUNE_ID_GPS_WARNING = 9 +uint8 TUNE_ID_ARMING_FAILURE = 10 +uint8 TUNE_ID_PARACHUTE_RELEASE = 11 +uint8 TUNE_ID_SINGLE_BEEP = 12 +uint8 TUNE_ID_HOME_SET = 13 +uint8 TUNE_ID_SD_INIT = 14 +uint8 TUNE_ID_SD_ERROR = 15 +uint8 TUNE_ID_PROG_PX4IO = 16 +uint8 TUNE_ID_PROG_PX4IO_OK = 17 +uint8 TUNE_ID_PROG_PX4IO_ERR = 18 +uint8 TUNE_ID_POWER_OFF = 19 +uint8 NUMBER_OF_TUNES = 20 + +uint8 tune_id # tune_id corresponding to TuneID::* from the tune_defaults.h in the tunes library +bool tune_override # if true the tune which is playing will be stopped and the new started +uint16 frequency # in Hz +uint32 duration # in us +uint32 silence # in us +uint8 volume # value between 0-100 if supported by backend + +uint8 VOLUME_LEVEL_MIN = 0 +uint8 VOLUME_LEVEL_DEFAULT = 20 +uint8 VOLUME_LEVEL_MAX = 100 + +uint8 ORB_QUEUE_LENGTH = 4 diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/UavcanParameterRequest.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/UavcanParameterRequest.msg new file mode 100644 index 00000000..3cfec15f --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/UavcanParameterRequest.msg @@ -0,0 +1,23 @@ +# UAVCAN-MAVLink parameter bridge request type +uint64 timestamp # time since system start (microseconds) + +uint8 MESSAGE_TYPE_PARAM_REQUEST_READ = 20 # MAVLINK_MSG_ID_PARAM_REQUEST_READ +uint8 MESSAGE_TYPE_PARAM_REQUEST_LIST = 21 # MAVLINK_MSG_ID_PARAM_REQUEST_LIST +uint8 MESSAGE_TYPE_PARAM_SET = 23 # MAVLINK_MSG_ID_PARAM_SET +uint8 message_type # MAVLink message type: PARAM_REQUEST_READ, PARAM_REQUEST_LIST, PARAM_SET + +uint8 NODE_ID_ALL = 0 # MAV_COMP_ID_ALL +uint8 node_id # UAVCAN node ID mapped from MAVLink component ID + +char[17] param_id # MAVLink/UAVCAN parameter name +int16 param_index # -1 if the param_id field should be used as identifier + +uint8 PARAM_TYPE_UINT8 = 1 # MAV_PARAM_TYPE_UINT8 +uint8 PARAM_TYPE_INT64 = 8 # MAV_PARAM_TYPE_INT64 +uint8 PARAM_TYPE_REAL32 = 9 # MAV_PARAM_TYPE_REAL32 +uint8 param_type # MAVLink parameter type + +int64 int_value # current value if param_type is int-like +float32 real_value # current value if param_type is float-like + +uint8 ORB_QUEUE_LENGTH = 4 diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/UavcanParameterValue.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/UavcanParameterValue.msg new file mode 100644 index 00000000..8eff663a --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/UavcanParameterValue.msg @@ -0,0 +1,9 @@ +# UAVCAN-MAVLink parameter bridge response type +uint64 timestamp # time since system start (microseconds) +uint8 node_id # UAVCAN node ID mapped from MAVLink component ID +char[17] param_id # MAVLink/UAVCAN parameter name +int16 param_index # parameter index, if known +uint16 param_count # number of parameters exposed by the node +uint8 param_type # MAVLink parameter type +int64 int_value # current value if param_type is int-like +float32 real_value # current value if param_type is float-like diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/UlogStream.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/UlogStream.msg new file mode 100644 index 00000000..d206b4a4 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/UlogStream.msg @@ -0,0 +1,19 @@ +# Message to stream ULog data from the logger. Corresponds to the LOGGING_DATA +# mavlink message + +uint64 timestamp # time since system start (microseconds) + +# flags bitmasks +uint8 FLAGS_NEED_ACK = 1 # if set, this message requires to be acked. + # Acked messages are published synchronous: a + # publisher waits for an ack before sending the + # next message + +uint8 length # length of data +uint8 first_message_offset # offset into data where first message starts. This + # can be used for recovery, when a previous message got lost +uint16 msg_sequence # allows determine drops +uint8 flags # see FLAGS_* +uint8[249] data # ulog data + +uint8 ORB_QUEUE_LENGTH = 16 # TODO: we might be able to reduce this if mavlink polled on the topic diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/UlogStreamAck.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/UlogStreamAck.msg new file mode 100644 index 00000000..e3747fff --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/UlogStreamAck.msg @@ -0,0 +1,8 @@ +# Ack a previously sent ulog_stream message that had +# the NEED_ACK flag set + +uint64 timestamp # time since system start (microseconds) +int32 ACK_TIMEOUT = 50 # timeout waiting for an ack until we retry to send the message [ms] +int32 ACK_MAX_TRIES = 50 # maximum amount of tries to (re-)send a message, each time waiting ACK_TIMEOUT ms + +uint16 msg_sequence diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/UnregisterExtComponent.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/UnregisterExtComponent.msg new file mode 100644 index 00000000..2ad78d4b --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/UnregisterExtComponent.msg @@ -0,0 +1,7 @@ +uint64 timestamp # time since system start (microseconds) + +char[25] name # either the mode name, or component name + +int8 arming_check_id # arming check registration ID (-1 if not registered) +int8 mode_id # assigned mode ID (-1 if not registered) +int8 mode_executor_id # assigned mode executor ID (-1 if not registered) diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleAcceleration.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleAcceleration.msg new file mode 100644 index 00000000..7d555d7f --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleAcceleration.msg @@ -0,0 +1,6 @@ + +uint64 timestamp # time since system start (microseconds) + +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +float32[3] xyz # Bias corrected acceleration (including gravity) in the FRD body frame XYZ-axis in m/s^2 diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleAirData.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleAirData.msg new file mode 100644 index 00000000..59ca5e5c --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleAirData.msg @@ -0,0 +1,15 @@ + +uint64 timestamp # time since system start (microseconds) + +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +uint32 baro_device_id # unique device ID for the selected barometer + +float32 baro_alt_meter # Altitude above MSL calculated from temperature compensated baro sensor data using an ISA corrected for sea level pressure SENS_BARO_QNH. +float32 baro_temp_celcius # Temperature in degrees Celsius +float32 baro_pressure_pa # Absolute pressure in Pascals + +float32 rho # air density +float32 eas2tas # equivalent airspeed to true airspeed conversion factor + +uint8 calibration_count # Calibration changed counter. Monotonically increases whenever calibration changes. diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleAngularAccelerationSetpoint.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleAngularAccelerationSetpoint.msg new file mode 100644 index 00000000..94da11b8 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleAngularAccelerationSetpoint.msg @@ -0,0 +1,5 @@ + +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds) + +float32[3] xyz # angular acceleration about X, Y, Z body axis in rad/s^2 diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleAngularVelocity.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleAngularVelocity.msg new file mode 100644 index 00000000..db3767c0 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleAngularVelocity.msg @@ -0,0 +1,9 @@ + +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds) + +float32[3] xyz # Bias corrected angular velocity about the FRD body frame XYZ-axis in rad/s + +float32[3] xyz_derivative # angular acceleration about the FRD body frame XYZ-axis in rad/s^2 + +# TOPICS vehicle_angular_velocity vehicle_angular_velocity_groundtruth diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleAttitude.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleAttitude.msg new file mode 100644 index 00000000..99e6f25c --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleAttitude.msg @@ -0,0 +1,13 @@ +# This is similar to the mavlink message ATTITUDE_QUATERNION, but for onboard use +# The quaternion uses the Hamilton convention, and the order is q(w, x, y, z) + +uint64 timestamp # time since system start (microseconds) + +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +float32[4] q # Quaternion rotation from the FRD body frame to the NED earth frame +float32[4] delta_q_reset # Amount by which quaternion has changed during last reset +uint8 quat_reset_counter # Quaternion reset counter + +# TOPICS vehicle_attitude vehicle_attitude_groundtruth external_ins_attitude +# TOPICS estimator_attitude diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleAttitudeSetpoint.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleAttitudeSetpoint.msg new file mode 100644 index 00000000..f52025d2 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleAttitudeSetpoint.msg @@ -0,0 +1,20 @@ +uint64 timestamp # time since system start (microseconds) + +float32 roll_body # body angle in NED frame (can be NaN for FW) +float32 pitch_body # body angle in NED frame (can be NaN for FW) +float32 yaw_body # body angle in NED frame (can be NaN for FW) + +float32 yaw_sp_move_rate # rad/s (commanded by user) + +# For quaternion-based attitude control +float32[4] q_d # Desired quaternion for quaternion control + +# For clarification: For multicopters thrust_body[0] and thrust[1] are usually 0 and thrust[2] is the negative throttle demand. +# For fixed wings thrust_x is the throttle demand and thrust_y, thrust_z will usually be zero. +float32[3] thrust_body # Normalized thrust command in body FRD frame [-1,1] + +bool reset_integral # Reset roll/pitch/yaw integrals (navigation logic change) + +bool fw_control_yaw_wheel # control heading with steering wheel (used for auto takeoff on runway) + +# TOPICS vehicle_attitude_setpoint mc_virtual_attitude_setpoint fw_virtual_attitude_setpoint diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleCommand.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleCommand.msg new file mode 100644 index 00000000..b147bef0 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleCommand.msg @@ -0,0 +1,187 @@ +# Vehicle Command uORB message. Used for commanding a mission / action / etc. +# Follows the MAVLink COMMAND_INT / COMMAND_LONG definition + +uint64 timestamp # time since system start (microseconds) + +uint16 VEHICLE_CMD_CUSTOM_0 = 0 # test command +uint16 VEHICLE_CMD_CUSTOM_1 = 1 # test command +uint16 VEHICLE_CMD_CUSTOM_2 = 2 # test command +uint16 VEHICLE_CMD_NAV_WAYPOINT = 16 # Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude| +uint16 VEHICLE_CMD_NAV_LOITER_UNLIM = 17 # Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| +uint16 VEHICLE_CMD_NAV_LOITER_TURNS = 18 # Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| +uint16 VEHICLE_CMD_NAV_LOITER_TIME = 19 # Loiter around this MISSION for X seconds |Seconds (decimal)| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| +uint16 VEHICLE_CMD_NAV_RETURN_TO_LAUNCH = 20 # Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_NAV_LAND = 21 # Land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude| +uint16 VEHICLE_CMD_NAV_TAKEOFF = 22 # Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| +uint16 VEHICLE_CMD_NAV_PRECLAND = 23 # Attempt a precision landing +uint16 VEHICLE_CMD_DO_ORBIT = 34 # Start orbiting on the circumference of a circle defined by the parameters. |Radius [m] |Velocity [m/s] |Yaw behaviour |Empty |Latitude/X |Longitude/Y |Altitude/Z | +uint16 VEHICLE_CMD_DO_FIGUREEIGHT = 35 # Start flying on the outline of a figure eight defined by the parameters. |Major Radius [m] |Minor Radius [m] |Velocity [m/s] |Orientation |Latitude/X |Longitude/Y |Altitude/Z | +uint16 VEHICLE_CMD_NAV_ROI = 80 # Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of interest mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| +uint16 VEHICLE_CMD_NAV_PATHPLANNING = 81 # Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| +uint16 VEHICLE_CMD_NAV_VTOL_TAKEOFF = 84 # Takeoff from ground / hand and transition to fixed wing |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| +uint16 VEHICLE_CMD_NAV_VTOL_LAND = 85 # Transition to MC and land at location |Empty| Empty| Empty| Desired yaw angle.| Latitude| Longitude| Altitude| +uint16 VEHICLE_CMD_NAV_GUIDED_LIMITS = 90 # set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, AMSL) - if vehicle moves below this alt, the command will be aborted and the mission will continue. 0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue. 0 means no upper altitude limit| horizontal move limit (in meters, AMSL) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty| +uint16 VEHICLE_CMD_NAV_GUIDED_MASTER = 91 # set id of master controller |System ID| Component ID| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_NAV_DELAY = 93 # Delay the next navigation command a number of seconds or until a specified time |Delay in seconds (decimal, -1 to enable time-of-day fields)| hour (24h format, UTC, -1 to ignore)| minute (24h format, UTC, -1 to ignore)| second (24h format, UTC)| Empty| Empty| Empty| +uint16 VEHICLE_CMD_NAV_LAST = 95 # NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_CONDITION_DELAY = 112 # Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_CONDITION_CHANGE_ALT = 113 # Ascend/descend at rate. Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude| +uint16 VEHICLE_CMD_CONDITION_DISTANCE = 114 # Delay mission state machine until within desired distance of next NAV point. |Distance (meters)| Empty| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_CONDITION_YAW = 115 # Reach a certain target angle. |target angle: [0-360], 0 is north| speed during yaw change:[deg per second]| direction: negative: counter clockwise, positive: clockwise [-1,1]| relative offset or absolute angle: [ 1,0]| Empty| Empty| Empty| +uint16 VEHICLE_CMD_CONDITION_LAST = 159 # NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_CONDITION_GATE = 4501 # Wait until passing a threshold |2D coord mode: 0: Orthogonal to planned route | Altitude mode: 0: Ignore altitude| Empty| Empty| Lat| Lon| Alt| +uint16 VEHICLE_CMD_DO_SET_MODE = 176 # Set system mode. |Mode, as defined by ENUM MAV_MODE| Empty| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_JUMP = 177 # Jump to the desired command in the mission list. Repeat this action only the specified number of times |Sequence number| Repeat count| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_CHANGE_SPEED = 178 # Change speed and/or throttle set points. |Speed type (0=Airspeed, 1=Ground Speed)| Speed (m/s, -1 indicates no change)| Throttle ( Percent, -1 indicates no change)| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_SET_HOME = 179 # Changes the home location either to the current location or a specified location. |Use current (1=use current location, 0=use specified location)| Empty| Empty| Empty| Latitude| Longitude| Altitude| +uint16 VEHICLE_CMD_DO_SET_PARAMETER = 180 # Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. |Parameter number| Parameter value| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_SET_RELAY = 181 # Set a relay to a condition. |Relay number| Setting (1=on, 0=off, others possible depending on system hardware)| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_REPEAT_RELAY = 182 # Cycle a relay on and off for a desired number of cycles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_REPEAT_SERVO = 184 # Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_FLIGHTTERMINATION = 185 # Terminate flight immediately |Flight termination activated if > 0.5| Empty| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_CHANGE_ALTITUDE = 186 # Set the vehicle to Loiter mode and change the altitude to specified value |Altitude| Frame of new altitude | Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_SET_ACTUATOR = 187 # Sets actuators (e.g. servos) to a desired value. |Actuator 1| Actuator 2| Actuator 3| Actuator 4| Actuator 5| Actuator 6| Index| +uint16 VEHICLE_CMD_DO_LAND_START = 189 # Mission command to perform a landing. This is used as a marker in a mission to tell the autopilot where a sequence of mission items that represents a landing starts. It may also be sent via a COMMAND_LONG to trigger a landing, in which case the nearest (geographically) landing sequence in the mission will be used. The Latitude/Longitude is optional, and may be set to 0/0 if not needed. If specified then it will be used to help find the closest landing sequence. |Empty| Empty| Empty| Empty| Latitude| Longitude| Empty| +uint16 VEHICLE_CMD_DO_GO_AROUND = 191 # Mission command to safely abort an autonomous landing. |Altitude (meters)| Empty| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_REPOSITION = 192 # Reposition to specific WGS84 GPS position. |Ground speed [m/s] |Bitmask |Loiter radius [m] for planes |Yaw [deg] |Latitude |Longitude |Altitude | +uint16 VEHICLE_CMD_DO_PAUSE_CONTINUE = 193 +uint16 VEHICLE_CMD_DO_SET_ROI_LOCATION = 195 # Sets the region of interest (ROI) to a location. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Empty| Empty| Empty| Empty| Latitude| Longitude| Altitude| +uint16 VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET = 196 # Sets the region of interest (ROI) to be toward next waypoint, with optional pitch/roll/yaw offset. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Empty| Empty| Empty| Empty| pitch offset from next waypoint| roll offset from next waypoint| yaw offset from next waypoint| +uint16 VEHICLE_CMD_DO_SET_ROI_NONE = 197 # Cancels any previous ROI command returning the vehicle/sensors to default flight characteristics. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Empty| Empty| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_CONTROL_VIDEO = 200 # Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_SET_ROI = 201 # Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of interest mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| +uint16 VEHICLE_CMD_DO_DIGICAM_CONTROL=203 +uint16 VEHICLE_CMD_DO_MOUNT_CONFIGURE=204 # Mission command to configure a camera or antenna mount |Mount operation mode (see MAV_MOUNT_MODE enum)| stabilize roll? (1 = yes, 0 = no)| stabilize pitch? (1 = yes, 0 = no)| stabilize yaw? (1 = yes, 0 = no)| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_MOUNT_CONTROL=205 # Mission command to control a camera or antenna mount |pitch or lat in degrees, depending on mount mode.| roll or lon in degrees depending on mount mode| yaw or alt (in meters) depending on mount mode| reserved| reserved| reserved| MAV_MOUNT_MODE enum value| +uint16 VEHICLE_CMD_DO_SET_CAM_TRIGG_DIST=206 # Mission command to set TRIG_DIST for this flight |Camera trigger distance (meters)| Shutter integration time (ms)| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_FENCE_ENABLE=207 # Mission command to enable the geofence |enable? (0=disable, 1=enable)| Empty| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_PARACHUTE=208 # Mission command to trigger a parachute |action (0=disable, 1=enable, 2=release, for some systems see PARACHUTE_ACTION enum, not in general message set.)| Empty| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_MOTOR_TEST=209 # motor test command |Instance (1, ...)| throttle type| throttle| timeout [s]| Motor count | Test order| Empty| +uint16 VEHICLE_CMD_DO_INVERTED_FLIGHT=210 # Change to/from inverted flight |inverted (0=normal, 1=inverted)| Empty| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_GRIPPER = 211 # Command to operate a gripper +uint16 VEHICLE_CMD_DO_SET_CAM_TRIGG_INTERVAL=214 # Mission command to set TRIG_INTERVAL for this flight |Camera trigger distance (meters)| Shutter integration time (ms)| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT=220 # Mission command to control a camera or antenna mount, using a quaternion as reference. |q1 - quaternion param #1, w (1 in null-rotation)| q2 - quaternion param #2, x (0 in null-rotation)| q3 - quaternion param #3, y (0 in null-rotation)| q4 - quaternion param #4, z (0 in null-rotation)| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_GUIDED_MASTER=221 # set id of master controller |System ID| Component ID| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_GUIDED_LIMITS=222 # set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, AMSL) - if vehicle moves below this alt, the command will be aborted and the mission will continue. 0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue. 0 means no upper altitude limit| horizontal move limit (in meters, AMSL) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty| +uint16 VEHICLE_CMD_DO_LAST = 240 # NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| +uint16 VEHICLE_CMD_PREFLIGHT_CALIBRATION = 241 # Trigger calibration. This command will be only accepted if in pre-flight mode. See mavlink spec MAV_CMD_PREFLIGHT_CALIBRATION +uint16 PREFLIGHT_CALIBRATION_TEMPERATURE_CALIBRATION = 3# param value for VEHICLE_CMD_PREFLIGHT_CALIBRATION to start temperature calibration +uint16 VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS = 242 # Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| +uint16 VEHICLE_CMD_PREFLIGHT_UAVCAN = 243 # UAVCAN configuration. If param 1 == 1 actuator mapping and direction assignment should be started +uint16 VEHICLE_CMD_PREFLIGHT_STORAGE = 245 # Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM| Reserved| Reserved| Empty| Empty| Empty| +uint16 VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN = 246 # Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer.| Reserved| Reserved| Empty| Empty| Empty| +uint16 VEHICLE_CMD_OBLIQUE_SURVEY=260 # Mission command to set a Camera Auto Mount Pivoting Oblique Survey for this flight|Camera trigger distance (meters)| Shutter integration time (ms)| Camera minimum trigger interval| Number of positions| Roll| Pitch| Empty| +uint16 VEHICLE_CMD_DO_SET_STANDARD_MODE=262 # Enable the specified standard MAVLink mode |MAV_STANDARD_MODE| +uint16 VEHICLE_CMD_GIMBAL_DEVICE_INFORMATION = 283 # Command to ask information about a low level gimbal + +uint16 VEHICLE_CMD_MISSION_START = 300 # start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| +uint16 VEHICLE_CMD_ACTUATOR_TEST = 310 # Actuator testing command|value [-1,1]|timeout [s]|Empty|Empty|output function| +uint16 VEHICLE_CMD_CONFIGURE_ACTUATOR = 311 # Actuator configuration command|configuration|Empty|Empty|Empty|output function| +uint16 VEHICLE_CMD_COMPONENT_ARM_DISARM = 400 # Arms / Disarms a component |1 to arm, 0 to disarm +uint16 VEHICLE_CMD_RUN_PREARM_CHECKS = 401 # Instructs a target system to run pre-arm checks. +uint16 VEHICLE_CMD_INJECT_FAILURE = 420 # Inject artificial failure for testing purposes +uint16 VEHICLE_CMD_START_RX_PAIR = 500 # Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| +uint16 VEHICLE_CMD_REQUEST_MESSAGE = 512 # Request to send a single instance of the specified message +uint16 VEHICLE_CMD_SET_CAMERA_MODE = 530 # Set camera capture mode (photo, video, etc.) +uint16 VEHICLE_CMD_SET_CAMERA_ZOOM = 531 # Set camera zoom +uint16 VEHICLE_CMD_SET_CAMERA_FOCUS = 532 +uint16 VEHICLE_CMD_DO_GIMBAL_MANAGER_PITCHYAW = 1000 # Setpoint to be sent to a gimbal manager to set a gimbal pitch and yaw +uint16 VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE = 1001 # Gimbal configuration to set which sysid/compid is in primary and secondary control +uint16 VEHICLE_CMD_IMAGE_START_CAPTURE = 2000 # Start image capture sequence. +uint16 VEHICLE_CMD_DO_TRIGGER_CONTROL = 2003 # Enable or disable on-board camera triggering system +uint16 VEHICLE_CMD_VIDEO_START_CAPTURE = 2500 # Start a video capture. +uint16 VEHICLE_CMD_VIDEO_STOP_CAPTURE = 2501 # Stop the current video capture. +uint16 VEHICLE_CMD_LOGGING_START = 2510 # start streaming ULog data +uint16 VEHICLE_CMD_LOGGING_STOP = 2511 # stop streaming ULog data +uint16 VEHICLE_CMD_CONTROL_HIGH_LATENCY = 2600 # control starting/stopping transmitting data over the high latency link +uint16 VEHICLE_CMD_DO_VTOL_TRANSITION = 3000 # Command VTOL transition +uint16 VEHICLE_CMD_ARM_AUTHORIZATION_REQUEST = 3001 # Request arm authorization +uint16 VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001 # Prepare a payload deployment in the flight plan +uint16 VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002 # Control a pre-programmed payload deployment +uint16 VEHICLE_CMD_FIXED_MAG_CAL_YAW = 42006 # Magnetometer calibration based on provided known yaw. This allows for fast calibration using WMM field tables in the vehicle, given only the known yaw of the vehicle. If Latitude and longitude are both zero then use the current vehicle location. +uint16 VEHICLE_CMD_DO_WINCH = 42600 # Command to operate winch. + +uint16 VEHICLE_CMD_EXTERNAL_POSITION_ESTIMATE = 43003 # external reset of estimator global position when deadreckoning + +# PX4 vehicle commands (beyond 16 bit mavlink commands) +uint32 VEHICLE_CMD_PX4_INTERNAL_START = 65537 # start of PX4 internal only vehicle commands (> UINT16_MAX) +uint32 VEHICLE_CMD_SET_GPS_GLOBAL_ORIGIN = 100000 # Sets the GPS coordinates of the vehicle local origin (0,0,0) position. |Empty|Empty|Empty|Empty|Latitude|Longitude|Altitude| +uint32 VEHICLE_CMD_SET_NAV_STATE = 100001 # Change mode by specifying nav_state directly. |nav_state|Empty|Empty|Empty|Empty|Empty|Empty| + +uint8 VEHICLE_MOUNT_MODE_RETRACT = 0 # Load and keep safe position (Roll,Pitch,Yaw) from permanent memory and stop stabilization | +uint8 VEHICLE_MOUNT_MODE_NEUTRAL = 1 # Load and keep neutral position (Roll,Pitch,Yaw) from permanent memory. | +uint8 VEHICLE_MOUNT_MODE_MAVLINK_TARGETING = 2 # Load neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization | +uint8 VEHICLE_MOUNT_MODE_RC_TARGETING = 3 # Load neutral position and start RC Roll,Pitch,Yaw control with stabilization | +uint8 VEHICLE_MOUNT_MODE_GPS_POINT = 4 # Load neutral position and start to point to Lat,Lon,Alt | +uint8 VEHICLE_MOUNT_MODE_ENUM_END = 5 # + +uint8 VEHICLE_ROI_NONE = 0 # No region of interest | +uint8 VEHICLE_ROI_WPNEXT = 1 # Point toward next MISSION | +uint8 VEHICLE_ROI_WPINDEX = 2 # Point toward given MISSION | +uint8 VEHICLE_ROI_LOCATION = 3 # Point toward fixed location | +uint8 VEHICLE_ROI_TARGET = 4 # Point toward target +uint8 VEHICLE_ROI_ENUM_END = 5 + +uint8 PARACHUTE_ACTION_DISABLE = 0 +uint8 PARACHUTE_ACTION_ENABLE = 1 +uint8 PARACHUTE_ACTION_RELEASE = 2 + +uint8 FAILURE_UNIT_SENSOR_GYRO = 0 +uint8 FAILURE_UNIT_SENSOR_ACCEL = 1 +uint8 FAILURE_UNIT_SENSOR_MAG = 2 +uint8 FAILURE_UNIT_SENSOR_BARO = 3 +uint8 FAILURE_UNIT_SENSOR_GPS = 4 +uint8 FAILURE_UNIT_SENSOR_OPTICAL_FLOW = 5 +uint8 FAILURE_UNIT_SENSOR_VIO = 6 +uint8 FAILURE_UNIT_SENSOR_DISTANCE_SENSOR = 7 +uint8 FAILURE_UNIT_SENSOR_AIRSPEED = 8 +uint8 FAILURE_UNIT_SYSTEM_BATTERY = 100 +uint8 FAILURE_UNIT_SYSTEM_MOTOR = 101 +uint8 FAILURE_UNIT_SYSTEM_SERVO = 102 +uint8 FAILURE_UNIT_SYSTEM_AVOIDANCE = 103 +uint8 FAILURE_UNIT_SYSTEM_RC_SIGNAL = 104 +uint8 FAILURE_UNIT_SYSTEM_MAVLINK_SIGNAL = 105 + +uint8 FAILURE_TYPE_OK = 0 +uint8 FAILURE_TYPE_OFF = 1 +uint8 FAILURE_TYPE_STUCK = 2 +uint8 FAILURE_TYPE_GARBAGE = 3 +uint8 FAILURE_TYPE_WRONG = 4 +uint8 FAILURE_TYPE_SLOW = 5 +uint8 FAILURE_TYPE_DELAYED = 6 +uint8 FAILURE_TYPE_INTERMITTENT = 7 + +# used as param1 in DO_CHANGE_SPEED command +uint8 SPEED_TYPE_AIRSPEED = 0 +uint8 SPEED_TYPE_GROUNDSPEED = 1 +uint8 SPEED_TYPE_CLIMB_SPEED = 2 +uint8 SPEED_TYPE_DESCEND_SPEED = 3 + +# used as param1 in ARM_DISARM command +int8 ARMING_ACTION_DISARM = 0 +int8 ARMING_ACTION_ARM = 1 + +# param2 in VEHICLE_CMD_DO_GRIPPER +uint8 GRIPPER_ACTION_RELEASE = 0 +uint8 GRIPPER_ACTION_GRAB = 1 + +uint8 ORB_QUEUE_LENGTH = 8 + +float32 param1 # Parameter 1, as defined by MAVLink uint16 VEHICLE_CMD enum. +float32 param2 # Parameter 2, as defined by MAVLink uint16 VEHICLE_CMD enum. +float32 param3 # Parameter 3, as defined by MAVLink uint16 VEHICLE_CMD enum. +float32 param4 # Parameter 4, as defined by MAVLink uint16 VEHICLE_CMD enum. +float64 param5 # Parameter 5, as defined by MAVLink uint16 VEHICLE_CMD enum. +float64 param6 # Parameter 6, as defined by MAVLink uint16 VEHICLE_CMD enum. +float32 param7 # Parameter 7, as defined by MAVLink uint16 VEHICLE_CMD enum. +uint32 command # Command ID +uint8 target_system # System which should execute the command +uint8 target_component # Component which should execute the command, 0 for all components +uint8 source_system # System sending the command +uint16 source_component # Component / mode executor sending the command +uint8 confirmation # 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) +bool from_external + +uint16 COMPONENT_MODE_EXECUTOR_START = 1000 + +# TOPICS vehicle_command gimbal_v1_command vehicle_command_mode_executor diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleCommandAck.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleCommandAck.msg new file mode 100644 index 00000000..6f54fa46 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleCommandAck.msg @@ -0,0 +1,33 @@ +# Vehicle Command Ackonwledgement uORB message. +# Used for acknowledging the vehicle command being received. +# Follows the MAVLink COMMAND_ACK message definition + +uint64 timestamp # time since system start (microseconds) + +# Result cases. This follows the MAVLink MAV_RESULT enum definition +uint8 VEHICLE_CMD_RESULT_ACCEPTED = 0 # Command ACCEPTED and EXECUTED | +uint8 VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED = 1 # Command TEMPORARY REJECTED/DENIED | +uint8 VEHICLE_CMD_RESULT_DENIED = 2 # Command PERMANENTLY DENIED | +uint8 VEHICLE_CMD_RESULT_UNSUPPORTED = 3 # Command UNKNOWN/UNSUPPORTED | +uint8 VEHICLE_CMD_RESULT_FAILED = 4 # Command executed, but failed | +uint8 VEHICLE_CMD_RESULT_IN_PROGRESS = 5 # Command being executed | +uint8 VEHICLE_CMD_RESULT_CANCELLED = 6 # Command Canceled + +# Arming denied specific cases +uint16 ARM_AUTH_DENIED_REASON_GENERIC = 0 +uint16 ARM_AUTH_DENIED_REASON_NONE = 1 +uint16 ARM_AUTH_DENIED_REASON_INVALID_WAYPOINT = 2 +uint16 ARM_AUTH_DENIED_REASON_TIMEOUT = 3 +uint16 ARM_AUTH_DENIED_REASON_AIRSPACE_IN_USE = 4 +uint16 ARM_AUTH_DENIED_REASON_BAD_WEATHER = 5 + +uint8 ORB_QUEUE_LENGTH = 4 + +uint32 command # Command that is being acknowledged +uint8 result # Command result +uint8 result_param1 # Also used as progress[%], it can be set with the reason why the command was denied, or the progress percentage when result is MAV_RESULT_IN_PROGRESS +int32 result_param2 # Additional parameter of the result, example: which parameter of MAV_CMD_NAV_WAYPOINT caused it to be denied. +uint8 target_system +uint16 target_component # Target component / mode executor + +bool from_external # Indicates if the command came from an external source diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleConstraints.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleConstraints.msg new file mode 100644 index 00000000..aa3a491b --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleConstraints.msg @@ -0,0 +1,9 @@ +# Local setpoint constraints in NED frame +# setting something to NaN means that no limit is provided + +uint64 timestamp # time since system start (microseconds) + +float32 speed_up # in meters/sec +float32 speed_down # in meters/sec + +bool want_takeoff # tell the controller to initiate takeoff when idling (ignored during flight) diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleControlMode.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleControlMode.msg new file mode 100644 index 00000000..9b33f9b8 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleControlMode.msg @@ -0,0 +1,22 @@ +uint64 timestamp # time since system start (microseconds) +bool flag_armed # synonym for actuator_armed.armed + +bool flag_multicopter_position_control_enabled + +bool flag_control_manual_enabled # true if manual input is mixed in +bool flag_control_auto_enabled # true if onboard autopilot should act +bool flag_control_offboard_enabled # true if offboard control should be used +bool flag_control_position_enabled # true if position is controlled +bool flag_control_velocity_enabled # true if horizontal velocity (implies direction) is controlled +bool flag_control_altitude_enabled # true if altitude is controlled +bool flag_control_climb_rate_enabled # true if climb rate is controlled +bool flag_control_acceleration_enabled # true if acceleration is controlled +bool flag_control_attitude_enabled # true if attitude stabilization is mixed in +bool flag_control_rates_enabled # true if rates are stabilized +bool flag_control_allocation_enabled # true if control allocation is enabled +bool flag_control_termination_enabled # true if flighttermination is enabled + +# TODO: use dedicated topic for external requests +uint8 source_id # Mode ID (nav_state) + +# TOPICS vehicle_control_mode config_control_setpoints diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleGlobalPosition.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleGlobalPosition.msg new file mode 100644 index 00000000..c7d9ee78 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleGlobalPosition.msg @@ -0,0 +1,30 @@ +# Fused global position in WGS84. +# This struct contains global position estimation. It is not the raw GPS +# measurement (@see vehicle_gps_position). This topic is usually published by the position +# estimator, which will take more sources of information into account than just GPS, +# e.g. control inputs of the vehicle in a Kalman-filter implementation. +# + +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +float64 lat # Latitude, (degrees) +float64 lon # Longitude, (degrees) +float32 alt # Altitude AMSL, (meters) +float32 alt_ellipsoid # Altitude above ellipsoid, (meters) + +float32 delta_alt # Reset delta for altitude +uint8 lat_lon_reset_counter # Counter for reset events on horizontal position coordinates +uint8 alt_reset_counter # Counter for reset events on altitude + +float32 eph # Standard deviation of horizontal position error, (metres) +float32 epv # Standard deviation of vertical position error, (metres) + +float32 terrain_alt # Terrain altitude WGS84, (metres) +bool terrain_alt_valid # Terrain altitude estimate is valid + +bool dead_reckoning # True if this position is estimated through dead-reckoning + +# TOPICS vehicle_global_position vehicle_global_position_groundtruth external_ins_global_position +# TOPICS estimator_global_position +# TOPICS aux_global_position diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleImu.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleImu.msg new file mode 100644 index 00000000..a71bb7a0 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleImu.msg @@ -0,0 +1,23 @@ +# IMU readings in SI-unit form. + +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample + +uint32 accel_device_id # Accelerometer unique device ID for the sensor that does not change between power cycles +uint32 gyro_device_id # Gyroscope unique device ID for the sensor that does not change between power cycles + +float32[3] delta_angle # delta angle about the FRD body frame XYZ-axis in rad over the integration time frame (delta_angle_dt) +float32[3] delta_velocity # delta velocity in the FRD body frame XYZ-axis in m/s over the integration time frame (delta_velocity_dt) + +uint16 delta_angle_dt # integration period in microseconds +uint16 delta_velocity_dt # integration period in microseconds + +uint8 CLIPPING_X = 1 +uint8 CLIPPING_Y = 2 +uint8 CLIPPING_Z = 4 + +uint8 delta_angle_clipping # bitfield indicating if there was any gyro clipping (per axis) during the integration time frame +uint8 delta_velocity_clipping # bitfield indicating if there was any accelerometer clipping (per axis) during the integration time frame + +uint8 accel_calibration_count # Calibration changed counter. Monotonically increases whenever accelermeter calibration changes. +uint8 gyro_calibration_count # Calibration changed counter. Monotonically increases whenever rate gyro calibration changes. diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleImuStatus.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleImuStatus.msg new file mode 100644 index 00000000..78fb4470 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleImuStatus.msg @@ -0,0 +1,28 @@ +uint64 timestamp # time since system start (microseconds) + +uint32 accel_device_id # unique device ID for the sensor that does not change between power cycles +uint32 gyro_device_id # unique device ID for the sensor that does not change between power cycles + +uint32[3] accel_clipping # total clipping per axis +uint32[3] gyro_clipping # total clipping per axis + +uint32 accel_error_count +uint32 gyro_error_count + +float32 accel_rate_hz +float32 gyro_rate_hz + +float32 accel_raw_rate_hz # full raw sensor sample rate (Hz) +float32 gyro_raw_rate_hz # full raw sensor sample rate (Hz) + +float32 accel_vibration_metric # high frequency vibration level in the accelerometer data (m/s/s) +float32 gyro_vibration_metric # high frequency vibration level in the gyro data (rad/s) +float32 delta_angle_coning_metric # average IMU delta angle coning correction (rad^2) + +float32[3] mean_accel # average accelerometer readings since last publication +float32[3] mean_gyro # average gyroscope readings since last publication +float32[3] var_accel # accelerometer variance since last publication +float32[3] var_gyro # gyroscope variance since last publication + +float32 temperature_accel +float32 temperature_gyro diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleLandDetected.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleLandDetected.msg new file mode 100644 index 00000000..fc0ca4a6 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleLandDetected.msg @@ -0,0 +1,19 @@ +uint64 timestamp # time since system start (microseconds) + +bool freefall # true if vehicle is currently in free-fall +bool ground_contact # true if vehicle has ground contact but is not landed (1. stage) +bool maybe_landed # true if the vehicle might have landed (2. stage) +bool landed # true if vehicle is currently landed on the ground (3. stage) + +bool in_ground_effect # indicates if from the perspective of the landing detector the vehicle might be in ground effect (baro). This flag will become true if the vehicle is not moving horizontally and is descending (crude assumption that user is landing). +bool in_descend + +bool has_low_throttle + +bool vertical_movement +bool horizontal_movement +bool rotational_movement + +bool close_to_ground_or_skipped_check + +bool at_rest diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleLocalPosition.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleLocalPosition.msg new file mode 100644 index 00000000..c1a0dffe --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleLocalPosition.msg @@ -0,0 +1,79 @@ +# Fused local position in NED. +# The coordinate system origin is the vehicle position at the time when the EKF2-module was started. + +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +bool xy_valid # true if x and y are valid +bool z_valid # true if z is valid +bool v_xy_valid # true if vx and vy are valid +bool v_z_valid # true if vz is valid + +# Position in local NED frame +float32 x # North position in NED earth-fixed frame, (metres) +float32 y # East position in NED earth-fixed frame, (metres) +float32 z # Down position (negative altitude) in NED earth-fixed frame, (metres) + +# Position reset delta +float32[2] delta_xy # Amount of lateral shift of position estimate in latest reset (in x and y) [m] +uint8 xy_reset_counter # Index of latest lateral position estimate reset +float32 delta_z # Amount of vertical shift of position estimate in latest reset [m] +uint8 z_reset_counter # Index of latest vertical position estimate reset + +# Velocity in NED frame +float32 vx # North velocity in NED earth-fixed frame, (metres/sec) +float32 vy # East velocity in NED earth-fixed frame, (metres/sec) +float32 vz # Down velocity in NED earth-fixed frame, (metres/sec) +float32 z_deriv # Down position time derivative in NED earth-fixed frame, (metres/sec) + +# Velocity reset delta +float32[2] delta_vxy # Amount of lateral shift of velocity estimate in latest reset (in x and y) [m/s] +uint8 vxy_reset_counter # Index of latest vertical velocity estimate reset +float32 delta_vz # Amount of vertical shift of velocity estimate in latest reset [m/s] +uint8 vz_reset_counter # Index of latest vertical velocity estimate reset + +# Acceleration in NED frame +float32 ax # North velocity derivative in NED earth-fixed frame, (metres/sec^2) +float32 ay # East velocity derivative in NED earth-fixed frame, (metres/sec^2) +float32 az # Down velocity derivative in NED earth-fixed frame, (metres/sec^2) + +float32 heading # Euler yaw angle transforming the tangent plane relative to NED earth-fixed frame, -PI..+PI, (radians) +float32 heading_var +float32 unaided_heading # Same as heading but generated by integrating corrected gyro data only +float32 delta_heading # Heading delta caused by latest heading reset [rad] +uint8 heading_reset_counter # Index of latest heading reset +bool heading_good_for_control + +float32 tilt_var + +# Position of reference point (local NED frame origin) in global (GPS / WGS84) frame +bool xy_global # true if position (x, y) has a valid global reference (ref_lat, ref_lon) +bool z_global # true if z has a valid global reference (ref_alt) +uint64 ref_timestamp # Time when reference position was set since system start, (microseconds) +float64 ref_lat # Reference point latitude, (degrees) +float64 ref_lon # Reference point longitude, (degrees) +float32 ref_alt # Reference altitude AMSL, (metres) + +# Distance to surface +float32 dist_bottom # Distance from from bottom surface to ground, (metres) +bool dist_bottom_valid # true if distance to bottom surface is valid +uint8 dist_bottom_sensor_bitfield # bitfield indicating what type of sensor is used to estimate dist_bottom +uint8 DIST_BOTTOM_SENSOR_NONE = 0 +uint8 DIST_BOTTOM_SENSOR_RANGE = 1 # (1 << 0) a range sensor is used to estimate dist_bottom field +uint8 DIST_BOTTOM_SENSOR_FLOW = 2 # (1 << 1) a flow sensor is used to estimate dist_bottom field (mostly fixed-wing use case) + +float32 eph # Standard deviation of horizontal position error, (metres) +float32 epv # Standard deviation of vertical position error, (metres) +float32 evh # Standard deviation of horizontal velocity error, (metres/sec) +float32 evv # Standard deviation of vertical velocity error, (metres/sec) + +bool dead_reckoning # True if this position is estimated through dead-reckoning + +# estimator specified vehicle limits +float32 vxy_max # maximum horizontal speed - set to 0 when limiting not required (meters/sec) +float32 vz_max # maximum vertical speed - set to 0 when limiting not required (meters/sec) +float32 hagl_min # minimum height above ground level - set to 0 when limiting not required (meters) +float32 hagl_max # maximum height above ground level - set to 0 when limiting not required (meters) + +# TOPICS vehicle_local_position vehicle_local_position_groundtruth external_ins_local_position +# TOPICS estimator_local_position diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleLocalPositionSetpoint.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleLocalPositionSetpoint.msg new file mode 100644 index 00000000..0093d52d --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleLocalPositionSetpoint.msg @@ -0,0 +1,19 @@ +# Local position setpoint in NED frame +# Telemetry of PID position controller to monitor tracking. +# NaN means the state was not controlled + +uint64 timestamp # time since system start (microseconds) + +float32 x # in meters NED +float32 y # in meters NED +float32 z # in meters NED + +float32 vx # in meters/sec +float32 vy # in meters/sec +float32 vz # in meters/sec + +float32[3] acceleration # in meters/sec^2 +float32[3] thrust # normalized thrust vector in NED + +float32 yaw # in radians NED -PI..+PI +float32 yawspeed # in radians/sec diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleMagnetometer.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleMagnetometer.msg new file mode 100644 index 00000000..f2249c78 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleMagnetometer.msg @@ -0,0 +1,10 @@ + +uint64 timestamp # time since system start (microseconds) + +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +uint32 device_id # unique device ID for the selected magnetometer + +float32[3] magnetometer_ga # Magnetic field in the FRD body frame XYZ-axis in Gauss + +uint8 calibration_count # Calibration changed counter. Monotonically increases whenever calibration changes. diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleOdometry.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleOdometry.msg new file mode 100644 index 00000000..fbdd1920 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleOdometry.msg @@ -0,0 +1,31 @@ +# Vehicle odometry data. Fits ROS REP 147 for aerial vehicles +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample + +uint8 POSE_FRAME_UNKNOWN = 0 +uint8 POSE_FRAME_NED = 1 # NED earth-fixed frame +uint8 POSE_FRAME_FRD = 2 # FRD world-fixed frame, arbitrary heading reference +uint8 pose_frame # Position and orientation frame of reference + +float32[3] position # Position in meters. Frame of reference defined by local_frame. NaN if invalid/unknown +float32[4] q # Quaternion rotation from FRD body frame to reference frame. First value NaN if invalid/unknown + +uint8 VELOCITY_FRAME_UNKNOWN = 0 +uint8 VELOCITY_FRAME_NED = 1 # NED earth-fixed frame +uint8 VELOCITY_FRAME_FRD = 2 # FRD world-fixed frame, arbitrary heading reference +uint8 VELOCITY_FRAME_BODY_FRD = 3 # FRD body-fixed frame +uint8 velocity_frame # Reference frame of the velocity data + +float32[3] velocity # Velocity in meters/sec. Frame of reference defined by velocity_frame variable. NaN if invalid/unknown + +float32[3] angular_velocity # Angular velocity in body-fixed frame (rad/s). NaN if invalid/unknown + +float32[3] position_variance +float32[3] orientation_variance +float32[3] velocity_variance + +uint8 reset_counter +int8 quality + +# TOPICS vehicle_odometry vehicle_mocap_odometry vehicle_visual_odometry +# TOPICS estimator_odometry diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleOpticalFlow.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleOpticalFlow.msg new file mode 100644 index 00000000..13bdb57b --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleOpticalFlow.msg @@ -0,0 +1,21 @@ +# Optical flow in XYZ body frame in SI units. + +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +float32[2] pixel_flow # (radians) accumulated optical flow in radians where a positive value is produced by a RH rotation about the body axis + +float32[3] delta_angle # (radians) accumulated gyro radians where a positive value is produced by a RH rotation about the body axis. (NAN if unavailable) + +float32 distance_m # (meters) Distance to the center of the flow field (NAN if unavailable) + +uint32 integration_timespan_us # (microseconds) accumulation timespan in microseconds + +uint8 quality # Average of quality of accumulated frames, 0: bad quality, 255: maximum quality + +float32 max_flow_rate # (radians/s) Magnitude of maximum angular which the optical flow sensor can measure reliably + +float32 min_ground_distance # (meters) Minimum distance from ground at which the optical flow sensor operates reliably +float32 max_ground_distance # (meters) Maximum distance from ground at which the optical flow sensor operates reliably diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleOpticalFlowVel.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleOpticalFlowVel.msg new file mode 100644 index 00000000..947131da --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleOpticalFlowVel.msg @@ -0,0 +1,15 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +float32[2] vel_body # velocity obtained from gyro-compensated and distance-scaled optical flow raw measurements in body frame(m/s) +float32[2] vel_ne # same as vel_body but in local frame (m/s) + +float32[2] flow_rate_uncompensated # integrated optical flow measurement (rad/s) +float32[2] flow_rate_compensated # integrated optical flow measurement compensated for angular motion (rad/s) + +float32[3] gyro_rate # gyro measurement synchronized with flow measurements (rad/s) + +float32[3] gyro_bias +float32[3] ref_gyro + +# TOPICS estimator_optical_flow_vel vehicle_optical_flow_vel diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleRatesSetpoint.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleRatesSetpoint.msg new file mode 100644 index 00000000..35a06c35 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleRatesSetpoint.msg @@ -0,0 +1,12 @@ +uint64 timestamp # time since system start (microseconds) + +# body angular rates in FRD frame +float32 roll # [rad/s] roll rate setpoint +float32 pitch # [rad/s] pitch rate setpoint +float32 yaw # [rad/s] yaw rate setpoint + +# For clarification: For multicopters thrust_body[0] and thrust[1] are usually 0 and thrust[2] is the negative throttle demand. +# For fixed wings thrust_x is the throttle demand and thrust_y, thrust_z will usually be zero. +float32[3] thrust_body # Normalized thrust command in body NED frame [-1,1] + +bool reset_integral # Reset roll/pitch/yaw integrals (navigation logic change) diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleRoi.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleRoi.msg new file mode 100644 index 00000000..2948f157 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleRoi.msg @@ -0,0 +1,21 @@ +# Vehicle Region Of Interest (ROI) + +uint64 timestamp # time since system start (microseconds) + +uint8 ROI_NONE = 0 # No region of interest +uint8 ROI_WPNEXT = 1 # Point toward next MISSION with optional offset +uint8 ROI_WPINDEX = 2 # Point toward given MISSION +uint8 ROI_LOCATION = 3 # Point toward fixed location +uint8 ROI_TARGET = 4 # Point toward target +uint8 ROI_ENUM_END = 5 + +uint8 mode # ROI mode (see above) + +float64 lat # Latitude to point to +float64 lon # Longitude to point to +float32 alt # Altitude to point to + +# additional angle offsets to next waypoint (only used with ROI_WPNEXT) +float32 roll_offset # angle offset in rad +float32 pitch_offset # angle offset in rad +float32 yaw_offset # angle offset in rad diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleStatus.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleStatus.msg new file mode 100644 index 00000000..4c711b97 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleStatus.msg @@ -0,0 +1,138 @@ +# Encodes the system state of the vehicle published by commander + +uint64 timestamp # time since system start (microseconds) + +uint64 armed_time # Arming timestamp (microseconds) +uint64 takeoff_time # Takeoff timestamp (microseconds) + +uint8 arming_state +uint8 ARMING_STATE_DISARMED = 1 +uint8 ARMING_STATE_ARMED = 2 + +uint8 latest_arming_reason +uint8 latest_disarming_reason +uint8 ARM_DISARM_REASON_TRANSITION_TO_STANDBY = 0 +uint8 ARM_DISARM_REASON_RC_STICK = 1 +uint8 ARM_DISARM_REASON_RC_SWITCH = 2 +uint8 ARM_DISARM_REASON_COMMAND_INTERNAL = 3 +uint8 ARM_DISARM_REASON_COMMAND_EXTERNAL = 4 +uint8 ARM_DISARM_REASON_MISSION_START = 5 +uint8 ARM_DISARM_REASON_SAFETY_BUTTON = 6 +uint8 ARM_DISARM_REASON_AUTO_DISARM_LAND = 7 +uint8 ARM_DISARM_REASON_AUTO_DISARM_PREFLIGHT = 8 +uint8 ARM_DISARM_REASON_KILL_SWITCH = 9 +uint8 ARM_DISARM_REASON_LOCKDOWN = 10 +uint8 ARM_DISARM_REASON_FAILURE_DETECTOR = 11 +uint8 ARM_DISARM_REASON_SHUTDOWN = 12 +uint8 ARM_DISARM_REASON_UNIT_TEST = 13 + +uint64 nav_state_timestamp # time when current nav_state activated + +uint8 nav_state_user_intention # Mode that the user selected (might be different from nav_state in a failsafe situation) + +uint8 nav_state # Currently active mode +uint8 NAVIGATION_STATE_MANUAL = 0 # Manual mode +uint8 NAVIGATION_STATE_ALTCTL = 1 # Altitude control mode +uint8 NAVIGATION_STATE_POSCTL = 2 # Position control mode +uint8 NAVIGATION_STATE_AUTO_MISSION = 3 # Auto mission mode +uint8 NAVIGATION_STATE_AUTO_LOITER = 4 # Auto loiter mode +uint8 NAVIGATION_STATE_AUTO_RTL = 5 # Auto return to launch mode +uint8 NAVIGATION_STATE_POSITION_SLOW = 6 +uint8 NAVIGATION_STATE_FREE5 = 7 +uint8 NAVIGATION_STATE_FREE4 = 8 +uint8 NAVIGATION_STATE_FREE3 = 9 +uint8 NAVIGATION_STATE_ACRO = 10 # Acro mode +uint8 NAVIGATION_STATE_FREE2 = 11 +uint8 NAVIGATION_STATE_DESCEND = 12 # Descend mode (no position control) +uint8 NAVIGATION_STATE_TERMINATION = 13 # Termination mode +uint8 NAVIGATION_STATE_OFFBOARD = 14 +uint8 NAVIGATION_STATE_STAB = 15 # Stabilized mode +uint8 NAVIGATION_STATE_FREE1 = 16 +uint8 NAVIGATION_STATE_AUTO_TAKEOFF = 17 # Takeoff +uint8 NAVIGATION_STATE_AUTO_LAND = 18 # Land +uint8 NAVIGATION_STATE_AUTO_FOLLOW_TARGET = 19 # Auto Follow +uint8 NAVIGATION_STATE_AUTO_PRECLAND = 20 # Precision land with landing target +uint8 NAVIGATION_STATE_ORBIT = 21 # Orbit in a circle +uint8 NAVIGATION_STATE_AUTO_VTOL_TAKEOFF = 22 # Takeoff, transition, establish loiter +uint8 NAVIGATION_STATE_EXTERNAL1 = 23 +uint8 NAVIGATION_STATE_EXTERNAL2 = 24 +uint8 NAVIGATION_STATE_EXTERNAL3 = 25 +uint8 NAVIGATION_STATE_EXTERNAL4 = 26 +uint8 NAVIGATION_STATE_EXTERNAL5 = 27 +uint8 NAVIGATION_STATE_EXTERNAL6 = 28 +uint8 NAVIGATION_STATE_EXTERNAL7 = 29 +uint8 NAVIGATION_STATE_EXTERNAL8 = 30 +uint8 NAVIGATION_STATE_MAX = 31 + +uint8 executor_in_charge # Current mode executor in charge (0=Autopilot) + +uint32 valid_nav_states_mask # Bitmask for all valid nav_state values +uint32 can_set_nav_states_mask # Bitmask for all modes that a user can select + +# Bitmask of detected failures +uint16 failure_detector_status +uint16 FAILURE_NONE = 0 +uint16 FAILURE_ROLL = 1 # (1 << 0) +uint16 FAILURE_PITCH = 2 # (1 << 1) +uint16 FAILURE_ALT = 4 # (1 << 2) +uint16 FAILURE_EXT = 8 # (1 << 3) +uint16 FAILURE_ARM_ESC = 16 # (1 << 4) +uint16 FAILURE_BATTERY = 32 # (1 << 5) +uint16 FAILURE_IMBALANCED_PROP = 64 # (1 << 6) +uint16 FAILURE_MOTOR = 128 # (1 << 7) + +uint8 hil_state +uint8 HIL_STATE_OFF = 0 +uint8 HIL_STATE_ON = 1 + +# If it's a VTOL, then the value will be VEHICLE_TYPE_ROTARY_WING while flying as a multicopter, and VEHICLE_TYPE_FIXED_WING when flying as a fixed-wing +uint8 vehicle_type +uint8 VEHICLE_TYPE_UNKNOWN = 0 +uint8 VEHICLE_TYPE_ROTARY_WING = 1 +uint8 VEHICLE_TYPE_FIXED_WING = 2 +uint8 VEHICLE_TYPE_ROVER = 3 +uint8 VEHICLE_TYPE_AIRSHIP = 4 + +uint8 FAILSAFE_DEFER_STATE_DISABLED = 0 +uint8 FAILSAFE_DEFER_STATE_ENABLED = 1 +uint8 FAILSAFE_DEFER_STATE_WOULD_FAILSAFE = 2 # Failsafes deferred, but would trigger a failsafe + +bool failsafe # true if system is in failsafe state (e.g.:RTL, Hover, Terminate, ...) +bool failsafe_and_user_took_over # true if system is in failsafe state but the user took over control +uint8 failsafe_defer_state # one of FAILSAFE_DEFER_STATE_* + +# Link loss +bool gcs_connection_lost # datalink to GCS lost +uint8 gcs_connection_lost_counter # counts unique GCS connection lost events +bool high_latency_data_link_lost # Set to true if the high latency data link (eg. RockBlock Iridium 9603 telemetry module) is lost + +# VTOL flags +bool is_vtol # True if the system is VTOL capable +bool is_vtol_tailsitter # True if the system performs a 90° pitch down rotation during transition from MC to FW +bool in_transition_mode # True if VTOL is doing a transition +bool in_transition_to_fw # True if VTOL is doing a transition from MC to FW + +# MAVLink identification +uint8 system_type # system type, contains mavlink MAV_TYPE +uint8 system_id # system id, contains MAVLink's system ID field +uint8 component_id # subsystem / component id, contains MAVLink's component ID field + +bool safety_button_available # Set to true if a safety button is connected +bool safety_off # Set to true if safety is off + +bool power_input_valid # set if input power is valid +bool usb_connected # set to true (never cleared) once telemetry received from usb link + +bool open_drone_id_system_present +bool open_drone_id_system_healthy + +bool parachute_system_present +bool parachute_system_healthy + +bool avoidance_system_required # Set to true if avoidance system is enabled via COM_OBS_AVOID parameter +bool avoidance_system_valid # Status of the obstacle avoidance system + +bool rc_calibration_in_progress +bool calibration_enabled + +bool pre_flight_checks_pass # true if all checks necessary to arm pass diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleThrustSetpoint.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleThrustSetpoint.msg new file mode 100644 index 00000000..444ee500 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleThrustSetpoint.msg @@ -0,0 +1,8 @@ + +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds) + +float32[3] xyz # thrust setpoint along X, Y, Z body axis [-1, 1] + +# TOPICS vehicle_thrust_setpoint +# TOPICS vehicle_thrust_setpoint_virtual_fw vehicle_thrust_setpoint_virtual_mc diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleTorqueSetpoint.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleTorqueSetpoint.msg new file mode 100644 index 00000000..c20519b1 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleTorqueSetpoint.msg @@ -0,0 +1,8 @@ + +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds) + +float32[3] xyz # torque setpoint about X, Y, Z body axis (normalized) + +# TOPICS vehicle_torque_setpoint +# TOPICS vehicle_torque_setpoint_virtual_fw vehicle_torque_setpoint_virtual_mc diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleTrajectoryBezier.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleTrajectoryBezier.msg new file mode 100644 index 00000000..d4bf99b4 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleTrajectoryBezier.msg @@ -0,0 +1,18 @@ +# Vehicle Waypoints Trajectory description. See also MAVLink MAV_TRAJECTORY_REPRESENTATION msg +# The topic vehicle_trajectory_bezier is used to send a smooth flight path from the +# companion computer / avoidance module to the position controller. + +uint64 timestamp # time since system start (microseconds) + +uint8 POINT_0 = 0 +uint8 POINT_1 = 1 +uint8 POINT_2 = 2 +uint8 POINT_3 = 3 +uint8 POINT_4 = 4 + +uint8 NUMBER_POINTS = 5 + +TrajectoryBezier[5] control_points +uint8 bezier_order + +# TOPICS vehicle_trajectory_bezier diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleTrajectoryWaypoint.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleTrajectoryWaypoint.msg new file mode 100644 index 00000000..6bff1cec --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VehicleTrajectoryWaypoint.msg @@ -0,0 +1,21 @@ +# Vehicle Waypoints Trajectory description. See also MAVLink MAV_TRAJECTORY_REPRESENTATION msg +# The topic vehicle_trajectory_waypoint_desired is used to send the user desired waypoints from the position controller to the companion computer / avoidance module. +# The topic vehicle_trajectory_waypoint is used to send the adjusted waypoints from the companion computer / avoidance module to the position controller. + +uint64 timestamp # time since system start (microseconds) + +uint8 MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS = 0 + +uint8 type # Type from MAV_TRAJECTORY_REPRESENTATION enum. + +uint8 POINT_0 = 0 +uint8 POINT_1 = 1 +uint8 POINT_2 = 2 +uint8 POINT_3 = 3 +uint8 POINT_4 = 4 + +uint8 NUMBER_POINTS = 5 + +TrajectoryWaypoint[5] waypoints + +# TOPICS vehicle_trajectory_waypoint vehicle_trajectory_waypoint_desired diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VelocityLimits.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VelocityLimits.msg new file mode 100644 index 00000000..9ab5115a --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VelocityLimits.msg @@ -0,0 +1,8 @@ +# Velocity and yaw rate limits for a multicopter position slow mode only + +uint64 timestamp # time since system start (microseconds) + +# absolute speeds, NAN means use default limit +float32 horizontal_velocity # [m/s] +float32 vertical_velocity # [m/s] +float32 yaw_rate # [rad/s] diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VtolVehicleStatus.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VtolVehicleStatus.msg new file mode 100644 index 00000000..61a82467 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/VtolVehicleStatus.msg @@ -0,0 +1,12 @@ +# VEHICLE_VTOL_STATE, should match 1:1 MAVLinks's MAV_VTOL_STATE +uint8 VEHICLE_VTOL_STATE_UNDEFINED = 0 +uint8 VEHICLE_VTOL_STATE_TRANSITION_TO_FW = 1 +uint8 VEHICLE_VTOL_STATE_TRANSITION_TO_MC = 2 +uint8 VEHICLE_VTOL_STATE_MC = 3 +uint8 VEHICLE_VTOL_STATE_FW = 4 + +uint64 timestamp # time since system start (microseconds) + +uint8 vehicle_vtol_state # current state of the vtol, see VEHICLE_VTOL_STATE + +bool fixed_wing_system_failure # vehicle in fixed-wing system failure failsafe mode (after quad-chute) diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/WheelEncoders.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/WheelEncoders.msg new file mode 100644 index 00000000..a4f3955d --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/WheelEncoders.msg @@ -0,0 +1,5 @@ +uint64 timestamp # time since system start (microseconds) + +# Two wheels: 0 right, 1 left +float32[2] wheel_speed # [rad/s] +float32[2] wheel_angle # [rad] diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/Wind.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/Wind.msg new file mode 100644 index 00000000..ff8b6f45 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/Wind.msg @@ -0,0 +1,16 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +float32 windspeed_north # Wind component in north / X direction (m/sec) +float32 windspeed_east # Wind component in east / Y direction (m/sec) + +float32 variance_north # Wind estimate error variance in north / X direction (m/sec)**2 - set to zero (no uncertainty) if not estimated +float32 variance_east # Wind estimate error variance in east / Y direction (m/sec)**2 - set to zero (no uncertainty) if not estimated + +float32 tas_innov # True airspeed innovation +float32 tas_innov_var # True airspeed innovation variance + +float32 beta_innov # Sideslip measurement innovation +float32 beta_innov_var # Sideslip measurement innovation variance + +# TOPICS wind estimator_wind diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/YawEstimatorStatus.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/YawEstimatorStatus.msg new file mode 100644 index 00000000..36091e26 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/msg/YawEstimatorStatus.msg @@ -0,0 +1,11 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +float32 yaw_composite # composite yaw from GSF (rad) +float32 yaw_variance # composite yaw variance from GSF (rad^2) +bool yaw_composite_valid + +float32[5] yaw # yaw estimate for each model in the filter bank (rad) +float32[5] innov_vn # North velocity innovation for each model in the filter bank (m/s) +float32[5] innov_ve # East velocity innovation for each model in the filter bank (m/s) +float32[5] weight # weighting for each model in the filter bank diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/package.xml b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/package.xml new file mode 100644 index 00000000..65291fb6 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/package.xml @@ -0,0 +1,26 @@ + + + + px4_msgs + 2.0.1 + Package with the ROS-equivalent of PX4 uORB msgs + Nuno Marques + Nuno Marques + BSD 3-Clause + + ament_cmake + rosidl_default_generators + + builtin_interfaces + ros_environment + + rosidl_default_runtime + + ament_lint_common + + rosidl_interface_packages + + + ament_cmake + + diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/srv/VehicleCommand.srv b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/srv/VehicleCommand.srv new file mode 100644 index 00000000..134e2a81 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/srv/VehicleCommand.srv @@ -0,0 +1,3 @@ +VehicleCommand request +--- +VehicleCommandAck reply diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/trajectory_controller/CMakeLists.txt b/robot/ros_ws/src/autonomy/3_local/c_controls/trajectory_controller/CMakeLists.txt new file mode 100644 index 00000000..880d7924 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/trajectory_controller/CMakeLists.txt @@ -0,0 +1,65 @@ +cmake_minimum_required(VERSION 3.8) +project(trajectory_controller) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(pluginlib REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(airstack_msgs REQUIRED) +find_package(airstack_common REQUIRED) +find_package(trajectory_library REQUIRED) + + +add_executable(trajectory_controller src/trajectory_controller.cpp) +target_include_directories(trajectory_controller PUBLIC + $ + $) +ament_target_dependencies(trajectory_controller + rclcpp + geometry_msgs + visualization_msgs + nav_msgs + std_srvs + tf2 + tf2_ros + airstack_msgs + airstack_common + trajectory_library) +install(TARGETS trajectory_controller + DESTINATION lib/${PROJECT_NAME}) + +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}/ + ) + +#install( +# DIRECTORY include/ +# DESTINATION include +#) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/trajectory_controller/index.html b/robot/ros_ws/src/autonomy/3_local/c_controls/trajectory_controller/index.html new file mode 100644 index 00000000..5223673b --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/trajectory_controller/index.html @@ -0,0 +1,2924 @@ + + + + + + + + + + + + + + + + + + + + + + + Trajectory Controller - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

Trajectory Controller

+

Overview

+

The trajectory controller takes in a trajectory and publishes a tracking point, which a controller should make the drone fly to, and a look ahead point, which a planner should plan from. The trajectory controller can interpret a trajectory as a standalone complete track, like a figure eight or racetrack pattern for tuning controls, or as separate segments that should be stitched together, for example trajectories output by a local planner.

+

The trajectory controller tries to keep the tracking point ahead of the robot in a pure pursuit fashion. The robot's position, the red X in the figure below, is projected onto the trajectory. A sphere, shown in 2d as the cyan circle, is placed around this projected point and the intersection between the sphere and the forward point on the trajectory is used as the tracking point, the blue X. The lookahead point, the orange X, is a fixed time duration along the trajectory.

+

Trajectory Controller

+

Parameters

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Parameter
Description
tf_prefixThe tf names published are prefixed with the string in this parameter. Tfs are published at the tracking point and lookahead point. There are stabilized versions, the same but with zero pitch and roll, of these two tfs that are also published.
target_frameThe tracking point and lookahead point are published in this frame.
look_ahead_timeHow far ahead of the tracking point the lookahead point will be in seconds.
min_virtual_tracking_velocityIf the velocity on the trajectory is less than this, the tracking point will just move forward in time instead of using the sphere to keep the tracking point ahead of the drone. The units are m/s.
sphere_radiusThis is the radius of the sphere used to determine the position of the tracking point. Making it larger pushes the tracking point farther ahead.
search_ahead_factorTo search for the point on the trajectory that intersects with the sphere, the algorithm checks a certain distance ahead along the trajectory. This distance is given by sphere_radius * search_ahead_factor. If the trajectory zigzags a lot relative to the size of the sphere, it's possible that the algorithm wouldn't iterate far enough along the trajectory to find the point where it intersects with the sphere. If a large value for this parameter is used and the trajectory loops back on itself, it is possible that this would cause the tracking point to jump ahead and skip a portion of the trajectory. In almost all cases, this parameter shouldn't need to be changed.
traj_vis_thicknessThe thickness of the trajectory visualization markers.
## Services
+

Trajectory Modes

+

There are several modes that the trajectory controller can be placed in with a service call to the set_trajectory_mode service: +See TrajectoryMode.srv.

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
ModeDescription
TRACKThis interprets a trajectory subscribed on ~/trajectory_override as a complete trajectory that the controller will follow. It is usually used for taking off, landing, and tuning controls on fixed trajectories like figure eights, racetracks, circles, etc...
ADD_SEGMENTThis interprets a trajectory subscribed on ~/trajectory_segment_to_add as a segment of a trajectory which will get stitched onto the current trajectory at the closest point to the start of the new segment. This is usually published by a local planner. Ideally it is published at the location of the lookahead point, which is a fixed time ahead of the tracking point. This fixed time should be greater than the time it takes to plan. For example, if the lookahead point is one second ahead of the tracking point, the local planner should be always take less than one second to plan otherwise the tracking point would already be past the start of the plan. If this happens, the trajectory will fail to be stitched and will be ignored.
PAUSEThis causes the tracking point to stop where it is.
REWINDThis makes the tracking point go backwards along the trajectory. This mode is usually used to make the drone blindly backtrack along its trajectory to get it out of a situation it is stuck in.
ROBOT_POSEThis makes the tracking point and lookahead point always be at the same position as the drone's odometry. This is useful for before takeoff, when the robot may be carried around so that the location where the takeoff starts is at the drone's position.
+

Subscriptions

+ + + + + + + + + + + + + + + + + + + + + + + + + +
Topic
TypeDescription
~/odometrynav_msgs/OdometryOdometry of the robot.
~/trajectory_segment_to_addairstack_msgs/TrajectoryXYZVYawFor ADD_SEGMENT mode, this is the trajectory segment to add to the current trajectory.
~/trajectory_overrideairstack_msgs/TrajectoryXYZVYawFor TRACK mode, this overrides the current trajectory and makes the robot follow this directly.
+

Publications

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Topic
TypeDescription
~/tracking_pointTODOTODO
~/look_aheadTODOTODO
~/traj_drone_pointTODOTODO
~/virtual_tracking_pointTODOTODO
~/closest_pointTODOTODO
~/trajectory_completion_percentageTODOTODO
~/trajectory_timeTODOTODO
~/tracking_errorTODOTODO
~/velocity_pubTODOTODO
~/debug_markersTODOTODO
~/trajectory_visTODOTODO
+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ + + +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/trajectory_controller/launch/trajectory_controller.launch.xml b/robot/ros_ws/src/autonomy/3_local/c_controls/trajectory_controller/launch/trajectory_controller.launch.xml new file mode 100644 index 00000000..099ee5b8 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/trajectory_controller/launch/trajectory_controller.launch.xml @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/trajectory_controller/launch/trajectory_controller_bag.launch b/robot/ros_ws/src/autonomy/3_local/c_controls/trajectory_controller/launch/trajectory_controller_bag.launch new file mode 100644 index 00000000..d7493916 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/trajectory_controller/launch/trajectory_controller_bag.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/trajectory_controller/msg/Trajectory.msg b/robot/ros_ws/src/autonomy/3_local/c_controls/trajectory_controller/msg/Trajectory.msg new file mode 100644 index 00000000..f3e50bae --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/trajectory_controller/msg/Trajectory.msg @@ -0,0 +1 @@ +nav_msgs/Odometry[] trajectory \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/trajectory_controller/package.xml b/robot/ros_ws/src/autonomy/3_local/c_controls/trajectory_controller/package.xml new file mode 100644 index 00000000..4c887640 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/trajectory_controller/package.xml @@ -0,0 +1,31 @@ + + + + trajectory_controller + 0.0.0 + TODO: Package description + uav + TODO: License declaration + + ament_cmake + + tf2 + tf2_ros + tf2_tools + rclcpp + pluginlib + geometry_msgs + visualization_msgs + nav_msgs + std_srvs + airstack_msgs + airstack_common + trajectory_library + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/trajectory_controller/src/tracking_point.py b/robot/ros_ws/src/autonomy/3_local/c_controls/trajectory_controller/src/tracking_point.py new file mode 100644 index 00000000..e8a57f8e --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/trajectory_controller/src/tracking_point.py @@ -0,0 +1,29 @@ +#!/usr/bin/python +import rospy +from nav_msgs.msg import Odometry +import numpy as np +from tf import transformations as trans + +if __name__ == '__main__': + rospy.init_node('tracking_point', anonymous=True) + + odom_pub = rospy.Publisher('/tracking_point', Odometry, queue_size=10) + odom = Odometry() + odom.header.frame_id = 'map' + odom.child_frame_id = 'base_link' + odom.pose.pose.position.x = 16. + odom.pose.pose.position.y = 5. + odom.pose.pose.position.z = 3. + + q = trans.quaternion_from_euler(0., 0., np.pi/2.) + + odom.pose.pose.orientation.x = q[0] + odom.pose.pose.orientation.y = q[1] + odom.pose.pose.orientation.z = q[2] + odom.pose.pose.orientation.w = q[3] + + rate = rospy.Rate(50.) + + while not rospy.is_shutdown(): + odom_pub.publish(odom) + rate.sleep() diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/trajectory_controller/src/trajectory_controller.cpp b/robot/ros_ws/src/autonomy/3_local/c_controls/trajectory_controller/src/trajectory_controller.cpp new file mode 100644 index 00000000..5f951556 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/trajectory_controller/src/trajectory_controller.cpp @@ -0,0 +1,541 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +//=================================================================================== +//----------------------------- Trajectory Control Node ----------------------------- +//=================================================================================== + +class TrajectoryControlNode : public rclcpp::Node { + private: + rclcpp::Subscription::SharedPtr traj_seg_to_add_sub; + rclcpp::Subscription::SharedPtr traj_override_sub; + rclcpp::Subscription::SharedPtr odom_sub; + + rclcpp::Publisher::SharedPtr trajectory_vis_pub; + rclcpp::Publisher::SharedPtr tracking_point_pub; + rclcpp::Publisher::SharedPtr look_ahead_pub; + rclcpp::Publisher::SharedPtr drone_point_pub; + rclcpp::Publisher::SharedPtr virtual_tracking_point_pub; + rclcpp::Publisher::SharedPtr closest_point_pub; + rclcpp::Publisher::SharedPtr trajectory_completion_percentage_pub; + rclcpp::Publisher::SharedPtr trajectory_time_pub; + rclcpp::Publisher::SharedPtr tracking_error_pub; + rclcpp::Publisher::SharedPtr velocity_pub; + rclcpp::Publisher::SharedPtr debug_markers_pub; + + tf2_ros::TransformBroadcaster* tf_broadcaster; + tf2_ros::TransformListener* tf_listener; + tf2_ros::Buffer* tf_buffer; + + rclcpp::Service::SharedPtr traj_mode_srv; + + rclcpp::TimerBase::SharedPtr timer; + + nav_msgs::msg::Odometry odom; + bool got_odom; + + double target_dt; + std::string tf_prefix; + std::string target_frame; + int trajectory_mode; + Trajectory* trajectory; + double prev_time; + double virtual_time; + double actual_time; + airstack_msgs::msg::Odometry look_ahead_point, drone_point; + airstack_msgs::msg::Odometry virtual_tracking_point_odom; + airstack_msgs::msg::Odometry closest_point_odom; + double tracking_point_distance_limit; + double velocity_look_ahead_time; + double look_ahead_time; + double virtual_tracking_ahead_time; + double min_virtual_tracking_velocity; + double sphere_radius; + double ff_min_velocity; + double search_ahead_factor; + double prev_vtp_time; + float traj_vis_thickness; + float current_velocity; + float time_multiplier; + float transition_velocity_scale; + float transition_dt; + bool new_rewind; + float rewind_skip_max_velocity; + float rewind_skip_max_distance; + + vis::MarkerArray markers; + + public: + TrajectoryControlNode(); + void timer_callback(); + + void traj_seg_to_add_callback(const airstack_msgs::msg::TrajectoryXYZVYaw::SharedPtr traj); + void traj_override_callback(const airstack_msgs::msg::TrajectoryXYZVYaw::SharedPtr traj); + void odom_callback(const nav_msgs::msg::Odometry::SharedPtr odom); + + void set_trajectory_mode(const std::shared_ptr req, + std::shared_ptr res); + + std::string mode; + float velocity_target; +}; + +TrajectoryControlNode::TrajectoryControlNode() : rclcpp::Node("trajectory_control_node") { + // init params + target_dt = 1. / airstack::get_param(this, "execute_target", 50.); + tf_prefix = airstack::get_param(this, "tf_prefix", std::string("")); + target_frame = airstack::get_param(this, "target_frame", std::string("world")); + tracking_point_distance_limit = airstack::get_param(this, "tracking_point_distance_limit", 0.5); + velocity_look_ahead_time = airstack::get_param(this, "velocity_look_ahead_time", 0.0); + look_ahead_time = airstack::get_param(this, "look_ahead_time", 1.0); + virtual_tracking_ahead_time = airstack::get_param(this, "virtual_tracking_ahead_time", 0.5); + min_virtual_tracking_velocity = airstack::get_param(this, "min_virtual_tracking_velocity", 0.3); + sphere_radius = airstack::get_param(this, "sphere_radius", 0.3); + ff_min_velocity = airstack::get_param(this, "ff_min_velocity", 0.4); + search_ahead_factor = airstack::get_param(this, "search_ahead_factor", 1.5); + transition_velocity_scale = airstack::get_param(this, "transition_velocity_scale", 1.0); + traj_vis_thickness = airstack::get_param(this, "traj_vis_thickness", 0.03); + rewind_skip_max_velocity = airstack::get_param(this, "rewind_skip_max_velocity", 0.1); + rewind_skip_max_distance = airstack::get_param(this, "rewind_skip_max_distance", 0.1); + got_odom = false; + + trajectory_mode = airstack_msgs::srv::TrajectoryMode::Request::ROBOT_POSE; + trajectory = new Trajectory(this, target_frame); + + // init subscribers + traj_seg_to_add_sub = this->create_subscription( + "trajectory_segment_to_add", 1, + std::bind(&TrajectoryControlNode::traj_seg_to_add_callback, this, std::placeholders::_1)); + traj_override_sub = this->create_subscription( + "trajectory_override", 1, + std::bind(&TrajectoryControlNode::traj_override_callback, this, std::placeholders::_1)); + odom_sub = this->create_subscription( + "odometry", 1, + std::bind(&TrajectoryControlNode::odom_callback, this, std::placeholders::_1)); + + tf_broadcaster = new tf2_ros::TransformBroadcaster(*this); + tf_buffer = new tf2_ros::Buffer(this->get_clock()); + tf_listener = new tf2_ros::TransformListener(*tf_buffer); + + // init publishers + trajectory_vis_pub = + this->create_publisher("trajectory_vis", 1); + tracking_point_pub = this->create_publisher("tracking_point", 1); + look_ahead_pub = this->create_publisher("look_ahead", 1); + drone_point_pub = this->create_publisher("traj_drone_point", 1); + virtual_tracking_point_pub = + this->create_publisher("virtual_tracking_point", 1); + closest_point_pub = this->create_publisher("closest_point", 1); + trajectory_completion_percentage_pub = + this->create_publisher("trajectory_completion_percentage", 1); + trajectory_time_pub = this->create_publisher("trajectory_time", 1); + tracking_error_pub = this->create_publisher("tracking_error", 1); + velocity_pub = + this->create_publisher("tracking_point_velocity_magnitude", 1); + debug_markers_pub = this->create_publisher( + "trajectory_controller_debug_markers", 1); + // broadcaster = new tf::TransformBroadcaster(); + // listener = new tf::TransformListener(); + + // init services + traj_mode_srv = this->create_service( + "set_trajectory_mode", std::bind(&TrajectoryControlNode::set_trajectory_mode, this, + std::placeholders::_1, std::placeholders::_2)); + + // timers + timer = rclcpp::create_timer(this, this->get_clock(), rclcpp::Duration::from_seconds(1. / 20.), + std::bind(&TrajectoryControlNode::timer_callback, this)); + + virtual_time = 0; + actual_time = 0; + prev_time = 0; + virtual_tracking_point_odom.header.frame_id = target_frame; + virtual_tracking_point_odom.child_frame_id = target_frame; + look_ahead_point.header.frame_id = target_frame; + look_ahead_point.child_frame_id = target_frame; + drone_point.header.frame_id = target_frame; + drone_point.child_frame_id = target_frame; + current_velocity = 0.f; + time_multiplier = 1.f; // for forwards or backwards. Forward uses 1.0, Rewind uses -1.0 + transition_dt = target_dt; + new_rewind = false; + + // init variables + prev_vtp_time = 0; +} + +void TrajectoryControlNode::timer_callback() { + static rclcpp::Time prev_execute_time = this->get_clock()->now(); + static rclcpp::Time curr_execute_time = this->get_clock()->now(); + curr_execute_time = this->get_clock()->now(); + double execute_elapsed = (curr_execute_time - prev_execute_time).seconds(); + prev_execute_time = curr_execute_time; + + if (!got_odom) { + RCLCPP_INFO_STREAM(this->get_logger(), "Waiting to receive odometry."); + return; + } + + if (time_multiplier > 1.f) time_multiplier = 1.f; + if (std::isnan(time_multiplier) || !std::isfinite(time_multiplier)) { + RCLCPP_INFO_STREAM(this->get_logger(), + "time_multiplier was an invalid value: " << time_multiplier); + time_multiplier = 1.f; + } + + // figure out what duration into the trajectory we are + rclcpp::Time now = this->get_clock()->now(); + tf2::Vector3 robot_point = tflib::to_tf(odom.pose.pose.position); + double tracking_error = + tflib::to_tf(virtual_tracking_point_odom.pose.position).distance(robot_point); + std_msgs::msg::Float32 tracking_error_msg; + tracking_error_msg.data = tracking_error; + tracking_error_pub->publish(tracking_error_msg); + + // visualization + markers.overwrite(); + markers + .add_sphere(target_frame, now - rclcpp::Duration::from_seconds(0.3), robot_point.x(), + robot_point.y(), robot_point.z(), sphere_radius) + .set_color(0.f, 0.f, 1.f, 0.7f); + trajectory_vis_pub->publish(trajectory->get_markers(now - rclcpp::Duration::from_seconds(0.3), + "traj_controller", 1, 1, 0, 1, false, false, + traj_vis_thickness)); + + // find closest point on entire trajectory + Waypoint closest_wp(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); + nav_msgs::msg::Odometry closest_odom; + bool closest_valid = false; // trajectory->get_closest_waypoint(robot_point, 0, + // trajectory->get_duration(), &closest_wp); + if (closest_valid) + closest_valid = trajectory->get_odom(closest_wp.get_time(), &closest_point_odom, now); + // else + // ROS_INFO("CLOSEST NOT VALID"); + if (closest_valid) closest_point_pub->publish(closest_point_odom); + + double current_virtual_ahead_time = 0.; + + if (trajectory_mode != airstack_msgs::srv::TrajectoryMode::Request::PAUSE) { + if (time_multiplier >= 1.f && + tflib::to_tf(virtual_tracking_point_odom.twist.linear).length() > + min_virtual_tracking_velocity) { + if (new_rewind) RCLCPP_INFO(this->get_logger(), "YO"); + // find closest point within time interval + Waypoint closest_ahead_wp(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); + bool closest_ahead_valid = trajectory->get_closest_waypoint( + robot_point, virtual_time, prev_vtp_time + look_ahead_time, &closest_ahead_wp); + if (closest_ahead_valid) { + virtual_time = closest_ahead_wp.get_time(); + + // visualization + markers + .add_sphere(target_frame, now - rclcpp::Duration::from_seconds(0.3), + closest_ahead_wp.get_x(), closest_ahead_wp.get_y(), + closest_ahead_wp.get_z(), 0.025f) + .set_color(0.f, 1.f, 0.f); + + Waypoint vtp_wp(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); + Waypoint end_wp(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); + bool vtp_valid = trajectory->get_waypoint_sphere_intersection( + virtual_time, search_ahead_factor * sphere_radius, + prev_vtp_time + look_ahead_time, robot_point, sphere_radius, + min_virtual_tracking_velocity, &vtp_wp, &end_wp); + if (vtp_valid) current_virtual_ahead_time = vtp_wp.get_time() - virtual_time; + + // visualization + if (vtp_valid) + markers + .add_sphere(target_frame, now - rclcpp::Duration::from_seconds(0.3), + vtp_wp.get_x(), vtp_wp.get_y(), vtp_wp.get_z(), 0.025f) + .set_color(0.f, 1.f, 0.f); + else + markers + .add_sphere(target_frame, now - rclcpp::Duration::from_seconds(0.3), + closest_ahead_wp.get_x(), closest_ahead_wp.get_y(), + closest_ahead_wp.get_z() + sphere_radius, 0.025f) + .set_color(1.f, 0.f, 0.f); + markers + .add_sphere(target_frame, now - rclcpp::Duration::from_seconds(0.3), + end_wp.get_x(), end_wp.get_y(), end_wp.get_z(), 0.025f) + .set_color(0.f, 0.f, 1.f); + } else + RCLCPP_INFO(this->get_logger(), "AHEAD NOT VALID"); + } else { + if (new_rewind) { + float before = virtual_time; + virtual_time = trajectory->get_skip_ahead_time( + virtual_time, rewind_skip_max_velocity, rewind_skip_max_distance); + RCLCPP_INFO_STREAM(this->get_logger(), + "SKIPPED from " << before << " to " << virtual_time << " (" + << (virtual_time - before) << ")"); + new_rewind = false; + } + virtual_time = std::min(trajectory->get_duration(), + virtual_time + time_multiplier * execute_elapsed); + if (time_multiplier < 1.f) { + time_multiplier += transition_dt; + } + } + } else if (trajectory_mode == airstack_msgs::srv::TrajectoryMode::Request::PAUSE) { + // don't update virtual_time + } + //*/ + + // get virtual waypoint + Waypoint virtual_wp(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); + bool virtual_valid = + trajectory->get_waypoint(virtual_time + current_virtual_ahead_time, &virtual_wp); + if (virtual_valid) { + virtual_valid = + trajectory->get_odom(virtual_wp.get_time(), &virtual_tracking_point_odom, now); + + // prevent oscillating between slow and normal mode + if (tflib::to_tf(virtual_tracking_point_odom.twist.linear).length() <= + min_virtual_tracking_velocity) + virtual_time += current_virtual_ahead_time; + + float look_ahead_multiplier = 1.f; + if (trajectory_mode == airstack_msgs::srv::TrajectoryMode::Request::REWIND) + look_ahead_multiplier = -1.f; + trajectory->get_odom(virtual_wp.get_time() + look_ahead_multiplier * look_ahead_time, + &look_ahead_point, now); + trajectory->get_odom(virtual_wp.get_time(), &drone_point, now); + actual_time = virtual_wp.get_time(); + prev_vtp_time = virtual_wp.get_time(); + } else { + RCLCPP_INFO_STREAM_ONCE( + this->get_logger(), + "Virtual waypoint does not correspond to a waypoint along the " + "reference trajectory. If no trajectory is available yet, this is okay."); + } + + if (trajectory->get_num_waypoints() <= 3) { + virtual_tracking_point_odom.twist.linear.x = 0; + virtual_tracking_point_odom.twist.linear.y = 0; + virtual_tracking_point_odom.twist.linear.z = 0; + } + if (tflib::to_tf(virtual_tracking_point_odom.twist.linear).length() <= + min_virtual_tracking_velocity) { + virtual_tracking_point_odom.twist.linear.x = 0; + virtual_tracking_point_odom.twist.linear.y = 0; + virtual_tracking_point_odom.twist.linear.z = 0; + virtual_tracking_point_odom.acceleration.x = 0; + virtual_tracking_point_odom.acceleration.y = 0; + virtual_tracking_point_odom.acceleration.z = 0; + virtual_tracking_point_odom.jerk.x = 0; + virtual_tracking_point_odom.jerk.y = 0; + virtual_tracking_point_odom.jerk.z = 0; + } + + if (trajectory_mode == airstack_msgs::srv::TrajectoryMode::Request::ROBOT_POSE) { + virtual_tracking_point_odom.pose = odom.pose.pose; + virtual_tracking_point_odom.twist.linear.x = 0; + virtual_tracking_point_odom.twist.linear.y = 0; + virtual_tracking_point_odom.twist.linear.z = 0; + virtual_tracking_point_odom.twist.angular.x = 0; + virtual_tracking_point_odom.twist.angular.y = 0; + virtual_tracking_point_odom.twist.angular.z = 0; + virtual_tracking_point_odom.acceleration.x = 0; + virtual_tracking_point_odom.acceleration.y = 0; + virtual_tracking_point_odom.acceleration.z = 0; + virtual_tracking_point_odom.jerk.x = 0; + virtual_tracking_point_odom.jerk.y = 0; + virtual_tracking_point_odom.jerk.z = 0; + look_ahead_point = virtual_tracking_point_odom; + drone_point = look_ahead_point; + } + + virtual_tracking_point_odom.header.stamp = now; + look_ahead_point.header.stamp = now; + drone_point.header.stamp = now; + + if (trajectory_mode == airstack_msgs::srv::TrajectoryMode::Request::REWIND) { + look_ahead_point.twist.linear.x *= -1.f; + look_ahead_point.twist.linear.y *= -1.f; + look_ahead_point.twist.linear.z *= -1.f; + } + + if (time_multiplier >= -1.f && time_multiplier <= 1.f) { + virtual_tracking_point_odom.twist.linear.x *= time_multiplier; + virtual_tracking_point_odom.twist.linear.y *= time_multiplier; + virtual_tracking_point_odom.twist.linear.z *= time_multiplier; + } + + // When the tracking point reaches the end of the trajectory, the velocity gets set to zero + if (virtual_wp.get_time() >= trajectory->get_duration() || + trajectory_mode == airstack_msgs::srv::TrajectoryMode::Request::PAUSE) { + virtual_tracking_point_odom.twist.linear.x = 0; + virtual_tracking_point_odom.twist.linear.y = 0; + virtual_tracking_point_odom.twist.linear.z = 0; + virtual_tracking_point_odom.twist.angular.x = 0; + virtual_tracking_point_odom.twist.angular.y = 0; + virtual_tracking_point_odom.twist.angular.z = 0; + virtual_tracking_point_odom.acceleration.x = 0; + virtual_tracking_point_odom.acceleration.y = 0; + virtual_tracking_point_odom.acceleration.z = 0; + virtual_tracking_point_odom.jerk.x = 0; + virtual_tracking_point_odom.jerk.y = 0; + virtual_tracking_point_odom.jerk.z = 0; + + look_ahead_point.twist.linear.x = 0; + look_ahead_point.twist.linear.y = 0; + look_ahead_point.twist.linear.z = 0; + look_ahead_point.twist.angular.x = 0; + look_ahead_point.twist.angular.y = 0; + look_ahead_point.twist.angular.z = 0; + look_ahead_point.acceleration.x = 0; + look_ahead_point.acceleration.y = 0; + look_ahead_point.acceleration.z = 0; + look_ahead_point.jerk.x = 0; + look_ahead_point.jerk.y = 0; + look_ahead_point.jerk.z = 0; + drone_point = look_ahead_point; + } + + std_msgs::msg::Float32 velocity_msg; + velocity_msg.data = sqrt( + virtual_tracking_point_odom.twist.linear.x * virtual_tracking_point_odom.twist.linear.x + + virtual_tracking_point_odom.twist.linear.y * virtual_tracking_point_odom.twist.linear.y + + virtual_tracking_point_odom.twist.linear.z * virtual_tracking_point_odom.twist.linear.z); + if (time_multiplier >= 1.f) current_velocity = velocity_msg.data; + velocity_pub->publish(velocity_msg); + tracking_point_pub->publish(virtual_tracking_point_odom); + look_ahead_pub->publish(look_ahead_point); + drone_point_pub->publish(drone_point); + + // create a tf for the tracking point odom + tf2::Stamped vtp_tf = tflib::to_tf(virtual_tracking_point_odom); + geometry_msgs::msg::TransformStamped transform = tf2::toMsg(vtp_tf); + transform.child_frame_id = tf_prefix + "tracking_point"; + geometry_msgs::msg::TransformStamped transform_stabilized = + tf2::toMsg(tflib::get_stabilized(vtp_tf)); + transform_stabilized.child_frame_id = tf_prefix + "tracking_point_stabilized"; + tf_broadcaster->sendTransform(transform); + tf_broadcaster->sendTransform(transform_stabilized); + + // create a tf for the look ahead odom + tf2::Stamped la_tf = tflib::to_tf(look_ahead_point); + geometry_msgs::msg::TransformStamped look_ahead_transform = tf2::toMsg(la_tf); + look_ahead_transform.child_frame_id = tf_prefix + "look_ahead_point"; + geometry_msgs::msg::TransformStamped look_ahead_transform_stabilized = + tf2::toMsg(tflib::get_stabilized(la_tf)); + look_ahead_transform_stabilized.child_frame_id = tf_prefix + "look_ahead_point_stabilized"; + + tf_broadcaster->sendTransform(look_ahead_transform); + tf_broadcaster->sendTransform(look_ahead_transform_stabilized); + + // publish completion percentage + std_msgs::msg::Float32 trajectory_completion_percentage; + trajectory_completion_percentage.data = virtual_time / trajectory->get_duration() * 100.f; + trajectory_completion_percentage_pub->publish(trajectory_completion_percentage); + + // publish current trajectory time + std_msgs::msg::Float32 trajectory_time; + trajectory_time.data = (trajectory_mode == airstack_msgs::srv::TrajectoryMode::Request::REWIND) + ? trajectory->get_duration() - virtual_time + : virtual_time; + trajectory_time_pub->publish(trajectory_time); + + // publish visualization + debug_markers_pub->publish(markers.get_marker_array()); + + new_rewind = false; +} + +void TrajectoryControlNode::set_trajectory_mode( + const std::shared_ptr req, + std::shared_ptr res) { + int prev_trajectory_mode = trajectory_mode; + trajectory_mode = req->mode; + + if (trajectory_mode == airstack_msgs::srv::TrajectoryMode::Request::PAUSE) { + time_multiplier = 0.f; + } + if (trajectory_mode == airstack_msgs::srv::TrajectoryMode::Request::ROBOT_POSE) { + trajectory->clear(); + } else if (trajectory_mode == airstack_msgs::srv::TrajectoryMode::Request::TRACK) { + trajectory->clear(); + } else if (trajectory_mode == airstack_msgs::srv::TrajectoryMode::Request::ADD_SEGMENT) { + if (prev_trajectory_mode != airstack_msgs::srv::TrajectoryMode::Request::PAUSE && + prev_trajectory_mode != airstack_msgs::srv::TrajectoryMode::Request::REWIND && + prev_trajectory_mode != airstack_msgs::srv::TrajectoryMode::Request::ADD_SEGMENT) { + trajectory->clear(); + virtual_time = 0; + actual_time = 0; + } + if (prev_trajectory_mode == airstack_msgs::srv::TrajectoryMode::Request::REWIND) { + virtual_time = trajectory->get_duration() - actual_time; + actual_time = virtual_time; + prev_vtp_time = virtual_time; + *trajectory = trajectory->get_reversed_trajectory(); + + time_multiplier *= -1.f; + if (current_velocity != 0.f) + transition_dt = transition_velocity_scale / current_velocity * target_dt; + else + transition_dt = transition_velocity_scale / 0.1f * target_dt; + } + } else if (trajectory_mode == airstack_msgs::srv::TrajectoryMode::Request::REWIND) { + if (prev_trajectory_mode != airstack_msgs::srv::TrajectoryMode::Request::REWIND) { + virtual_time = trajectory->get_duration() - actual_time; + actual_time = virtual_time; + prev_vtp_time = virtual_time; + *trajectory = trajectory->get_reversed_trajectory(); + new_rewind = true; + + time_multiplier *= -1.f; + if (current_velocity != 0.f) + transition_dt = transition_velocity_scale / current_velocity * target_dt; + else + transition_dt = transition_velocity_scale / 0.1f * target_dt; + } + } + + res->success = true; +} + +void TrajectoryControlNode::traj_seg_to_add_callback( + const airstack_msgs::msg::TrajectoryXYZVYaw::SharedPtr traj) { + if (trajectory_mode == airstack_msgs::srv::TrajectoryMode::Request::ADD_SEGMENT) + trajectory->merge(Trajectory(this, *traj), virtual_time); +} + +void TrajectoryControlNode::traj_override_callback( + const airstack_msgs::msg::TrajectoryXYZVYaw::SharedPtr traj) { + virtual_time = 0; + trajectory->clear(); + trajectory->merge(Trajectory(this, *traj)); +} + +void TrajectoryControlNode::odom_callback(const nav_msgs::msg::Odometry::SharedPtr odom) { + // this->odom = odom; + // An odometry's child frame is the frame of the robot, and the Twist (velocities) are put in + // that frame. This line converts the odometry's position AND velocities to the target frame, + // typically "world" or "map". + if (tflib::transform_odometry(tf_buffer, *odom, target_frame, target_frame, &(this->odom))) { + got_odom = true; + } else { + RCLCPP_INFO_STREAM(this->get_logger(), "Failed to transform odometry in frame " + << odom->header.frame_id << " to target frame " + << target_frame); + } +} + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/trajectory_controller/srv/TrajectoryMode.srv b/robot/ros_ws/src/autonomy/3_local/c_controls/trajectory_controller/srv/TrajectoryMode.srv new file mode 100644 index 00000000..9b354e2c --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/c_controls/trajectory_controller/srv/TrajectoryMode.srv @@ -0,0 +1,9 @@ +int32 PAUSE=0 +int32 ROBOT_POSE=1 +int32 TRACK=2 +int32 ADD_SEGMENT=3 +int32 REWIND=4 + +int32 mode +--- +bool success \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/3_local/c_controls/trajectory_controller/trajectory_controller.png b/robot/ros_ws/src/autonomy/3_local/c_controls/trajectory_controller/trajectory_controller.png new file mode 100644 index 00000000..d9a2eca0 Binary files /dev/null and b/robot/ros_ws/src/autonomy/3_local/c_controls/trajectory_controller/trajectory_controller.png differ diff --git a/robot/ros_ws/src/autonomy/3_local/local_bringup/CMakeLists.txt b/robot/ros_ws/src/autonomy/3_local/local_bringup/CMakeLists.txt new file mode 100644 index 00000000..19c86fbb --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/local_bringup/CMakeLists.txt @@ -0,0 +1,32 @@ +cmake_minimum_required(VERSION 3.8) +project(local_bringup) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +# Install files. +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME}) +# install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) +# install(DIRECTORY params DESTINATION share/${PROJECT_NAME}) + +ament_package() diff --git a/robot/ros_ws/src/autonomy/3_local/local_bringup/LICENSE b/robot/ros_ws/src/autonomy/3_local/local_bringup/LICENSE new file mode 100644 index 00000000..d6456956 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/local_bringup/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/robot/ros_ws/src/autonomy/3_local/local_bringup/launch/local.launch.xml b/robot/ros_ws/src/autonomy/3_local/local_bringup/launch/local.launch.xml new file mode 100644 index 00000000..b3c15b82 --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/local_bringup/launch/local.launch.xml @@ -0,0 +1,85 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + ?> + + \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/3_local/local_bringup/package.xml b/robot/ros_ws/src/autonomy/3_local/local_bringup/package.xml new file mode 100644 index 00000000..4075599f --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/local_bringup/package.xml @@ -0,0 +1,18 @@ + + + + local_bringup + 0.0.0 + TODO: Package description + andrew + Apache-2.0 + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/robot/ros_ws/src/autonomy/3_local/local_bringup/rviz/droan.rviz b/robot/ros_ws/src/autonomy/3_local/local_bringup/rviz/droan.rviz new file mode 100644 index 00000000..e8754fbc --- /dev/null +++ b/robot/ros_ws/src/autonomy/3_local/local_bringup/rviz/droan.rviz @@ -0,0 +1,675 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /TF1/Frames1 + - /Sensors1 + - /Local1 + - /Local1/DROAN1 + - /Local1/Trajectory Controller1 + - /Global1 + Splitter Ratio: 0.590062141418457 + Tree Height: 1085 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: Expansion Cloud +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 100 + Reference Frame: + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + American_Beech: + Value: true + Plane: + Value: true + base_link: + Value: true + base_link_frd: + Value: false + base_link_stabilized: + Value: false + front_stereo: + Value: false + left_camera: + Value: true + look_ahead_point: + Value: false + look_ahead_point_stabilized: + Value: false + map: + Value: true + map_FLU: + Value: false + map_ned: + Value: false + odom: + Value: false + odom_ned: + Value: false + ouster: + Value: false + right_camera: + Value: true + tracking_point: + Value: true + tracking_point_stabilized: + Value: false + world: + Value: false + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + world: + American_Beech: + {} + Plane: + {} + map_FLU: + map: + base_link: + base_link_frd: + {} + front_stereo: + left_camera: + {} + right_camera: + {} + ouster: + {} + base_link_stabilized: + {} + look_ahead_point: + {} + look_ahead_point_stabilized: + {} + map_ned: + {} + tracking_point: + {} + tracking_point_stabilized: + {} + Update Interval: 0 + Value: true + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Front Left RGB + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: sensors/front_stereo/left/image_rect + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 100 + Median window: 5 + Min Value: 0 + Name: Front Left Depth + Normalize Range: false + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: sensors/front_stereo/left/depth + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 6.571824073791504 + Min Value: -0.5682187080383301 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 170; 170; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Lidar + Position Transformer: XYZ + Selectable: true + Size (Pixels): 1 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: sensors/ouster/point_cloud + Use Fixed Frame: true + Use rainbow: true + Value: false + - Angle Tolerance: 0 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: false + Keep: 1 + Name: Odometry + Position Tolerance: 0 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Axes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: odometry_conversion/odometry + Value: false + Enabled: true + Name: Sensors + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/Marker + Enabled: true + Name: Disparity Frustum + Namespaces: + frustum: true + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_1/droan/frustum + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Disparity Map Collision Checking + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_1/droan/disparity_map_debug + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Disparity Graph Poses + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_1/droan/disparity_graph + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Trimmed Global Plan for DROAN + Namespaces: + global_plan: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: droan/local_planner_global_plan_vis + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: ExpansionPoly + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: droan/expansion_poly + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 220 + Min Color: 0; 0; 0 + Min Intensity: 120 + Name: Expansion Cloud + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: droan/expansion_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Traj Library + Namespaces: + trajectory_0: true + trajectory_1: true + trajectory_10: true + trajectory_100: true + trajectory_101: true + trajectory_102: true + trajectory_103: true + trajectory_104: true + trajectory_105: true + trajectory_106: true + trajectory_107: true + trajectory_108: true + trajectory_109: true + trajectory_11: true + trajectory_110: true + trajectory_111: true + trajectory_112: true + trajectory_113: true + trajectory_114: true + trajectory_115: true + trajectory_116: true + trajectory_117: true + trajectory_118: true + trajectory_119: true + trajectory_12: true + trajectory_120: true + trajectory_121: true + trajectory_122: true + trajectory_123: true + trajectory_124: true + trajectory_125: true + trajectory_126: true + trajectory_127: true + trajectory_128: true + trajectory_129: true + trajectory_13: true + trajectory_130: true + trajectory_131: true + trajectory_132: true + trajectory_133: true + trajectory_134: true + trajectory_135: true + trajectory_136: true + trajectory_137: true + trajectory_138: true + trajectory_139: true + trajectory_14: true + trajectory_140: true + trajectory_141: true + trajectory_142: true + trajectory_143: true + trajectory_144: true + trajectory_145: true + trajectory_146: true + trajectory_147: true + trajectory_148: true + trajectory_149: true + trajectory_15: true + trajectory_150: true + trajectory_151: true + trajectory_152: true + trajectory_153: true + trajectory_154: true + trajectory_155: true + trajectory_156: true + trajectory_157: true + trajectory_158: true + trajectory_159: true + trajectory_16: true + trajectory_160: true + trajectory_161: true + trajectory_17: true + trajectory_18: true + trajectory_19: true + trajectory_2: true + trajectory_20: true + trajectory_21: true + trajectory_22: true + trajectory_23: true + trajectory_24: true + trajectory_25: true + trajectory_26: true + trajectory_27: true + trajectory_28: true + trajectory_29: true + trajectory_3: true + trajectory_30: true + trajectory_31: true + trajectory_32: true + trajectory_33: true + trajectory_34: true + trajectory_35: true + trajectory_36: true + trajectory_37: true + trajectory_38: true + trajectory_39: true + trajectory_4: true + trajectory_40: true + trajectory_41: true + trajectory_42: true + trajectory_43: true + trajectory_44: true + trajectory_45: true + trajectory_46: true + trajectory_47: true + trajectory_48: true + trajectory_49: true + trajectory_5: true + trajectory_50: true + trajectory_51: true + trajectory_52: true + trajectory_53: true + trajectory_54: true + trajectory_55: true + trajectory_56: true + trajectory_57: true + trajectory_58: true + trajectory_59: true + trajectory_6: true + trajectory_60: true + trajectory_61: true + trajectory_62: true + trajectory_63: true + trajectory_64: true + trajectory_65: true + trajectory_66: true + trajectory_67: true + trajectory_68: true + trajectory_69: true + trajectory_7: true + trajectory_70: true + trajectory_71: true + trajectory_72: true + trajectory_73: true + trajectory_74: true + trajectory_75: true + trajectory_76: true + trajectory_77: true + trajectory_78: true + trajectory_79: true + trajectory_8: true + trajectory_80: true + trajectory_81: true + trajectory_82: true + trajectory_83: true + trajectory_84: true + trajectory_85: true + trajectory_86: true + trajectory_87: true + trajectory_88: true + trajectory_89: true + trajectory_9: true + trajectory_90: true + trajectory_91: true + trajectory_92: true + trajectory_93: true + trajectory_94: true + trajectory_95: true + trajectory_96: true + trajectory_97: true + trajectory_98: true + trajectory_99: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: droan/trajectory_library_vis + Value: true + Enabled: true + Name: DROAN + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Traj Vis + Namespaces: + traj_controller: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: trajectory_controller/trajectory_vis + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Traj Debug + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: trajectory_controller/trajectory_controller_debug_markers + Value: false + Enabled: true + Name: Trajectory Controller + Enabled: true + Name: Local + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/Marker + Enabled: false + Name: VDB Mapping Marker + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: vdb_mapping/vdb_map_visualization + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 0; 255; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: Global Plan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_1/global_plan + Value: true + Enabled: true + Name: Global + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 8.18502426147461 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 3.3486995697021484 + Y: -0.9512473344802856 + Z: 1.4642823934555054 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.560396134853363 + Target Frame: + Value: Orbit (rviz) + Yaw: 2.143571615219116 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Front Left Depth: + collapsed: false + Front Left RGB: + collapsed: false + Height: 1376 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001e5000004c6fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000004c6000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000002eb000000c9000000000000000000000001000001f6000004c6fc0200000007fb00000016004c006500660074002000430061006d006500720061010000003b000001880000000000000000fb00000014004c006500660074002000440065007000740068010000003b0000016a0000000000000000fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001c00460072006f006e00740020004c0065006600740020005200470042010000003b0000020e0000002800fffffffb0000002000460072006f006e00740020004c006500660074002000440065007000740068010000024f000002b20000002800fffffffb0000000a0056006900650077007300000000fd000001a8000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009ba0000003efc0100000002fb0000000800540069006d00650100000000000009ba0000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000005d3000004c600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2490 + X: 1990 + Y: 27 diff --git a/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/CMakeLists.txt b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/CMakeLists.txt new file mode 100644 index 00000000..47d2a13f --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/CMakeLists.txt @@ -0,0 +1,98 @@ +cmake_minimum_required(VERSION 3.0.2) +set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/CMakeModules" ${CMAKE_MODULE_PATH}) + +option(BUILDING_TESTS "Build unit tests." ON) + +project(vdb_mapping CXX C) + +if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) + message("${PROJECT_NAME}: You did not request a specific build type: selecting 'Release'.") + set(CMAKE_BUILD_TYPE Release) +endif() + +## System dependencies are found with CMake's conventions +find_package(Eigen3 REQUIRED) +find_package(OpenVDB REQUIRED) +find_package(PCL REQUIRED) +find_package(TBB REQUIRED) +find_package(ZSTD REQUIRED) + +########### +## Build ## +########### + + +add_library(${PROJECT_NAME} INTERFACE) + + +### Declare a C++ library +target_include_directories(${PROJECT_NAME} + INTERFACE + $ + $ + ${EIGEN3_INCLUDE_DIR} + ${PCL_INCLUDE_DIRS} + ${ZSTD_INCLUDE_DIRS} + ) + +target_link_libraries(${PROJECT_NAME} + INTERFACE + OpenVDB::openvdb + ${EIGEN3_LIBRARIES} + ${PCL_LIBRARIES} + ${ZSTD_LIBRARIES} + -lImath + ) + +add_library(${PROJECT_NAME}::${PROJECT_NAME} ALIAS ${PROJECT_NAME}) + +## +## Build testing if enabled by option +## +if (BUILDING_TESTS) + enable_testing() + add_subdirectory(tests) +else() + message(STATUS "Building tests disabled.") +endif() + +############# +## Install ## +############# + +install(TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME}_targets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + +install( + EXPORT ${PROJECT_NAME}_targets + DESTINATION lib/cmake/${PROJECT_NAME} + FILE ${PROJECT_NAME}Targets.cmake + NAMESPACE ${PROJECT_NAME}:: + ) + +## Mark cpp header files for installation + install( + DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} + FILES_MATCHING + PATTERN "*.h" + PATTERN "*.hpp" + ) + +include(CMakePackageConfigHelpers) +write_basic_package_version_file(${PROJECT_NAME}ConfigVersion.cmake VERSION 1.0.0 + COMPATIBILITY SameMajorVersion) +install( + FILES + ${PROJECT_NAME}Config.cmake ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}ConfigVersion.cmake + CMakeModules/FindOpenVDB.cmake + CMakeModules/OpenVDBUtils.cmake + CMakeModules/FindBlosc.cmake + DESTINATION + lib/cmake/${PROJECT_NAME} + ) diff --git a/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/CMakeModules/FindBlosc.cmake b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/CMakeModules/FindBlosc.cmake new file mode 100644 index 00000000..44a0f3f5 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/CMakeModules/FindBlosc.cmake @@ -0,0 +1,394 @@ +# Copyright Contributors to the OpenVDB Project +# SPDX-License-Identifier: MPL-2.0 +# +#[=======================================================================[.rst: + +FindBlosc +--------- + +Find Blosc include dirs and libraries + +Use this module by invoking find_package with the form:: + + find_package(Blosc + [version] [EXACT] # Minimum or EXACT version e.g. 1.5.0 + [REQUIRED] # Fail with error if Blosc is not found + ) + +IMPORTED Targets +^^^^^^^^^^^^^^^^ + +``Blosc::blosc`` + This module defines IMPORTED target Blosc::Blosc, if Blosc has been found. + +Result Variables +^^^^^^^^^^^^^^^^ + +This will define the following variables: + +``Blosc_FOUND`` + True if the system has the Blosc library. +``Blosc_VERSION`` + The version of the Blosc library which was found. +``Blosc_INCLUDE_DIRS`` + Include directories needed to use Blosc. +``Blosc_RELEASE_LIBRARIES`` + Libraries needed to link to the release version of Blosc. +``Blosc_RELEASE_LIBRARY_DIRS`` + Blosc release library directories. +``Blosc_DEBUG_LIBRARIES`` + Libraries needed to link to the debug version of Blosc. +``Blosc_DEBUG_LIBRARY_DIRS`` + Blosc debug library directories. + +Deprecated - use [RELEASE|DEBUG] variants: + +``Blosc_LIBRARIES`` + Libraries needed to link to Blosc. +``Blosc_LIBRARY_DIRS`` + Blosc library directories. + +Cache Variables +^^^^^^^^^^^^^^^ + +The following cache variables may also be set: + +``Blosc_INCLUDE_DIR`` + The directory containing ``blosc.h``. +``Blosc_LIBRARY`` + The path to the Blosc library. may include target_link_libraries() debug/optimized keywords +``Blosc_LIBRARY_RELEASE`` + The path to the Blosc release library. +``Blosc_LIBRARY_DEBUG`` + The path to the Blosc debug library. + +Hints +^^^^^ + +Instead of explicitly setting the cache variables, the following variables +may be provided to tell this module where to look. + +``Blosc_ROOT`` + Preferred installation prefix. +``BLOSC_INCLUDEDIR`` + Preferred include directory e.g. /include +``BLOSC_LIBRARYDIR`` + Preferred library directory e.g. /lib +``BLOSC_DEBUG_SUFFIX`` + Suffix of the debug version of blosc. Defaults to "_d", OR the empty string for VCPKG_TOOLCHAIN +``SYSTEM_LIBRARY_PATHS`` + Global list of library paths intended to be searched by and find_xxx call +``BLOSC_USE_STATIC_LIBS`` + Only search for static blosc libraries +``BLOSC_USE_EXTERNAL_SOURCES`` + Set to ON if Blosc has been built using external sources for LZ4, snappy, + zlib and zstd. Default is OFF. +``DISABLE_CMAKE_SEARCH_PATHS`` + Disable CMakes default search paths for find_xxx calls in this module + +#]=======================================================================] + +cmake_minimum_required(VERSION 3.15) +include(GNUInstallDirs) + +mark_as_advanced( + Blosc_INCLUDE_DIR + Blosc_LIBRARY +) + +set(_FIND_BLOSC_ADDITIONAL_OPTIONS "") +if(DISABLE_CMAKE_SEARCH_PATHS) + set(_FIND_BLOSC_ADDITIONAL_OPTIONS NO_DEFAULT_PATH) +endif() + +# Set _BLOSC_ROOT based on a user provided root var. Xxx_ROOT and ENV{Xxx_ROOT} +# are prioritised over the legacy capitalized XXX_ROOT variables for matching +# CMake 3.12 behaviour +# @todo deprecate -D and ENV BLOSC_ROOT from CMake 3.12 +if(Blosc_ROOT) + set(_BLOSC_ROOT ${Blosc_ROOT}) +elseif(DEFINED ENV{Blosc_ROOT}) + set(_BLOSC_ROOT $ENV{Blosc_ROOT}) +elseif(BLOSC_ROOT) + set(_BLOSC_ROOT ${BLOSC_ROOT}) +elseif(DEFINED ENV{BLOSC_ROOT}) + set(_BLOSC_ROOT $ENV{BLOSC_ROOT}) +endif() + +# Additionally try and use pkconfig to find blosc +if(USE_PKGCONFIG) + if(NOT DEFINED PKG_CONFIG_FOUND) + find_package(PkgConfig) + endif() + if(PKG_CONFIG_FOUND) + pkg_check_modules(PC_Blosc QUIET blosc) + endif() +endif() + +# ------------------------------------------------------------------------ +# Search for blosc include DIR +# ------------------------------------------------------------------------ + +set(_BLOSC_INCLUDE_SEARCH_DIRS "") +list(APPEND _BLOSC_INCLUDE_SEARCH_DIRS + ${BLOSC_INCLUDEDIR} + ${_BLOSC_ROOT} + ${PC_Blosc_INCLUDE_DIRS} + ${SYSTEM_LIBRARY_PATHS} +) + +# Look for a standard blosc header file. +find_path(Blosc_INCLUDE_DIR blosc.h + ${_FIND_BLOSC_ADDITIONAL_OPTIONS} + PATHS ${_BLOSC_INCLUDE_SEARCH_DIRS} + PATH_SUFFIXES ${CMAKE_INSTALL_INCLUDEDIR} include +) + +if(EXISTS "${Blosc_INCLUDE_DIR}/blosc.h") + file(STRINGS "${Blosc_INCLUDE_DIR}/blosc.h" + _blosc_version_major_string REGEX "#define BLOSC_VERSION_MAJOR +[0-9]+ " + ) + string(REGEX REPLACE "#define BLOSC_VERSION_MAJOR +([0-9]+).*$" "\\1" + _blosc_version_major_string "${_blosc_version_major_string}" + ) + string(STRIP "${_blosc_version_major_string}" Blosc_VERSION_MAJOR) + + file(STRINGS "${Blosc_INCLUDE_DIR}/blosc.h" + _blosc_version_minor_string REGEX "#define BLOSC_VERSION_MINOR +[0-9]+ " + ) + string(REGEX REPLACE "#define BLOSC_VERSION_MINOR +([0-9]+).*$" "\\1" + _blosc_version_minor_string "${_blosc_version_minor_string}" + ) + string(STRIP "${_blosc_version_minor_string}" Blosc_VERSION_MINOR) + + file(STRINGS "${Blosc_INCLUDE_DIR}/blosc.h" + _blosc_version_release_string REGEX "#define BLOSC_VERSION_RELEASE +[0-9]+ " + ) + string(REGEX REPLACE "#define BLOSC_VERSION_RELEASE +([0-9]+).*$" "\\1" + _blosc_version_release_string "${_blosc_version_release_string}" + ) + string(STRIP "${_blosc_version_release_string}" Blosc_VERSION_RELEASE) + + unset(_blosc_version_major_string) + unset(_blosc_version_minor_string) + unset(_blosc_version_release_string) + + set(Blosc_VERSION ${Blosc_VERSION_MAJOR}.${Blosc_VERSION_MINOR}.${Blosc_VERSION_RELEASE}) +endif() + +# ------------------------------------------------------------------------ +# Search for blosc lib DIR +# ------------------------------------------------------------------------ + +set(_BLOSC_LIBRARYDIR_SEARCH_DIRS "") +list(APPEND _BLOSC_LIBRARYDIR_SEARCH_DIRS + ${BLOSC_LIBRARYDIR} + ${_BLOSC_ROOT} + ${PC_Blosc_LIBRARY_DIRS} + ${SYSTEM_LIBRARY_PATHS} +) + +# Library suffix handling + +set(_BLOSC_ORIG_CMAKE_FIND_LIBRARY_SUFFIXES ${CMAKE_FIND_LIBRARY_SUFFIXES}) + +if(WIN32) + if(BLOSC_USE_STATIC_LIBS) + set(CMAKE_FIND_LIBRARY_SUFFIXES ".lib") + endif() +else() + if(BLOSC_USE_STATIC_LIBS) + set(CMAKE_FIND_LIBRARY_SUFFIXES ".a") + endif() +endif() + +set(Blosc_LIB_COMPONENTS "") +# NOTE: Search for debug version first (see vcpkg hack) +list(APPEND BLOSC_BUILD_TYPES DEBUG RELEASE) + +foreach(BUILD_TYPE ${BLOSC_BUILD_TYPES}) + set(_BLOSC_LIB_NAME blosc) + + set(_BLOSC_CMAKE_IGNORE_PATH ${CMAKE_IGNORE_PATH}) + if(VCPKG_TOOLCHAIN) + # Blosc is installed very strangely in VCPKG (debug/release libs have the + # same name, static build uses external deps, dll doesn't) and blosc itself + # comes with almost zero downstream CMake support for us to detect settings. + # We should not support external package managers in our own modules like + # this, but there doesn't seem to be a work around + if(NOT DEFINED BLOSC_DEBUG_SUFFIX) + set(BLOSC_DEBUG_SUFFIX "") + endif() + if(BUILD_TYPE STREQUAL RELEASE) + if(EXISTS ${Blosc_LIBRARY_DEBUG}) + get_filename_component(_BLOSC_DEBUG_DIR ${Blosc_LIBRARY_DEBUG} DIRECTORY) + list(APPEND CMAKE_IGNORE_PATH ${_BLOSC_DEBUG_DIR}) + endif() + endif() + endif() + + if(BUILD_TYPE STREQUAL DEBUG) + if(NOT DEFINED BLOSC_DEBUG_SUFFIX) + set(BLOSC_DEBUG_SUFFIX _d) + endif() + set(_BLOSC_LIB_NAME "${_BLOSC_LIB_NAME}${BLOSC_DEBUG_SUFFIX}") + endif() + + find_library(Blosc_LIBRARY_${BUILD_TYPE} ${_BLOSC_LIB_NAME} + ${_FIND_BLOSC_ADDITIONAL_OPTIONS} + PATHS ${_BLOSC_LIBRARYDIR_SEARCH_DIRS} + PATH_SUFFIXES ${CMAKE_INSTALL_LIBDIR} lib64 lib + ) + + list(APPEND Blosc_LIB_COMPONENTS ${Blosc_LIBRARY_${BUILD_TYPE}}) + set(CMAKE_IGNORE_PATH ${_BLOSC_CMAKE_IGNORE_PATH}) +endforeach() + +# Reset library suffix + +set(CMAKE_FIND_LIBRARY_SUFFIXES ${_BLOSC_ORIG_CMAKE_FIND_LIBRARY_SUFFIXES}) +unset(_BLOSC_ORIG_CMAKE_FIND_LIBRARY_SUFFIXES) + +if(Blosc_LIBRARY_DEBUG AND Blosc_LIBRARY_RELEASE) + # if the generator is multi-config or if CMAKE_BUILD_TYPE is set for + # single-config generators, set optimized and debug libraries + get_property(_isMultiConfig GLOBAL PROPERTY GENERATOR_IS_MULTI_CONFIG) + if(_isMultiConfig OR CMAKE_BUILD_TYPE) + set(Blosc_LIBRARY optimized ${Blosc_LIBRARY_RELEASE} debug ${Blosc_LIBRARY_DEBUG}) + else() + # For single-config generators where CMAKE_BUILD_TYPE has no value, + # just use the release libraries + set(Blosc_LIBRARY ${Blosc_LIBRARY_RELEASE}) + endif() + # FIXME: This probably should be set for both cases + set(Blosc_LIBRARIES optimized ${Blosc_LIBRARY_RELEASE} debug ${Blosc_LIBRARY_DEBUG}) +endif() + +# if only the release version was found, set the debug variable also to the release version +if(Blosc_LIBRARY_RELEASE AND NOT Blosc_LIBRARY_DEBUG) + set(Blosc_LIBRARY_DEBUG ${Blosc_LIBRARY_RELEASE}) + set(Blosc_LIBRARY ${Blosc_LIBRARY_RELEASE}) + set(Blosc_LIBRARIES ${Blosc_LIBRARY_RELEASE}) +endif() + +# if only the debug version was found, set the release variable also to the debug version +if(Blosc_LIBRARY_DEBUG AND NOT Blosc_LIBRARY_RELEASE) + set(Blosc_LIBRARY_RELEASE ${Blosc_LIBRARY_DEBUG}) + set(Blosc_LIBRARY ${Blosc_LIBRARY_DEBUG}) + set(Blosc_LIBRARIES ${Blosc_LIBRARY_DEBUG}) +endif() + +# If the debug & release library ends up being the same, omit the keywords +if("${Blosc_LIBRARY_RELEASE}" STREQUAL "${Blosc_LIBRARY_DEBUG}") + set(Blosc_LIBRARY ${Blosc_LIBRARY_RELEASE} ) + set(Blosc_LIBRARIES ${Blosc_LIBRARY_RELEASE} ) +endif() + +if(Blosc_LIBRARY) + set(Blosc_FOUND TRUE) +else() + set(Blosc_FOUND FALSE) +endif() + +# ------------------------------------------------------------------------ +# Cache and set Blosc_FOUND +# ------------------------------------------------------------------------ + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(Blosc + FOUND_VAR Blosc_FOUND + REQUIRED_VARS + Blosc_LIBRARY + Blosc_INCLUDE_DIR + VERSION_VAR Blosc_VERSION +) + +if(NOT Blosc_FOUND) + if(Blosc_FIND_REQUIRED) + message(FATAL_ERROR "Unable to find Blosc") + endif() + return() +endif() + +# Partition release/debug lib vars + +set(Blosc_RELEASE_LIBRARIES ${Blosc_LIBRARY_RELEASE}) +get_filename_component(Blosc_RELEASE_LIBRARY_DIRS ${Blosc_RELEASE_LIBRARIES} DIRECTORY) +set(Blosc_DEBUG_LIBRARIES ${Blosc_LIBRARY_DEBUG}) +get_filename_component(Blosc_DEBUG_LIBRARY_DIRS ${Blosc_DEBUG_LIBRARIES} DIRECTORY) +set(Blosc_LIBRARIES ${Blosc_RELEASE_LIBRARIES}) +set(Blosc_LIBRARY_DIRS ${Blosc_RELEASE_LIBRARY_DIRS}) +set(Blosc_INCLUDE_DIRS ${Blosc_INCLUDE_DIR}) +set(Blosc_INCLUDE_DIRS ${Blosc_INCLUDE_DIR}) + +# Configure lib type. If XXX_USE_STATIC_LIBS, we always assume a static +# lib is in use. If win32, we can't mark the import .libs as shared, so +# these are always marked as UNKNOWN. Otherwise, infer from extension. +set(BLOSC_LIB_TYPE UNKNOWN) +if(BLOSC_USE_STATIC_LIBS) + set(BLOSC_LIB_TYPE STATIC) +elseif(UNIX) + get_filename_component(_BLOSC_EXT ${Blosc_LIBRARY_RELEASE} EXT) + if(_BLOSC_EXT STREQUAL ".a") + set(BLOSC_LIB_TYPE STATIC) + elseif(_BLOSC_EXT STREQUAL ".so" OR + _BLOSC_EXT STREQUAL ".dylib") + set(BLOSC_LIB_TYPE SHARED) + endif() +endif() + +get_filename_component(Blosc_LIBRARY_DIRS ${Blosc_LIBRARY_RELEASE} DIRECTORY) + +if(NOT TARGET Blosc::blosc) + add_library(Blosc::blosc ${BLOSC_LIB_TYPE} IMPORTED) + set_target_properties(Blosc::blosc PROPERTIES + INTERFACE_COMPILE_OPTIONS "${PC_Blosc_CFLAGS_OTHER}" + INTERFACE_INCLUDE_DIRECTORIES "${Blosc_INCLUDE_DIRS}") + + # Standard location + set_target_properties(Blosc::blosc PROPERTIES + IMPORTED_LINK_INTERFACE_LANGUAGES "CXX" + IMPORTED_LOCATION "${Blosc_LIBRARY}") + + # Release location + if(EXISTS "${Blosc_LIBRARY_RELEASE}") + set_property(TARGET Blosc::blosc APPEND PROPERTY + IMPORTED_CONFIGURATIONS RELEASE) + set_target_properties(Blosc::blosc PROPERTIES + IMPORTED_LINK_INTERFACE_LANGUAGES_RELEASE "CXX" + IMPORTED_LOCATION_RELEASE "${Blosc_LIBRARY_RELEASE}") + endif() + + # Debug location + if(EXISTS "${Blosc_LIBRARY_DEBUG}") + set_property(TARGET Blosc::blosc APPEND PROPERTY + IMPORTED_CONFIGURATIONS DEBUG) + set_target_properties(Blosc::blosc PROPERTIES + IMPORTED_LINK_INTERFACE_LANGUAGES_DEBUG "CXX" + IMPORTED_LOCATION_DEBUG "${Blosc_LIBRARY_DEBUG}") + endif() + + # Blosc may optionally be compiled with external sources for + # lz4, snappy and zlib . Add them as interface libs if requested + # (there doesn't seem to be a way to figure this out automatically). + # We assume they live along side blosc + if(BLOSC_USE_EXTERNAL_SOURCES) + set_target_properties(Blosc::blosc PROPERTIES + INTERFACE_LINK_DIRECTORIES + "\$<\$:${Blosc_RELEASE_LIBRARY_DIRS}>;\$<\$:${Blosc_DEBUG_LIBRARY_DIRS}>") + target_link_libraries(Blosc::blosc INTERFACE + $<$:lz4;snappy;zlib> + $<$:lz4d;snappyd;zlibd>) + + if(BLOSC_USE_STATIC_LIBS) + target_link_libraries(Blosc::blosc INTERFACE + $<$:zstd_static> + $<$:zstd_staticd>) + else() + target_link_libraries(Blosc::blosc INTERFACE + $<$:zstd> + $<$:zstdd>) + endif() + endif() +endif() + diff --git a/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/CMakeModules/FindGTestPackage.cmake b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/CMakeModules/FindGTestPackage.cmake new file mode 100644 index 00000000..9ccbbdba --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/CMakeModules/FindGTestPackage.cmake @@ -0,0 +1,129 @@ +# SetupGTestTarget +# ------------------------- +# +# Finds the gtest package. Setups a gtest target, so that consumers can use gtest. +# +# setup_gtest_target() +# +# :: +# +# - Defines the variable the target should be stored in. +# +# This function tries to find the googletest framework. +# There are 3 stages: +# 1. Try to find a proper installed googletest package. (e.g. through "make install") +# 2. Try to find a source version from a debian package. (Usually under /usr/src) +# 3. If enabled then try to download the source from the repository directly. +# +# Depending on the found version the variable is set to the correct target. +# +# :: +# + +option(DOWNLOAD_GTEST "Download googletest framework if not found on system." OFF) + +function(find_gtest_package target) + + # 1) Try to find a proper installed gtest suite + + find_package(GTest) + + if(GTEST_FOUND) + if(NOT TARGET GTest::GTest) + if(NOT TARGET GTest) + if(DEFINED GTEST_LIBRARY) + # -lpthread neccessary for Ubuntu 14.04 + set(${target} ${GTEST_LIBRARY} -lpthread PARENT_SCOPE) + else() + + message(FATAL_ERROR "The found gtest package does neither contain GTest nor GTest::GTest. Please make sure GTest are installed correctly.") + + endif() + else() + # -lpthread neccessary for Ubuntu 14.04 + set(${target} GTest -lpthread PARENT_SCOPE) + + if(NOT CMAKE_REQUIRED_QUIET) + message(STATUS "GTest target found as: GTest") + endif() + endif() + else() + set(${target} "GTest::GTest" PARENT_SCOPE) + + if(NOT CMAKE_REQUIRED_QUIET) + message(STATUS "GTest target found as: GTest::GTest") + endif() + endif() + + else() + + # 2) Try to find the source version (on Ubuntu) + + find_path(GTEST_SRC_DIR src/gtest.cc + HINTS + ENV GTEST_DIR + PATHS + /usr/src/googletest/googletest + /usr/src/gtest + DOC "Path to the source version of gtest, if available" ) + + if(GTEST_SRC_DIR) + + if(NOT TARGET gtest) + add_subdirectory(${GTEST_SRC_DIR} "${CMAKE_CURRENT_BINARY_DIR}/gtest" EXCLUDE_FROM_ALL) + # provide the target for caller + set(${target} "gtest" PARENT_SCOPE) + + if(NOT CMAKE_REQUIRED_QUIET) + message(STATUS "GTest target will be build from source: " ${GTEST_SRC_DIR}) + endif() + + endif() + + else() + + # 3) Try to Download and unpack googletest if enabled + # see https://crascit.com/2015/07/25/cmake-gtest/ for an explanation of the complete process + + if(DOWNLOAD_GTEST) + + # try to guess where the .in for the download is + find_path(GTEST_DL_TMPL_DIR FindGTest.CMakeLists.txt.in + PATHS + ${CMAKE_SOURCE_DIR}/CMakeModules/ + ${CMAKE_CURRENT_SOURCE_DIR}/CMakeModules/ + ${CMAKE_CURRENT_SOURCE_DIR}/../CMakeModules/) + + configure_file("${GTEST_DL_TMPL_DIR}/FindGTest.CMakeLists.txt.in" "${CMAKE_BINARY_DIR}/gtest-download/CMakeLists.txt") + execute_process(COMMAND "${CMAKE_COMMAND}" -G "${CMAKE_GENERATOR}" . + WORKING_DIRECTORY "${CMAKE_BINARY_DIR}/gtest-download") + # this is not the build itself, it just downloads the gtest repository + execute_process(COMMAND "${CMAKE_COMMAND}" --build . + WORKING_DIRECTORY "${CMAKE_BINARY_DIR}/gtest-download" + RESULT_VARIABLE GTEST_BUILD_RESULT) + + if(${GTEST_BUILD_RESULT} EQUAL 0) + + set(gtest_force_shared_crt ON CACHE BOOL "" FORCE) + + add_subdirectory("${CMAKE_BINARY_DIR}/gtest-src" "${CMAKE_BINARY_DIR}/gtest-build" EXCLUDE_FROM_ALL) + # provide the target for caller + set(${target} "gtest" PARENT_SCOPE) + + else() + + message(FATAL_ERROR "Could not find gtest package/source nor did downloading from repository succeed. Make sure you have gtest installed.") + + endif() + + else() + + message(FATAL_ERROR "Could not find gtest package nor gtest source. USE DOWNLOAD_GTEST option to build from source.") + + endif() + + endif() + + endif() +endfunction(find_gtest_package) + diff --git a/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/CMakeModules/FindOpenVDB.cmake b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/CMakeModules/FindOpenVDB.cmake new file mode 100644 index 00000000..0dac1de4 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/CMakeModules/FindOpenVDB.cmake @@ -0,0 +1,838 @@ +# Copyright Contributors to the OpenVDB Project +# SPDX-License-Identifier: MPL-2.0 +# +#[=======================================================================[.rst: + +FindOpenVDB +----------- + +Find OpenVDB include dirs, libraries and settings + +Use this module by invoking find_package with the form:: + + find_package(OpenVDB + [version] [EXACT] # Minimum or EXACT version + [REQUIRED] # Fail with error if OpenVDB is not found + [COMPONENTS ...] # OpenVDB libraries by their canonical name + # e.g. "openvdb" for "libopenvdb", + # "pyopenvdb" for the python plugin + # "openvdb_ax" for the OpenVDB AX extension + # "openvdb_houdini" for the houdini plugin + # "nanovdb" for the nanovdb extension + ) + +IMPORTED Targets +^^^^^^^^^^^^^^^^ + +``OpenVDB::openvdb`` + The core openvdb library target. +``OpenVDB::openvdb_je`` + The core openvdb library target with jemalloc. +``OpenVDB::pyopenvdb`` + The openvdb python library target. +``OpenVDB::openvdb_houdini`` + The openvdb houdini library target. +``OpenVDB::openvdb_ax`` + The openvdb_ax library target. +``OpenVDB::nanovdb`` + The nanovdb library target. + +Result Variables +^^^^^^^^^^^^^^^^ + +This will define the following variables: + +``OpenVDB_FOUND`` + True if the system has the OpenVDB library. +``OpenVDB_VERSION`` + The version of the OpenVDB library which was found. +``OpenVDB_INCLUDE_DIRS`` + Include directories needed to use OpenVDB. +``OpenVDB_LIBRARIES`` + Libraries needed to link to OpenVDB. +``OpenVDB_LIBRARY_DIRS`` + OpenVDB library directories. +``OpenVDB_DEFINITIONS`` + Definitions to use when compiling code that uses OpenVDB. +``OpenVDB_${COMPONENT}_FOUND`` + True if the system has the named OpenVDB component. +``OpenVDB_USES_BLOSC`` + True if the OpenVDB Library has been built with blosc support +``OpenVDB_USES_ZLIB`` + True if the OpenVDB Library has been built with zlib support +``OpenVDB_USES_LOG4CPLUS`` + True if the OpenVDB Library has been built with log4cplus support +``OpenVDB_USES_IMATH_HALF`` + True if the OpenVDB Library has been built with Imath half support +``OpenVDB_ABI`` + Set if this module was able to determine the ABI number the located + OpenVDB Library was built against. Unset otherwise. + +Cache Variables +^^^^^^^^^^^^^^^ + +The following cache variables may also be set: + +``OpenVDB_INCLUDE_DIR`` + The directory containing ``openvdb/version.h``. +``OpenVDB_${COMPONENT}_INCLUDE_DIR`` + Individual component include directories for OpenVDB +``OpenVDB_${COMPONENT}_LIBRARY`` + Individual component libraries for OpenVDB + +Hints +^^^^^ + +Instead of explicitly setting the cache variables, the following variables +may be provided to tell this module where to look. + +``OpenVDB_ROOT`` + Preferred installation prefix. +``OPENVDB_INCLUDEDIR`` + Preferred include directory e.g. /include +``OPENVDB_LIBRARYDIR`` + Preferred library directory e.g. /lib +``OPENVDB_${COMPONENT}_ROOT`` + Preferred installation prefix of a specific component. +``OPENVDB_${COMPONENT}_INCLUDEDIR`` + Preferred include directory of a specific component e.g. /include +``OPENVDB_${COMPONENT}_LIBRARYDIR`` + Preferred library directory of a specific component e.g. /lib +``SYSTEM_LIBRARY_PATHS`` + Global list of library paths intended to be searched by and find_xxx call +``OPENVDB_USE_STATIC_LIBS`` + Only search for static openvdb libraries +``DISABLE_CMAKE_SEARCH_PATHS`` + Disable CMakes default search paths for find_xxx calls in this module + +#]=======================================================================] + +cmake_minimum_required(VERSION 3.15) +include(GNUInstallDirs) + + +# Include utility functions for version information +include(${CMAKE_CURRENT_LIST_DIR}/OpenVDBUtils.cmake) + +mark_as_advanced( + OpenVDB_INCLUDE_DIR + OpenVDB_LIBRARY +) + +set(_FIND_OPENVDB_ADDITIONAL_OPTIONS "") +if(DISABLE_CMAKE_SEARCH_PATHS) + set(_FIND_OPENVDB_ADDITIONAL_OPTIONS NO_DEFAULT_PATH) +endif() + +set(_OPENVDB_COMPONENT_LIST + openvdb + openvdb_je + pyopenvdb + openvdb_ax + openvdb_houdini + nanovdb +) + +if(OpenVDB_FIND_COMPONENTS) + set(OPENVDB_COMPONENTS_PROVIDED TRUE) + set(_IGNORED_COMPONENTS "") + foreach(COMPONENT ${OpenVDB_FIND_COMPONENTS}) + if(NOT ${COMPONENT} IN_LIST _OPENVDB_COMPONENT_LIST) + list(APPEND _IGNORED_COMPONENTS ${COMPONENT}) + endif() + endforeach() + + if(_IGNORED_COMPONENTS) + message(STATUS "Ignoring unknown components of OpenVDB:") + foreach(COMPONENT ${_IGNORED_COMPONENTS}) + message(STATUS " ${COMPONENT}") + endforeach() + list(REMOVE_ITEM OpenVDB_FIND_COMPONENTS ${_IGNORED_COMPONENTS}) + endif() +else() + set(OPENVDB_COMPONENTS_PROVIDED FALSE) + set(OpenVDB_FIND_COMPONENTS openvdb) +endif() + +# always make sure openvdb is picked up as a component i.e. +# find_package(OpenVDB COMPONENTS pyopenvdb) results in both +# openvdb and pyopenvdb targets. Also make sure it appears +# first in the component lists. +list(INSERT OpenVDB_FIND_COMPONENTS 0 openvdb) +list(REMOVE_DUPLICATES OpenVDB_FIND_COMPONENTS) + +# Set _OPENVDB_ROOT based on a user provided root var. Xxx_ROOT and ENV{Xxx_ROOT} +# are prioritised over the legacy capitalized XXX_ROOT variables for matching +# CMake 3.12 behaviour +# @todo deprecate -D and ENV OPENVDB_ROOT from CMake 3.12 +if(OpenVDB_ROOT) + set(_OPENVDB_ROOT ${OpenVDB_ROOT}) +elseif(DEFINED ENV{OpenVDB_ROOT}) + set(_OPENVDB_ROOT $ENV{OpenVDB_ROOT}) +elseif(OPENVDB_ROOT) + set(_OPENVDB_ROOT ${OPENVDB_ROOT}) +elseif(DEFINED ENV{OPENVDB_ROOT}) + set(_OPENVDB_ROOT $ENV{OPENVDB_ROOT}) +endif() + +# Additionally try and use pkconfig to find OpenVDB +if(USE_PKGCONFIG) + if(NOT DEFINED PKG_CONFIG_FOUND) + find_package(PkgConfig) + endif() + pkg_check_modules(PC_OpenVDB QUIET OpenVDB) +endif() + +# This CMake module supports being called from external packages AND from +# within the OpenVDB repository for building openvdb components with the +# core library build disabled. Determine where we are being called from: +# +# (repo structure = /cmake/FindOpenVDB.cmake) +# (inst structure = /lib/cmake/OpenVDB/FindOpenVDB.cmake) + +get_filename_component(_DIR_NAME ${CMAKE_CURRENT_LIST_DIR} NAME) + +if(${_DIR_NAME} STREQUAL "cmake") + # Called from root repo for openvdb components +elseif(${_DIR_NAME} STREQUAL "OpenVDB") + # Set the install variable to track directories if this is being called from + # an installed location and from another package. The expected installation + # directory structure is: + # /lib/cmake/OpenVDB/FindOpenVDB.cmake + # /include + # /bin + get_filename_component(_IMPORT_PREFIX ${CMAKE_CURRENT_LIST_DIR} DIRECTORY) + get_filename_component(_IMPORT_PREFIX ${_IMPORT_PREFIX} DIRECTORY) + get_filename_component(_IMPORT_PREFIX ${_IMPORT_PREFIX} DIRECTORY) + set(_OPENVDB_INSTALL ${_IMPORT_PREFIX}) + list(APPEND _OPENVDB_ROOT ${_OPENVDB_INSTALL}) +endif() + +unset(_DIR_NAME) +unset(_IMPORT_PREFIX) + +# ------------------------------------------------------------------------ +# Search for OpenVDB include DIR +# ------------------------------------------------------------------------ + +set(_OPENVDB_INCLUDE_SEARCH_DIRS "") +list(APPEND _OPENVDB_INCLUDE_SEARCH_DIRS + ${OPENVDB_INCLUDEDIR} + ${_OPENVDB_ROOT} + ${PC_OpenVDB_INCLUDE_DIRS} + ${SYSTEM_LIBRARY_PATHS} +) + +foreach(COMPONENT ${OpenVDB_FIND_COMPONENTS}) + # Add in extra component paths + set(_VDB_COMPONENT_SEARCH_DIRS ${_OPENVDB_INCLUDE_SEARCH_DIRS}) + list(APPEND _VDB_COMPONENT_SEARCH_DIRS + ${OPENVDB_${COMPONENT}_ROOT} + ${OPENVDB_${COMPONENT}_INCLUDEDIR} + ) + if(_VDB_COMPONENT_SEARCH_DIRS) + list(REMOVE_DUPLICATES _VDB_COMPONENT_SEARCH_DIRS) + endif() + + # Look for a standard header files. + if(${COMPONENT} STREQUAL "openvdb") + # Look for a standard OpenVDB header file. + find_path(OpenVDB_${COMPONENT}_INCLUDE_DIR openvdb/version.h + ${_FIND_OPENVDB_ADDITIONAL_OPTIONS} + PATHS ${_VDB_COMPONENT_SEARCH_DIRS} + PATH_SUFFIXES + ${CMAKE_INSTALL_INCLUDEDIR} + include + ) + elseif(${COMPONENT} STREQUAL "pyopenvdb") + find_path(OpenVDB_${COMPONENT}_INCLUDE_DIR pyopenvdb.h + ${_FIND_OPENVDB_ADDITIONAL_OPTIONS} + PATHS ${_VDB_COMPONENT_SEARCH_DIRS} + PATH_SUFFIXES + ${CMAKE_INSTALL_INCLUDEDIR}/openvdb/python + ${CMAKE_INSTALL_INCLUDEDIR}/openvdb + ${CMAKE_INSTALL_INCLUDEDIR} + include + ) + elseif(${COMPONENT} STREQUAL "openvdb_ax") + # Look for a standard OpenVDB header file. + find_path(OpenVDB_${COMPONENT}_INCLUDE_DIR compiler/Compiler.h + ${_FIND_OPENVDB_ADDITIONAL_OPTIONS} + PATHS ${_VDB_COMPONENT_SEARCH_DIRS} + PATH_SUFFIXES + ${CMAKE_INSTALL_INCLUDEDIR}/openvdb/openvdb_ax + ${CMAKE_INSTALL_INCLUDEDIR}/openvdb_ax + ${CMAKE_INSTALL_INCLUDEDIR} + include + ) + elseif(${COMPONENT} STREQUAL "openvdb_houdini") + # @note Expects both houdini_utils and openvdb_houdini folders + # to be located in the same place + find_path(OpenVDB_${COMPONENT}_INCLUDE_DIR openvdb_houdini/SOP_NodeVDB.h + ${_FIND_OPENVDB_ADDITIONAL_OPTIONS} + PATHS ${_VDB_COMPONENT_SEARCH_DIRS} + PATH_SUFFIXES + ${CMAKE_INSTALL_INCLUDEDIR}/openvdb + ${CMAKE_INSTALL_INCLUDEDIR} + include + ) + elseif(${COMPONENT} STREQUAL "nanovdb") + # Look for NanoVDB.h + find_path(OpenVDB_${COMPONENT}_INCLUDE_DIR NanoVDB.h + ${_FIND_OPENVDB_ADDITIONAL_OPTIONS} + PATHS ${_VDB_COMPONENT_SEARCH_DIRS} + PATH_SUFFIXES + ${CMAKE_INSTALL_INCLUDEDIR}/nanovdb + ${CMAKE_INSTALL_INCLUDEDIR} + include + ) + endif() + unset(_VDB_COMPONENT_SEARCH_DIRS) +endforeach() + +set(OpenVDB_INCLUDE_DIR ${OpenVDB_openvdb_INCLUDE_DIR} + CACHE PATH "The OpenVDB core include directory") + +set(_OPENVDB_VERSION_HEADER "${OpenVDB_INCLUDE_DIR}/openvdb/version.h") +OPENVDB_VERSION_FROM_HEADER("${_OPENVDB_VERSION_HEADER}" + VERSION OpenVDB_VERSION + MAJOR OpenVDB_MAJOR_VERSION + MINOR OpenVDB_MINOR_VERSION + PATCH OpenVDB_PATCH_VERSION + ABI OpenVDB_ABI_FROM_HEADER # will be OpenVDB_MAJOR_VERSION prior to 8.1.0 +) + +if(OpenVDB_VERSION VERSION_LESS 8.1.0) + set(_OPENVDB_HAS_NEW_VERSION_HEADER FALSE) + # ABI gets computed later +else() + set(_OPENVDB_HAS_NEW_VERSION_HEADER TRUE) + set(OpenVDB_ABI ${OpenVDB_ABI_FROM_HEADER}) +endif() +unset(OpenVDB_ABI_FROM_HEADER) + +# ------------------------------------------------------------------------ +# Search for OPENVDB lib DIR +# ------------------------------------------------------------------------ + +set(_OPENVDB_LIBRARYDIR_SEARCH_DIRS "") + +# Append to _OPENVDB_LIBRARYDIR_SEARCH_DIRS in priority order + +list(APPEND _OPENVDB_LIBRARYDIR_SEARCH_DIRS + ${OPENVDB_LIBRARYDIR} + ${_OPENVDB_ROOT} + ${PC_OpenVDB_LIBRARY_DIRS} + ${SYSTEM_LIBRARY_PATHS} +) + +# Library suffix handling + +set(_OPENVDB_ORIG_CMAKE_FIND_LIBRARY_SUFFIXES ${CMAKE_FIND_LIBRARY_SUFFIXES}) + +set(OPENVDB_PYTHON_PATH_SUFFIXES + lib64/python + lib64/python2.7 + lib64/python3 + lib/python + lib/python2.7 + lib/python3 +) + +# Recurse through all the site-packages and dist-packages on the file system +file(GLOB PYTHON_SITE_PACKAGES ${CMAKE_INSTALL_FULL_LIBDIR}/python**/*) +foreach(_site_package_full_dir ${PYTHON_SITE_PACKAGES}) + string(REPLACE ${CMAKE_INSTALL_FULL_LIBDIR} "${CMAKE_INSTALL_LIBDIR}" + _site_package_dir ${_site_package_full_dir}) + list(APPEND OPENVDB_PYTHON_PATH_SUFFIXES ${_site_package_dir}) +endforeach() + +set(OPENVDB_LIB_PATH_SUFFIXES + ${CMAKE_INSTALL_LIBDIR} + lib64 + lib +) + +list(REMOVE_DUPLICATES OPENVDB_PYTHON_PATH_SUFFIXES) +list(REMOVE_DUPLICATES OPENVDB_LIB_PATH_SUFFIXES) + +# Static library setup +if(WIN32) + if(OPENVDB_USE_STATIC_LIBS) + set(CMAKE_FIND_LIBRARY_SUFFIXES ".lib") + endif() +else() + if(OPENVDB_USE_STATIC_LIBS) + set(CMAKE_FIND_LIBRARY_SUFFIXES ".a") + endif() +endif() + +set(OpenVDB_LIB_COMPONENTS "") + +foreach(COMPONENT ${OpenVDB_FIND_COMPONENTS}) + message("COMPONENT = " ${COMPONENT}) + set(LIB_NAME ${COMPONENT}) + + # Add in extra component paths + set(_VDB_COMPONENT_SEARCH_DIRS ${_OPENVDB_LIBRARYDIR_SEARCH_DIRS}) + list(APPEND _VDB_COMPONENT_SEARCH_DIRS + ${OPENVDB_${COMPONENT}_ROOT} + ${OPENVDB_${COMPONENT}_LIBRARYDIR} + ) + + if(${COMPONENT} STREQUAL "pyopenvdb") + set(_OPENVDB_ORIG_CMAKE_FIND_LIBRARY_PREFIXES ${CMAKE_FIND_LIBRARY_PREFIXES}) + set(CMAKE_FIND_LIBRARY_PREFIXES ";lib") # find non-prefixed + find_library(OpenVDB_${COMPONENT}_LIBRARY ${LIB_NAME} + ${_FIND_OPENVDB_ADDITIONAL_OPTIONS} + PATHS ${_VDB_COMPONENT_SEARCH_DIRS} + PATH_SUFFIXES ${OPENVDB_PYTHON_PATH_SUFFIXES} + ) + set(CMAKE_FIND_LIBRARY_PREFIXES ${_OPENVDB_ORIG_CMAKE_FIND_LIBRARY_PREFIXES}) + elseif(${COMPONENT} STREQUAL "openvdb_je") + # alias to the result of openvdb which should be handled first + set(OpenVDB_${COMPONENT}_LIBRARY ${OpenVDB_openvdb_LIBRARY}) + else() + find_library(OpenVDB_${COMPONENT}_LIBRARY ${LIB_NAME} + ${_FIND_OPENVDB_ADDITIONAL_OPTIONS} + PATHS ${_VDB_COMPONENT_SEARCH_DIRS} + PATH_SUFFIXES ${OPENVDB_LIB_PATH_SUFFIXES} + ) + endif() + + list(APPEND OpenVDB_LIB_COMPONENTS ${OpenVDB_${COMPONENT}_LIBRARY}) + if(${COMPONENT} STREQUAL "nanovdb") + # nanovdb is headers-only, no lib component + if(OpenVDB_${COMPONENT}_INCLUDE_DIR) + set(OpenVDB_${COMPONENT}_FOUND TRUE) + else() + set(OpenVDB_${COMPONENT}_FOUND FALSE) + endif() + else() + if(OpenVDB_${COMPONENT}_LIBRARY) + set(OpenVDB_${COMPONENT}_FOUND TRUE) + else() + set(OpenVDB_${COMPONENT}_FOUND FALSE) + endif() + endif() + unset(_VDB_COMPONENT_SEARCH_DIRS) +endforeach() + +unset(OPENVDB_PYTHON_PATH_SUFFIXES) +unset(OPENVDB_LIB_PATH_SUFFIXES) + +# Reset library suffix + +set(CMAKE_FIND_LIBRARY_SUFFIXES ${_OPENVDB_ORIG_CMAKE_FIND_LIBRARY_SUFFIXES}) +unset(_OPENVDB_ORIG_CMAKE_FIND_LIBRARY_SUFFIXES) + +# ------------------------------------------------------------------------ +# Cache and set OPENVDB_FOUND +# ------------------------------------------------------------------------ +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(OpenVDB + FOUND_VAR OpenVDB_FOUND + REQUIRED_VARS + OpenVDB_INCLUDE_DIR + OpenVDB_LIB_COMPONENTS + VERSION_VAR OpenVDB_VERSION + HANDLE_COMPONENTS +) + +# ------------------------------------------------------------------------ +# Determine ABI number +# ------------------------------------------------------------------------ + +# Set the ABI number the library was built against. The old system, +# which didn't define the ABI in the build config, uses vdb_print + +if(NOT _OPENVDB_HAS_NEW_VERSION_HEADER) + if(_OPENVDB_INSTALL) + OPENVDB_ABI_VERSION_FROM_PRINT( + "${_OPENVDB_INSTALL}/bin/vdb_print" + ABI OpenVDB_ABI + ) + else() + # Try and find vdb_print from the include path + OPENVDB_ABI_VERSION_FROM_PRINT( + "${OpenVDB_INCLUDE_DIR}/../bin/vdb_print" + ABI OpenVDB_ABI + ) + endif() +endif() + +if(NOT OpenVDB_FIND_QUIET) + if(NOT OpenVDB_ABI) + message(WARNING "Unable to determine OpenVDB ABI version from OpenVDB " + "installation. The library major version \"${OpenVDB_MAJOR_VERSION}\" " + "will be inferred. If this is not correct, use " + "add_definitions(-DOPENVDB_ABI_VERSION_NUMBER=N)" + ) + else() + message(STATUS "OpenVDB ABI Version: ${OpenVDB_ABI}") + endif() +endif() + +# ------------------------------------------------------------------------ +# Handle OpenVDB dependencies and interface settings +# ------------------------------------------------------------------------ + +# Handle openvdb_houdini first to configure search paths + +if(openvdb_houdini IN_LIST OpenVDB_FIND_COMPONENTS) + include(OpenVDBHoudiniSetup) +endif() + +# Add standard dependencies + +find_package(TBB REQUIRED COMPONENTS tbb) + +if(NOT OPENVDB_USE_STATIC_LIBS AND NOT Boost_USE_STATIC_LIBS) + # @note Both of these must be set for Boost 1.70 (VFX2020) to link against + # boost shared libraries (more specifically libraries built with -fPIC). + # http://boost.2283326.n4.nabble.com/CMake-config-scripts-broken-in-1-70-td4708957.html + # https://github.com/boostorg/boost_install/commit/160c7cb2b2c720e74463865ef0454d4c4cd9ae7c + set(BUILD_SHARED_LIBS ON) + set(Boost_USE_STATIC_LIBS OFF) +endif() + +find_package(Boost REQUIRED COMPONENTS iostreams system) + +# Add deps for pyopenvdb +# @todo track for numpy + +if(pyopenvdb IN_LIST OpenVDB_FIND_COMPONENTS) + find_package(PythonLibs REQUIRED) + + # Boost python handling - try and find both python and pythonXx (version suffixed). + # Prioritize the version suffixed library, failing if neither exist. + + find_package(Boost ${MINIMUM_BOOST_VERSION} + QUIET COMPONENTS python${PYTHON_VERSION_MAJOR}${PYTHON_VERSION_MINOR} + ) + + if(TARGET Boost::python${PYTHON_VERSION_MAJOR}${PYTHON_VERSION_MINOR}) + set(BOOST_PYTHON_LIB "python${PYTHON_VERSION_MAJOR}${PYTHON_VERSION_MINOR}") + message(STATUS "Found boost_python${PYTHON_VERSION_MAJOR}${PYTHON_VERSION_MINOR}") + else() + find_package(Boost ${MINIMUM_BOOST_VERSION} QUIET COMPONENTS python) + if(TARGET Boost::python) + set(BOOST_PYTHON_LIB "python") + message(STATUS "Found non-suffixed boost_python, assuming to be python version " + "\"${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR}\" compatible" + ) + else() + message(FATAL_ERROR "Unable to find boost_python or " + "boost_python${PYTHON_VERSION_MAJOR}${PYTHON_VERSION_MINOR}." + ) + endif() + endif() +endif() + +# Add deps for openvdb_ax + +if(openvdb_ax IN_LIST OpenVDB_FIND_COMPONENTS) + find_package(LLVM REQUIRED) + find_library(found_LLVM LLVM HINTS ${LLVM_LIBRARY_DIRS}) + + if(found_LLVM) + set(LLVM_LIBS "LLVM") + else() + llvm_map_components_to_libnames(_llvm_libs + native core executionengine support mcjit passes objcarcopts) + set(LLVM_LIBS "${_llvm_libs}") + endif() + + if(NOT OpenVDB_FIND_QUIET) + message(STATUS "Found LLVM: ${LLVM_DIR} (found version \"${LLVM_PACKAGE_VERSION}\")") + endif() +endif() + +# As the way we resolve optional libraries relies on library file names, use +# the configuration options from the main CMakeLists.txt to allow users +# to manually identify the requirements of OpenVDB builds if they know them. +set(OpenVDB_USES_BLOSC ${USE_BLOSC}) +set(OpenVDB_USES_ZLIB ${USE_ZLIB}) +set(OpenVDB_USES_LOG4CPLUS ${USE_LOG4CPLUS}) +set(OpenVDB_USES_IMATH_HALF ${USE_IMATH_HALF}) +set(OpenVDB_DEFINITIONS) + +if(WIN32) + if(OPENVDB_USE_STATIC_LIBS) + list(APPEND OpenVDB_DEFINITIONS OPENVDB_STATICLIB) + else() + list(APPEND OpenVDB_DEFINITIONS OPENVDB_DLL) + endif() + # Newer version of OpenVDB define these in Platform.h, but they are also + # provided here to maintain backwards compatibility with header include + # others + list(APPEND OpenVDB_DEFINITIONS _WIN32) + list(APPEND OpenVDB_DEFINITIONS NOMINMAX) +endif() + +if(OpenVDB_ABI) + # Newer version of OpenVDB defines this in version.h, but it is are also + # provided here to maintain backwards compatibility with header include + # others + list(APPEND OpenVDB_DEFINITIONS OPENVDB_ABI_VERSION_NUMBER=${OpenVDB_ABI}) +endif() + +# Configure deps + +if(_OPENVDB_HAS_NEW_VERSION_HEADER) + OPENVDB_GET_VERSION_DEFINE(${_OPENVDB_VERSION_HEADER} "OPENVDB_USE_IMATH_HALF" OpenVDB_USES_IMATH_HALF) + OPENVDB_GET_VERSION_DEFINE(${_OPENVDB_VERSION_HEADER} "OPENVDB_USE_BLOSC" OpenVDB_USES_BLOSC) + OPENVDB_GET_VERSION_DEFINE(${_OPENVDB_VERSION_HEADER} "OPENVDB_USE_ZLIB" OpenVDB_USES_ZLIB) +elseif(NOT OPENVDB_USE_STATIC_LIBS) + # Use GetPrerequisites to see which libraries this OpenVDB lib has linked to + # which we can query for optional deps. This basically runs ldd/otoll/objdump + # etc to track deps. We could use a vdb_config binary tools here to improve + # this process + include(GetPrerequisites) + + set(_EXCLUDE_SYSTEM_PREREQUISITES 1) + set(_RECURSE_PREREQUISITES 0) + set(_OPENVDB_PREREQUISITE_LIST) + + get_prerequisites(${OpenVDB_openvdb_LIBRARY} + _OPENVDB_PREREQUISITE_LIST + ${_EXCLUDE_SYSTEM_PREREQUISITES} + ${_RECURSE_PREREQUISITES} + "" + "${SYSTEM_LIBRARY_PATHS}" + ) + + unset(_EXCLUDE_SYSTEM_PREREQUISITES) + unset(_RECURSE_PREREQUISITES) + + # Search for optional dependencies + foreach(PREREQUISITE ${_OPENVDB_PREREQUISITE_LIST}) + set(_HAS_DEP) + get_filename_component(PREREQUISITE ${PREREQUISITE} NAME) + + string(FIND ${PREREQUISITE} "blosc" _HAS_DEP) + if(NOT ${_HAS_DEP} EQUAL -1) + set(OpenVDB_USES_BLOSC ON) + endif() + + string(FIND ${PREREQUISITE} "zlib" _HAS_DEP) + if(NOT ${_HAS_DEP} EQUAL -1) + set(OpenVDB_USES_ZLIB ON) + endif() + + string(FIND ${PREREQUISITE} "log4cplus" _HAS_DEP) + if(NOT ${_HAS_DEP} EQUAL -1) + set(OpenVDB_USES_LOG4CPLUS ON) + endif() + + string(FIND ${PREREQUISITE} "Half" _HAS_DEP) + if(NOT ${_HAS_DEP} EQUAL -1) + set(OpenVDB_USES_IMATH_HALF ON) + endif() + endforeach() + + unset(_OPENVDB_PREREQUISITE_LIST) +endif() + +if(OpenVDB_USES_BLOSC) + find_package(Blosc REQUIRED) +endif() + +if(OpenVDB_USES_ZLIB) + find_package(ZLIB REQUIRED) +endif() + +if(OpenVDB_USES_LOG4CPLUS) + find_package(Log4cplus REQUIRED) +endif() + +if(OpenVDB_USES_IMATH_HALF) + find_package(Imath CONFIG) + if (NOT TARGET Imath::Imath) + find_package(IlmBase REQUIRED COMPONENTS Half) + endif() + + if(WIN32) + # @note OPENVDB_OPENEXR_STATICLIB is old functionality and should be removed + if(OPENEXR_USE_STATIC_LIBS OR + (${ILMBASE_LIB_TYPE} STREQUAL STATIC_LIBRARY) OR + (${IMATH_LIB_TYPE} STREQUAL STATIC_LIBRARY)) + list(APPEND OpenVDB_DEFINITIONS OPENVDB_OPENEXR_STATICLIB) + endif() + endif() +endif() + +if(UNIX) + find_package(Threads REQUIRED) +endif() + +# Set deps. Note that the order here is important. If we're building against +# Houdini 17.5 we must include IlmBase deps first to ensure the users chosen +# namespaced headers are correctly prioritized. Otherwise other include paths +# from shared installs (including houdini) may pull in the wrong headers + +set(_OPENVDB_VISIBLE_DEPENDENCIES + Boost::iostreams + Boost::system +) + +if(OpenVDB_USES_IMATH_HALF) + list(APPEND _OPENVDB_VISIBLE_DEPENDENCIES $ $) +endif() + +if(OpenVDB_USES_LOG4CPLUS) + list(APPEND _OPENVDB_VISIBLE_DEPENDENCIES Log4cplus::log4cplus) + list(APPEND OpenVDB_DEFINITIONS OPENVDB_USE_LOG4CPLUS) +endif() + +list(APPEND _OPENVDB_VISIBLE_DEPENDENCIES + TBB::tbb +) +if(UNIX) + list(APPEND _OPENVDB_VISIBLE_DEPENDENCIES + Threads::Threads + ) +endif() + +set(_OPENVDB_HIDDEN_DEPENDENCIES) + +if(NOT OPENVDB_USE_STATIC_LIBS) + if(OpenVDB_USES_BLOSC) + list(APPEND _OPENVDB_HIDDEN_DEPENDENCIES Blosc::blosc) + endif() + if(OpenVDB_USES_ZLIB) + list(APPEND _OPENVDB_HIDDEN_DEPENDENCIES ZLIB::ZLIB) + endif() +endif() + +if(openvdb_je IN_LIST OpenVDB_FIND_COMPONENTS) + find_package(Jemalloc REQUIRED) +endif() + +# ------------------------------------------------------------------------ +# Configure imported targets +# ------------------------------------------------------------------------ + +set(OpenVDB_LIBRARIES ${OpenVDB_LIB_COMPONENTS}) +set(OpenVDB_INCLUDE_DIRS ${OpenVDB_INCLUDE_DIR}) + +set(OpenVDB_LIBRARY_DIRS "") +foreach(LIB ${OpenVDB_LIB_COMPONENTS}) + get_filename_component(_OPENVDB_LIBDIR ${LIB} DIRECTORY) + list(APPEND OpenVDB_LIBRARY_DIRS ${_OPENVDB_LIBDIR}) +endforeach() +list(REMOVE_DUPLICATES OpenVDB_LIBRARY_DIRS) + +# OpenVDB::openvdb + +if(NOT TARGET OpenVDB::openvdb) + set(OPENVDB_openvdb_LIB_TYPE UNKNOWN) + if(OPENVDB_USE_STATIC_LIBS) + set(OPENVDB_openvdb_LIB_TYPE STATIC) + elseif(UNIX) + get_filename_component(_OPENVDB_openvdb_EXT + ${OpenVDB_openvdb_LIBRARY} EXT) + if(_OPENVDB_openvdb_EXT STREQUAL ".a") + set(OPENVDB_openvdb_LIB_TYPE STATIC) + elseif(_OPENVDB_openvdb_EXT STREQUAL ".so" OR + _OPENVDB_openvdb_EXT STREQUAL ".dylib") + set(OPENVDB_openvdb_LIB_TYPE SHARED) + endif() + endif() + + add_library(OpenVDB::openvdb ${OPENVDB_openvdb_LIB_TYPE} IMPORTED) + set_target_properties(OpenVDB::openvdb PROPERTIES + IMPORTED_LOCATION "${OpenVDB_openvdb_LIBRARY}" + INTERFACE_COMPILE_OPTIONS "${PC_OpenVDB_CFLAGS_OTHER}" + INTERFACE_COMPILE_DEFINITIONS "${OpenVDB_DEFINITIONS}" + INTERFACE_INCLUDE_DIRECTORIES "${OpenVDB_INCLUDE_DIR}" + IMPORTED_LINK_DEPENDENT_LIBRARIES "${_OPENVDB_HIDDEN_DEPENDENCIES}" # non visible deps + INTERFACE_LINK_LIBRARIES "${_OPENVDB_VISIBLE_DEPENDENCIES}" # visible deps (headers) + INTERFACE_COMPILE_FEATURES cxx_std_14 + ) +endif() + +# OpenVDB::openvdb_je + +if(OpenVDB_openvdb_je_LIBRARY) + if(NOT TARGET OpenVDB::openvdb_je) + add_library(OpenVDB::openvdb_je INTERFACE IMPORTED) + target_link_libraries(OpenVDB::openvdb_je INTERFACE OpenVDB::openvdb) + target_link_libraries(OpenVDB::openvdb_je INTERFACE Jemalloc::jemalloc) + endif() +endif() + +# OpenVDB::pyopenvdb + +if(OpenVDB_pyopenvdb_LIBRARY) + if(NOT TARGET OpenVDB::pyopenvdb) + add_library(OpenVDB::pyopenvdb MODULE IMPORTED) + set_target_properties(OpenVDB::pyopenvdb PROPERTIES + IMPORTED_LOCATION "${OpenVDB_pyopenvdb_LIBRARY}" + INTERFACE_INCLUDE_DIRECTORIES "${OpenVDB_pyopenvdb_INCLUDE_DIR};${PYTHON_INCLUDE_DIR}" + INTERFACE_LINK_LIBRARIES "OpenVDB::openvdb;Boost::${BOOST_PYTHON_LIB};${PYTHON_LIBRARIES}" + INTERFACE_COMPILE_FEATURES cxx_std_14 + ) + endif() +endif() + +# OpenVDB::openvdb_houdini + +if(OpenVDB_openvdb_houdini_LIBRARY) + if(NOT TARGET OpenVDB::openvdb_houdini) + add_library(OpenVDB::openvdb_houdini SHARED IMPORTED) + set_target_properties(OpenVDB::openvdb_houdini PROPERTIES + IMPORTED_LOCATION "${OpenVDB_openvdb_houdini_LIBRARY}" + INTERFACE_INCLUDE_DIRECTORIES "${OpenVDB_openvdb_houdini_INCLUDE_DIR}" + INTERFACE_LINK_LIBRARIES "OpenVDB::openvdb;Houdini" + INTERFACE_COMPILE_FEATURES cxx_std_14 + ) + endif() +endif() + +# OpenVDB::openvdb_ax + +if(OpenVDB_openvdb_ax_LIBRARY) + set(OPENVDB_openvdb_ax_LIB_TYPE UNKNOWN) + if(OPENVDB_USE_STATIC_LIBS) + set(OPENVDB_openvdb_ax_LIB_TYPE STATIC) + elseif(UNIX) + get_filename_component(_OPENVDB_openvdb_ax_EXT + ${OpenVDB_openvdb_ax_LIBRARY} EXT) + if(_OPENVDB_openvdb_ax_EXT STREQUAL ".a") + set(OPENVDB_openvdb_ax_LIB_TYPE STATIC) + elseif(_OPENVDB_openvdb_ax_EXT STREQUAL ".so" OR + _OPENVDB_openvdb_ax_EXT STREQUAL ".dylib") + set(OPENVDB_openvdb_ax_LIB_TYPE SHARED) + endif() + endif() + + + if(NOT TARGET OpenVDB::openvdb_ax) + add_library(OpenVDB::openvdb_ax UNKNOWN IMPORTED) + set_target_properties(OpenVDB::openvdb_ax PROPERTIES + IMPORTED_LOCATION "${OpenVDB_openvdb_ax_LIBRARY}" + INTERFACE_INCLUDE_DIRECTORIES "${OpenVDB_openvdb_ax_INCLUDE_DIR}" + INTERFACE_SYSTEM_INCLUDE_DIRECTORIES "${LLVM_INCLUDE_DIRS}" + INTERFACE_LINK_LIBRARIES "OpenVDB::openvdb;${LLVM_LIBS}" + INTERFACE_COMPILE_FEATURES cxx_std_14 + ) + endif() +endif() + +# OpenVDB::nanovdb + +if(OpenVDB_nanovdb_LIBRARY) + if(NOT TARGET OpenVDB::nanovdb) + add_library(OpenVDB::nanovdb INTERFACE IMPORTED) + set_target_properties(OpenVDB::nanovdb PROPERTIES + IMPORTED_LOCATION "${OpenVDB_nanovdb_LIBRARY}" + INTERFACE_INCLUDE_DIRECTORIES "${OpenVDB_nanovdb_INCLUDE_DIR}" + INTERFACE_LINK_LIBRARIES "OpenVDB::openvdb;" + INTERFACE_COMPILE_FEATURES cxx_std_14 + ) + endif() +endif() + +unset(_OPENVDB_VISIBLE_DEPENDENCIES) +unset(_OPENVDB_HIDDEN_DEPENDENCIES) diff --git a/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/CMakeModules/FindZSTD.cmake b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/CMakeModules/FindZSTD.cmake new file mode 100644 index 00000000..42c5dd92 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/CMakeModules/FindZSTD.cmake @@ -0,0 +1,41 @@ +# Copyright (c) Meta Platforms, Inc. and affiliates. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# +# - Try to find Facebook zstd library +# This will define +# ZSTD_FOUND +# ZSTD_INCLUDE_DIR +# ZSTD_LIBRARY +# + +find_path(ZSTD_INCLUDE_DIR NAMES zstd.h) + +find_library(ZSTD_LIBRARY_DEBUG NAMES zstdd zstd_staticd) +find_library(ZSTD_LIBRARY_RELEASE NAMES zstd zstd_static) + +include(SelectLibraryConfigurations) +SELECT_LIBRARY_CONFIGURATIONS(ZSTD) + +include(FindPackageHandleStandardArgs) +FIND_PACKAGE_HANDLE_STANDARD_ARGS( + ZSTD DEFAULT_MSG + ZSTD_LIBRARY ZSTD_INCLUDE_DIR +) + +if (ZSTD_FOUND) + message(STATUS "Found Zstd: ${ZSTD_LIBRARY}") +endif() + +mark_as_advanced(ZSTD_INCLUDE_DIR ZSTD_LIBRARY) diff --git a/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/CMakeModules/OpenVDBUtils.cmake b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/CMakeModules/OpenVDBUtils.cmake new file mode 100644 index 00000000..084f27d7 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/CMakeModules/OpenVDBUtils.cmake @@ -0,0 +1,164 @@ +# Copyright Contributors to the OpenVDB Project +# SPDX-License-Identifier: MPL-2.0 +# +#[=======================================================================[.rst: + +OpenVDBUtils.cmake +------------------ + +A utility CMake file which provides helper functions for configuring an +OpenVDB installation. + +Use this module by invoking include with the form:: + + include ( OpenVDBUtils ) + + +The following functions are provided: + +``OPENVDB_GET_VERSION_DEFINE`` + + OPENVDB_GET_VERSION_DEFINE ( ) + + Parse the provided version file to retrieve the a given OpenVDB define + specified by and store the result in . If the define has a + value, the value is stored. If the define has no value but exists, ON is + stored. Else, OFF is stored. + + If the file does not exist, is unmodified. + +``OPENVDB_VERSION_FROM_HEADER`` + + OPENVDB_VERSION_FROM_HEADER ( + VERSION [] + MAJOR [] + MINOR [] + PATCH [] ) + + Parse the provided version file to retrieve the current OpenVDB + version information. The file is expected to be a version.h file + as found in the following path of an OpenVDB repository: + openvdb/version.h + + If the file does not exist, variables are unmodified. + +``OPENVDB_ABI_VERSION_FROM_PRINT`` + + OPENVDB_ABI_VERSION_FROM_PRINT ( + [QUIET] + ABI [] ) + + Retrieve the ABI version that an installation of OpenVDB was compiled + for using the provided vdb_print binary. Parses the result of: + vdb_print --version + + If the binary does not exist or fails to launch, variables are + unmodified. + +#]=======================================================================] + +cmake_minimum_required(VERSION 3.15) + + +function(OPENVDB_GET_VERSION_DEFINE HEADER KEY VALUE) + if(NOT EXISTS ${HEADER}) + return() + endif() + + file(STRINGS "${HEADER}" details REGEX "^#define[\t ]+${KEY}[\t ]+.*") + if(details) + # define has a value, extract it and return + string(REGEX REPLACE "^.*${KEY}[\t ]+([0-9]*).*$" "\\1" details "${details}") + set(${VALUE} ${details} PARENT_SCOPE) + return() + endif() + + # try re-searching for single defines + file(STRINGS "${HEADER}" details REGEX "^#define[\t ]+${KEY}.*") + if(details) + set(${VALUE} ON PARENT_SCOPE) + else() + set(${VALUE} OFF PARENT_SCOPE) + endif() +endfunction() + + +######################################################################## +######################################################################## + + +function(OPENVDB_VERSION_FROM_HEADER OPENVDB_VERSION_FILE) + cmake_parse_arguments(_VDB "" "VERSION;MAJOR;MINOR;PATCH;ABI" "" ${ARGN}) + + if(NOT EXISTS ${OPENVDB_VERSION_FILE}) + return() + endif() + + OPENVDB_GET_VERSION_DEFINE(${OPENVDB_VERSION_FILE} "OPENVDB_LIBRARY_MAJOR_VERSION_NUMBER" _OpenVDB_MAJOR_VERSION) + OPENVDB_GET_VERSION_DEFINE(${OPENVDB_VERSION_FILE} "OPENVDB_LIBRARY_MINOR_VERSION_NUMBER" _OpenVDB_MINOR_VERSION) + OPENVDB_GET_VERSION_DEFINE(${OPENVDB_VERSION_FILE} "OPENVDB_LIBRARY_PATCH_VERSION_NUMBER" _OpenVDB_PATCH_VERSION) + OPENVDB_GET_VERSION_DEFINE(${OPENVDB_VERSION_FILE} "OPENVDB_ABI_VERSION_NUMBER" _OpenVDB_ABI_VERSION) + + if(_VDB_VERSION) + set(${_VDB_VERSION} + ${_OpenVDB_MAJOR_VERSION}.${_OpenVDB_MINOR_VERSION}.${_OpenVDB_PATCH_VERSION} + PARENT_SCOPE + ) + endif() + if(_VDB_MAJOR) + set(${_VDB_MAJOR} ${_OpenVDB_MAJOR_VERSION} PARENT_SCOPE) + endif() + if(_VDB_MINOR) + set(${_VDB_MINOR} ${_OpenVDB_MINOR_VERSION} PARENT_SCOPE) + endif() + if(_VDB_PATCH) + set(${_VDB_PATCH} ${_OpenVDB_PATCH_VERSION} PARENT_SCOPE) + endif() + if(_VDB_ABI) + set(${_VDB_ABI} ${_OpenVDB_ABI_VERSION} PARENT_SCOPE) + endif() +endfunction() + + +######################################################################## +######################################################################## + + +function(OPENVDB_ABI_VERSION_FROM_PRINT OPENVDB_PRINT) + cmake_parse_arguments(_VDB "QUIET" "ABI" "" ${ARGN}) + + if(NOT EXISTS ${OPENVDB_PRINT}) + return() + endif() + + set(_VDB_PRINT_VERSION_STRING "") + set(_VDB_PRINT_RETURN_STATUS "") + + if(${_VDB_QUIET}) + execute_process(COMMAND ${OPENVDB_PRINT} "--version" + RESULT_VARIABLE _VDB_PRINT_RETURN_STATUS + OUTPUT_VARIABLE _VDB_PRINT_VERSION_STRING + ERROR_QUIET + OUTPUT_STRIP_TRAILING_WHITESPACE + ) + else() + execute_process(COMMAND ${OPENVDB_PRINT} "--version" + RESULT_VARIABLE _VDB_PRINT_RETURN_STATUS + OUTPUT_VARIABLE _VDB_PRINT_VERSION_STRING + OUTPUT_STRIP_TRAILING_WHITESPACE + ) + endif() + + if(${_VDB_PRINT_RETURN_STATUS}) + return() + endif() + + set(_OpenVDB_ABI) + string(REGEX REPLACE ".*abi([0-9]*).*" "\\1" _OpenVDB_ABI ${_VDB_PRINT_VERSION_STRING}) + unset(_VDB_PRINT_RETURN_STATUS) + unset(_VDB_PRINT_VERSION_STRING) + + if(_VDB_ABI) + set(${_VDB_ABI} ${_OpenVDB_ABI} PARENT_SCOPE) + endif() +endfunction() diff --git a/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/LICENSE b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/LICENSE new file mode 100644 index 00000000..3532c0a8 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/LICENSE @@ -0,0 +1,178 @@ +Copyright 2021 FZI Forschungszentrum Informatik + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grantsto You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS diff --git a/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/include/vdb_mapping/OccupancyVDBMapping.h b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/include/vdb_mapping/OccupancyVDBMapping.h new file mode 100644 index 00000000..18b0e761 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/include/vdb_mapping/OccupancyVDBMapping.h @@ -0,0 +1,101 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2021 FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Marvin Große Besselmann grosse@fzi.de + * \author Lennart Puck puck@fzi.de + * \date 2021-04-29 + * + */ +//---------------------------------------------------------------------- +#ifndef VDB_MAPPING_OCCUPANCY_VDB_MAPPING_H_INCLUDED +#define VDB_MAPPING_OCCUPANCY_VDB_MAPPING_H_INCLUDED + +#include "vdb_mapping/VDBMapping.h" + +namespace vdb_mapping { + +/*! + * \brief Accumulation of configuration parameters + */ +struct Config : BaseConfig +{ + double prob_hit; + double prob_miss; + double prob_thres_min; + double prob_thres_max; +}; + +class OccupancyVDBMapping : public VDBMapping +{ +public: + OccupancyVDBMapping(const double resolution) + : VDBMapping(resolution) + { + } + + /*! + * \brief Handles changing the mapping config + * + * \param config Configuration structure + */ + inline void setConfig(const Config& config) override; + +protected: + inline bool updateFreeNode(float& voxel_value, bool& active) override; + inline bool updateOccupiedNode(float& voxel_value, bool& active) override; + inline bool setNodeToFree(float& voxel_value, bool& active) override; + inline bool setNodeToOccupied(float& voxel_value, bool& active) override; + + inline void createMapFromPointCloud(const PointCloudT::Ptr& cloud, + const bool set_background, + const bool clear_map) override; + + /*! + * \brief Probability update value for passing an obstacle + */ + float m_logodds_hit; + /*! + * \brief Probability update value for passing free space + */ + float m_logodds_miss; + /*! + * \brief Upper occupancy probability threshold + */ + float m_logodds_thres_min; + /*! + * \brief Lower occupancy probability threshold + */ + float m_logodds_thres_max; + /*! + * \brief Maximum clamping point for logodds + */ + float m_max_logodds; + /*! + * \brief Minimum clamping point for logodds + */ + float m_min_logodds; +}; + + +} // namespace vdb_mapping + +#include "OccupancyVDBMapping.hpp" +#endif /* VDB_MAPPING_OCCUPANCY_VDB_MAPPING_H_INCLUDED */ diff --git a/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/include/vdb_mapping/OccupancyVDBMapping.hpp b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/include/vdb_mapping/OccupancyVDBMapping.hpp new file mode 100644 index 00000000..9b952dcb --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/include/vdb_mapping/OccupancyVDBMapping.hpp @@ -0,0 +1,148 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2021 FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Marvin Große Besselmann grosse@fzi.de + * \author Lennart Puck puck@fzi.de + * \date 2021-04-29 + * + */ +//---------------------------------------------------------------------- + + +#include "vdb_mapping/OccupancyVDBMapping.h" + +namespace vdb_mapping { + +bool OccupancyVDBMapping::updateFreeNode(float& voxel_value, bool& active) +{ + voxel_value += m_logodds_miss; + if (voxel_value < m_logodds_thres_min) + { + active = false; + if (voxel_value < m_min_logodds) + { + voxel_value = m_min_logodds; + } + } + return true; +} + +bool OccupancyVDBMapping::updateOccupiedNode(float& voxel_value, bool& active) +{ + voxel_value += m_logodds_hit; + if (voxel_value > m_logodds_thres_max) + { + active = true; + if (voxel_value > m_max_logodds) + { + voxel_value = m_max_logodds; + } + } + return true; +} + +bool OccupancyVDBMapping::setNodeToFree(float& voxel_value, bool& active) +{ + voxel_value = m_min_logodds; + active = false; + return true; +} + +bool OccupancyVDBMapping::setNodeToOccupied(float& voxel_value, bool& active) +{ + voxel_value = m_max_logodds; + active = true; + return true; +} + +void OccupancyVDBMapping::createMapFromPointCloud(const PointCloudT::Ptr& cloud, + const bool set_background, + const bool clear_map) +{ + if (clear_map) + { + m_vdb_grid->clear(); + } + + typename GridT::Accessor acc = m_vdb_grid->getAccessor(); + + for (auto point : cloud->points) + { + openvdb::Vec3d index_coord = + m_vdb_grid->worldToIndex(openvdb::Vec3d(point.x, point.y, point.z)); + acc.setValueOn(openvdb::Coord::floor(index_coord), m_max_logodds); + } + openvdb::CoordBBox bbox = m_vdb_grid->evalActiveVoxelBoundingBox(); + + if (set_background) + { + for (int x = bbox.min().x(); x <= bbox.max().x(); ++x) + { + for (int y = bbox.min().y(); y <= bbox.max().y(); ++y) + { + for (int z = bbox.min().z(); z <= bbox.max().z(); ++z) + { + openvdb::Coord index_coord = openvdb::Coord(x, y, z); + if (!acc.isValueOn(index_coord)) + { + acc.setValueOff(index_coord, m_min_logodds); + } + } + } + } + } + + m_vdb_grid->pruneGrid(); +} + +void OccupancyVDBMapping::setConfig(const Config& config) +{ + // call base class function + VDBMapping::setConfig(config); + + // Sanity Check for input config + if (config.prob_miss > 0.5) + { + std::cerr << "Probability for a miss should be below 0.5 but is " << config.prob_miss + << std::endl; + return; + } + if (config.prob_hit < 0.5) + { + std::cerr << "Probability for a hit should be above 0.5 but is " << config.prob_hit + << std::endl; + return; + } + + // Store probabilities as log odds + m_logodds_miss = static_cast(log(config.prob_miss) - log(1 - config.prob_miss)); + m_logodds_hit = static_cast(log(config.prob_hit) - log(1 - config.prob_hit)); + m_logodds_thres_min = + static_cast(log(config.prob_thres_min) - log(1 - config.prob_thres_min)); + m_logodds_thres_max = + static_cast(log(config.prob_thres_max) - log(1 - config.prob_thres_max)); + // Values to clamp the logodds in order to prevent non dynamic map behavior + m_max_logodds = static_cast(log(0.99) - log(0.01)); + m_min_logodds = static_cast(log(0.01) - log(0.99)); + m_config_set = true; +} + +} // namespace vdb_mapping diff --git a/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/include/vdb_mapping/VDBMapping.h b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/include/vdb_mapping/VDBMapping.h new file mode 100644 index 00000000..bd6dafad --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/include/vdb_mapping/VDBMapping.h @@ -0,0 +1,437 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2021 FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Marvin Große Besselmann grosse@fzi.de + * \author Lennart Puck puck@fzi.de + * \date 2020-12-23 + * + */ +//---------------------------------------------------------------------- +#ifndef VDB_MAPPING_VDB_MAPPING_H_INCLUDED +#define VDB_MAPPING_VDB_MAPPING_H_INCLUDED + + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace vdb_mapping { + + +/*! + * \brief Accumulation of configuration parameters + */ +struct BaseConfig +{ + double max_range; + std::string map_directory_path; +}; +/*! + * \brief Main Mapping class which handles all data integration + */ +template +class VDBMapping +{ +public: + using PointT = pcl::PointXYZ; + using PointCloudT = pcl::PointCloud; + + using RayT = openvdb::math::Ray; + using Vec3T = RayT::Vec3Type; + using DDAT = openvdb::math::DDA; + + using GridT = openvdb::Grid::Type>; + using UpdateGridT = openvdb::Grid::Type>; + + + VDBMapping() = delete; + VDBMapping(const VDBMapping&) = delete; + + VDBMapping& operator=(const VDBMapping&) = delete; + + /*! + * \brief Construktur creates a new VDBMapping objekt with parametrizable grid resolution + * + * \param resolution Resolution of the VDB Grid + */ + VDBMapping(const double resolution); + virtual ~VDBMapping() = default; + + /*! + * \brief Creates a new VDB Grid + * + * \param resolution Resolution of the grid + * + * \returns Grid shared pointer + */ + typename GridT::Ptr createVDBMap(double resolution); + + /*! + * \brief Reset the current map + */ + void resetMap(); + + /*! + * \brief Saves the current map + */ + bool saveMap() const; + + /*! + * \brief Saves the active values of the current map as PCD file + * + * \returns Saving pcd successfull + */ + bool saveMapToPCD(); + + /*! + * \brief Loads a stored map + */ + bool loadMap(const std::string& file_path); + + /*! + * \brief Loads a stored map from a pcd file + * + * \param file_path Path to pcd file + * \param set_background Specifies if the background should be set + * \param clear_map Specifies if the map has to be cleared before inserting data + * + * \returns Loading of map successfull + */ + bool + loadMapFromPCD(const std::string& file_path, const bool set_background, const bool clear_map); + + /*! + * \brief Accumulates a new sensor point cloud to the update grid + * + * \param cloud Input cloud in map coordinates + * \param origin Sensor position in map coordinates + * \param max_range Maximum raycasting range of this measurement + */ + void accumulateUpdate(const PointCloudT::ConstPtr& cloud, + const Eigen::Matrix& origin, + const double& max_range); + + /*! + * \brief Integrates the accumulated updates into the map + * + * \param update_grid Update grid + * \param overwrite_grid Overwrite grid + */ + void integrateUpdate(UpdateGridT::Ptr& update_grid, UpdateGridT::Ptr& overwrite_grid); + + /*! + * \brief Resets the updates grid + */ + void resetUpdate(); + + /*! + * \brief Handles the integration of new PointCloud data into the VDB data structure. + * All datapoints are raycasted starting from the origin position + * + * \param cloud Input cloud in map coordinates + * \param origin Sensor position in map coordinates + * + * \returns Was the insertion of the new pointcloud successful + */ + bool insertPointCloud(const PointCloudT::ConstPtr& cloud, + const Eigen::Matrix& origin); + + /*! + * \brief Handles the integration of new PointCloud data into the VDB data structure. + * All datapoints are raycasted starting from the origin position + * + * \param cloud Input cloud in map coordinates + * \param origin Sensor position in map coordinates + * \param update_grid Update grid that was created internally while mapping + * \param overwrite_grid Overwrite grid containing all changed voxel indices + * + * \returns Was the insertion of the new pointcloud successful + */ + bool insertPointCloud(const PointCloudT::ConstPtr& cloud, + const Eigen::Matrix& origin, + UpdateGridT::Ptr& update_grid, + UpdateGridT::Ptr& overwrite_grid); + + bool removePointsFromGrid(const PointCloudT::ConstPtr& cloud); + bool addPointsToGrid(const PointCloudT::ConstPtr& cloud); + + /*! + * \brief Raycasts a Pointcloud into an update Grid + * + * For each point in the input pointcloud, a raycast is performed from the origin. + * All cells along these rays are marked as active. + * + * All points are clipped according to the config's max_range parameter. If a point is within + * this range, its corresponding cell value in the update grid is set to true and will be handled + * as a sensor hit in the map update. + * + * \param cloud Input sensor point cloud + * \param origin Origin of the sensor measurement + * \param update_grid_acc Accessor to the grid in which the raycasting takes place + * + * \returns Raycasted update grid + */ + bool raycastPointCloud(const PointCloudT::ConstPtr& cloud, + const Eigen::Matrix& origin, + UpdateGridT::Accessor& update_grid_acc); + + /*! + * \brief Raycasts a Pointcloud into an update Grid + * For each point in the input pointcloud, a raycast is performed from the origin. + * All cells along these rays are marked as active. + * + * All points are clipped according to the config's max_range parameter. If a point is within + * this range, its corresponding cell value in the update grid is set to true and will be handled + * as a sensor hit in the map update. + * + * \param cloud Input sensor point cloud + * \param origin Origin of the sensor measurement + * \param raycast_range Maximum raycasting range + * \param update_grid_acc Accessor to the grid in which the raycasting takes place + * + * \returns Raycasted update grid + */ + bool raycastPointCloud(const PointCloudT::ConstPtr& cloud, + const Eigen::Matrix& origin, + const double raycast_range, + UpdateGridT::Accessor& update_grid_acc); + + /*! + * \brief Casts a single ray into an update grid structure + * + * Each cell along the ray is marked as active. + * + * \param ray_origin_world Ray origin in world coordinates + * \param ray_origin_index Ray origin in index coordinates + * \param ray_end_world Ray endpoint in world coordinates + * \param update_grid_acc Accessor to the update grid + * + * \returns Final visited index coordinate + */ + openvdb::Coord castRayIntoGrid(const openvdb::Vec3d& ray_origin_world, + const Vec3T& ray_origin_index, + const openvdb::Vec3d& ray_end_world, + UpdateGridT::Accessor& update_grid_acc) const; + + bool raytrace(const openvdb::Vec3d& ray_origin_world, + const openvdb::Vec3d& ray_direction, + const double max_ray_length, + openvdb::Vec3d& end_point); + + /*! + * \brief Overwrites the active states of a map given an update grid + * + * \param update_grid Update Grid containing all states that changed during the last update + */ + void overwriteMap(const UpdateGridT::Ptr& update_grid); + + /*! + * \brief Incorporates the information of an update grid to the internal map. This will update the + * probabilities of all cells specified by the update grid. + * + * \param temp_grid Grid containing all cells which shall be updated + * + * \returns Was the insertion of the pointcloud successuff + */ + UpdateGridT::Ptr updateMap(const UpdateGridT::Ptr& temp_grid); + + /*! + * \brief Returns a pointer to the VDB map structure + * + * \returns Map pointer + */ + typename GridT::Ptr getGrid() const { return m_vdb_grid; } + + /*! + * \brief Creates a world coordinate bounding box around a transform + * + * \param min_boundary Minimum boundary of box + * \param max_boundary Maximum boundary of box + * \param map_to_reference_tf Transform from map to reference frame + * + * \returns World coordinate bounding box + */ + openvdb::BBoxd + createWorldBoundingBox(const Eigen::Matrix& min_boundary, + const Eigen::Matrix& max_boundary, + const Eigen::Matrix& map_to_reference_tf) const; + /*! + * \brief Creates an index coordinate bounding box around a transform + * + * \param min_boundary Minimum boundary of box + * \param max_boundary Maximum boundary of box + * \param map_to_reference_tf Transform from map to reference frame + * + * \returns Index coordinate bounding box + */ + openvdb::CoordBBox + createIndexBoundingBox(const Eigen::Matrix& min_boundary, + const Eigen::Matrix& max_boundary, + const Eigen::Matrix& map_to_reference_tf) const; + + /*! + * \brief Generates an update grid from a bouding box and a reference frame + * + * \param min_boundary Minimum boundary of the box + * \param max_boundary Maximum boundary of the box + * \param map_to_reference_tf Transform from map to reference frame + * + * \returns Update Grid containing the information within the bounding box + */ + typename UpdateGridT::Ptr + getMapSectionUpdateGrid(const Eigen::Matrix& min_boundary, + const Eigen::Matrix& max_boundary, + const Eigen::Matrix& map_to_reference_tf) const; + /*! + * \brief Generates a grid from a bounding box and a reference frame + * + * \param min_boundary Minimum boundary of the box + * \param max_boundary Maximum boundary of the box + * \param map_to_reference_tf Transform from map to reference frame + * + * \returns Grid containing the information within the bounding box + */ + typename GridT::Ptr + getMapSectionGrid(const Eigen::Matrix& min_boundary, + const Eigen::Matrix& max_boundary, + const Eigen::Matrix& map_to_reference_tf) const; + /*! + * \brief Generates a grid or update grid from a bounding box and a reference frame + * + * @tparam TResultGrid Resulting Grid Type + * \param min_boundary Minimum boundary of the box + * \param max_boundary Maximum boundary of the box + * \param map_to_reference_tf Transform from map to reference frame + * + * \returns Grid/UpdateGrid containing the information within the bounding box + */ + template + typename TResultGrid::Ptr + getMapSection(const Eigen::Matrix& min_boundary, + const Eigen::Matrix& max_boundary, + const Eigen::Matrix& map_to_reference_tf) const; + + /*! + * \brief Applies a map section grid to the map + * + * \param section Section grid containing the information about part of the map. The boundary box + * of the section is encoded in the grids meta information + * + */ + void applyMapSectionGrid(const typename VDBMapping::GridT::Ptr section); + + /*! + * \brief Applies a map section update grid to the map + * + * \param section Section grid containing the information about part of the map. The boundary box + * of the section is encoded in the grids meta information + * + */ + void + applyMapSectionUpdateGrid(const typename VDBMapping::UpdateGridT::Ptr section); + + /*! + * \brief Applies a map section to the map + * + * \param section Section grid containing the information about part of the map. The boundary box + * of the section is encoded in the grids meta information + * + */ + template + void applyMapSection(typename TSectionGrid::Ptr section); + + /*! + * \brief Handles changing the mapping config + * + * \param config Configuration structure + */ + virtual void setConfig(const TConfig& config); + + std::vector compressString(const std::string& string) const; + + std::string decompressByteArray(const std::vector& byte_array) const; + + template + std::vector gridToByteArray(typename TGrid::Ptr grid); + + template + typename TGrid::Ptr byteArrayToGrid(std::vector byte_array); + + +protected: + virtual bool updateFreeNode(TData& voxel_value, bool& active) { return false; } + virtual bool updateOccupiedNode(TData& voxel_value, bool& active) { return false; } + virtual bool setNodeToFree(TData& voxel_value, bool& active) { return false; } + virtual bool setNodeToOccupied(TData& voxel_value, bool& active) { return false; } + + virtual void createMapFromPointCloud(const PointCloudT::Ptr& cloud, + const bool set_background, + const bool clear_map) + { + std::cerr << "Not implemented for data type" << std::endl; + } + + /*! + * \brief VDB grid pointer + */ + typename GridT::Ptr m_vdb_grid; + /*! + * \brief Maximum raycasting distance + */ + double m_max_range; + /*! + * \brief Grid resolution of the map + */ + double m_resolution; + /*! + * \brief path where the maps will be stored + */ + std::string m_map_directory_path; + /*! + * \brief Flag checking wether a valid config was already loaded + */ + bool m_config_set; + + UpdateGridT::Ptr m_update_grid; + + unsigned int m_compression_level = 1; +}; + +#include "VDBMapping.hpp" + +} // namespace vdb_mapping + +#endif /* VDB_MAPPING_VDB_MAPPING_H_INCLUDED */ diff --git a/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/include/vdb_mapping/VDBMapping.hpp b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/include/vdb_mapping/VDBMapping.hpp new file mode 100644 index 00000000..7775d391 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/include/vdb_mapping/VDBMapping.hpp @@ -0,0 +1,696 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2021 FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Marvin Große Besselmann grosse@fzi.de + * \author Lennart Puck puck@fzi.de + * \date 2020-12-23 + * + */ +//---------------------------------------------------------------------- + + +#include + +template +VDBMapping::VDBMapping(const double resolution) + : m_resolution(resolution) + , m_config_set(false) +{ + // Initialize Grid + openvdb::initialize(); + if (!GridT::isRegistered()) + { + GridT::registerGrid(); + } + if (!UpdateGridT::isRegistered()) + { + UpdateGridT::registerGrid(); + } + m_vdb_grid = createVDBMap(m_resolution); + m_update_grid = UpdateGridT::create(false); +} + +template +void VDBMapping::resetMap() +{ + m_vdb_grid->clear(); + m_vdb_grid = createVDBMap(m_resolution); + m_update_grid = UpdateGridT::create(false); +} + + +template +bool VDBMapping::saveMap() const +{ + auto timestamp = std::chrono::system_clock::now(); + std::time_t now_tt = std::chrono::system_clock::to_time_t(timestamp); + std::tm tm = *std::localtime(&now_tt); + std::stringstream sstime; + sstime << std::put_time(&tm, "%Y-%m-%d_%H-%M-%S"); + + std::string map_name = m_map_directory_path + sstime.str() + "_map.vdb"; + std::cout << map_name << std::endl; + openvdb::io::File file_handle(map_name); + openvdb::GridPtrVec grids; + grids.push_back(m_vdb_grid); + file_handle.write(grids); + file_handle.close(); + return true; +} + +template +bool VDBMapping::saveMapToPCD() +{ + auto timestamp = std::chrono::system_clock::now(); + std::time_t now_tt = std::chrono::system_clock::to_time_t(timestamp); + std::tm tm = *std::localtime(&now_tt); + std::stringstream sstime; + sstime << std::put_time(&tm, "%Y-%m-%d_%H-%M-%S"); + std::string pcd_path = m_map_directory_path + sstime.str() + "_active_values_map.pcd"; + + PointCloudT::Ptr cloud(new PointCloudT); + + cloud->points.reserve(m_vdb_grid->activeVoxelCount()); + + for (typename GridT::ValueOnCIter iter = m_vdb_grid->cbeginValueOn(); iter; ++iter) + { + openvdb::Vec3d world_coord = m_vdb_grid->indexToWorld(iter.getCoord()); + + world_coord += 0.5 * m_resolution; + PointT point; + point.x = static_cast(world_coord.x()); + point.y = static_cast(world_coord.y()); + point.z = static_cast(world_coord.z()); + cloud->points.push_back(point); + } + + cloud->points.shrink_to_fit(); + cloud->width = cloud->points.size(); + cloud->height = 1; + + if (pcl::io::savePCDFile(pcd_path, *cloud) != 0) + { + PCL_ERROR("Could not write PCD file."); + return false; + } + std::cout << "Wrote pcd to: " << pcd_path << std::endl; + return true; +} + +template +bool VDBMapping::loadMap(const std::string& file_path) +{ + openvdb::io::File file_handle(file_path); + file_handle.open(); + openvdb::GridBase::Ptr base_grid; + for (openvdb::io::File::NameIterator name_iter = file_handle.beginName(); + name_iter != file_handle.endName(); + ++name_iter) + { + base_grid = file_handle.readGrid(name_iter.gridName()); + m_vdb_grid->clear(); + m_vdb_grid = openvdb::gridPtrCast(base_grid); + } + file_handle.close(); + + + return true; +} + +template +bool VDBMapping::loadMapFromPCD(const std::string& file_path, + const bool set_background, + const bool clear_map) +{ + PointCloudT::Ptr cloud(new PointCloudT); + if (pcl::io::loadPCDFile(file_path, *cloud) == -1) + { + PCL_ERROR("Could not open PCD file"); + return false; + } + createMapFromPointCloud(cloud, set_background, clear_map); + return true; +} + +template +typename VDBMapping::GridT::Ptr +VDBMapping::createVDBMap(double resolution) +{ + typename GridT::Ptr new_map = GridT::create(TData()); + new_map->setTransform(openvdb::math::Transform::createLinearTransform(m_resolution)); + new_map->setGridClass(openvdb::GRID_LEVEL_SET); + return new_map; +} + +template +openvdb::BBoxd VDBMapping::createWorldBoundingBox( + const Eigen::Matrix& min_boundary, + const Eigen::Matrix& max_boundary, + const Eigen::Matrix& map_to_reference_tf) const +{ + pcl::PointCloud::Ptr corners(new pcl::PointCloud()); + corners->points.emplace_back(static_cast(min_boundary.x()), + static_cast(min_boundary.y()), + static_cast(min_boundary.z())); + corners->points.emplace_back(static_cast(min_boundary.x()), + static_cast(min_boundary.y()), + static_cast(max_boundary.z())); + corners->points.emplace_back(static_cast(min_boundary.x()), + static_cast(max_boundary.y()), + static_cast(min_boundary.z())); + corners->points.emplace_back(static_cast(min_boundary.x()), + static_cast(max_boundary.y()), + static_cast(max_boundary.z())); + corners->points.emplace_back(static_cast(max_boundary.x()), + static_cast(min_boundary.y()), + static_cast(min_boundary.z())); + corners->points.emplace_back(static_cast(max_boundary.x()), + static_cast(min_boundary.y()), + static_cast(max_boundary.z())); + corners->points.emplace_back(static_cast(max_boundary.x()), + static_cast(max_boundary.y()), + static_cast(min_boundary.z())); + corners->points.emplace_back(static_cast(max_boundary.x()), + static_cast(max_boundary.y()), + static_cast(max_boundary.z())); + pcl::transformPointCloud(*corners, *corners, map_to_reference_tf); + pcl::PointXYZ min_pt; + pcl::PointXYZ max_pt; + pcl::getMinMax3D(*corners, min_pt, max_pt); + + return openvdb::BBoxd(openvdb::Vec3d(min_pt.x, min_pt.y, min_pt.z), + openvdb::Vec3d(max_pt.x, max_pt.y, max_pt.z)); +} + +template +openvdb::CoordBBox VDBMapping::createIndexBoundingBox( + const Eigen::Matrix& min_boundary, + const Eigen::Matrix& max_boundary, + const Eigen::Matrix& map_to_reference_tf) const +{ + openvdb::BBoxd world_bb = createWorldBoundingBox(min_boundary, max_boundary, map_to_reference_tf); + + + openvdb::Vec3d min_index = m_vdb_grid->worldToIndex(world_bb.min()); + openvdb::Vec3d max_index = m_vdb_grid->worldToIndex(world_bb.max()); + + return {openvdb::Coord::floor(min_index), openvdb::Coord::floor(max_index)}; +} + +template +typename VDBMapping::UpdateGridT::Ptr +VDBMapping::getMapSectionUpdateGrid( + const Eigen::Matrix& min_boundary, + const Eigen::Matrix& max_boundary, + const Eigen::Matrix& map_to_reference_tf) const +{ + return getMapSection::UpdateGridT>( + min_boundary, max_boundary, map_to_reference_tf); +} + +template +typename VDBMapping::GridT::Ptr VDBMapping::getMapSectionGrid( + const Eigen::Matrix& min_boundary, + const Eigen::Matrix& max_boundary, + const Eigen::Matrix& map_to_reference_tf) const +{ + return getMapSection::GridT>( + min_boundary, max_boundary, map_to_reference_tf); +} + +template +template +typename TResultGrid::Ptr VDBMapping::getMapSection( + const Eigen::Matrix& min_boundary, + const Eigen::Matrix& max_boundary, + const Eigen::Matrix& map_to_reference_tf) const +{ + typename TResultGrid::Ptr temp_grid = TResultGrid::create(false); + temp_grid->setTransform(openvdb::math::Transform::createLinearTransform(m_resolution)); + + typename TResultGrid::Accessor temp_acc = temp_grid->getAccessor(); + + openvdb::CoordBBox bounding_box( + createIndexBoundingBox(min_boundary, max_boundary, map_to_reference_tf)); + + for (auto iter = m_vdb_grid->cbeginValueOn(); iter; ++iter) + { + if (bounding_box.isInside(iter.getCoord())) + { + temp_acc.setValueOn(iter.getCoord(), true); + } + } + + openvdb::Vec3d min(bounding_box.min().x(), bounding_box.min().y(), bounding_box.min().z()); + openvdb::Vec3d max(bounding_box.max().x(), bounding_box.max().y(), bounding_box.max().z()); + temp_grid->insertMeta("bb_min", openvdb::Vec3DMetadata(min)); + temp_grid->insertMeta("bb_max", openvdb::Vec3DMetadata(max)); + return temp_grid; +} + +template +void VDBMapping::applyMapSectionGrid( + const typename VDBMapping::GridT::Ptr section) +{ + applyMapSection::GridT>(section); +} + +template +void VDBMapping::applyMapSectionUpdateGrid( + const typename VDBMapping::UpdateGridT::Ptr section) +{ + applyMapSection::UpdateGridT>(section); +} + +template +template +void VDBMapping::applyMapSection(typename TSectionGrid::Ptr section) +{ + typename TSectionGrid::Accessor section_acc = section->getAccessor(); + typename GridT::Accessor acc = m_vdb_grid->getAccessor(); + + openvdb::Vec3d min = section->template metaValue("bb_min"); + openvdb::Vec3d max = section->template metaValue("bb_max"); + openvdb::CoordBBox bbox(openvdb::Coord::floor(min), openvdb::Coord::floor(max)); + + for (auto iter = m_vdb_grid->cbeginValueOn(); iter; ++iter) + { + if (bbox.isInside(iter.getCoord())) + { + acc.setActiveState(iter.getCoord(), false); + } + } + for (auto iter = section->cbeginValueOn(); iter; ++iter) + { + acc.setActiveState(iter.getCoord(), true); + } +} + + +template +bool VDBMapping::insertPointCloud(const PointCloudT::ConstPtr& cloud, + const Eigen::Matrix& origin) +{ + UpdateGridT::Ptr update_grid; + UpdateGridT::Ptr overwrite_grid; + + return insertPointCloud(cloud, origin, update_grid, overwrite_grid); +} + +template +bool VDBMapping::insertPointCloud(const PointCloudT::ConstPtr& cloud, + const Eigen::Matrix& origin, + UpdateGridT::Ptr& update_grid, + UpdateGridT::Ptr& overwrite_grid) +{ + accumulateUpdate(cloud, origin, m_max_range); + integrateUpdate(update_grid, overwrite_grid); + resetUpdate(); + return true; +} + +template +bool VDBMapping::removePointsFromGrid(const PointCloudT::ConstPtr& cloud) +{ + typename GridT::Accessor acc = m_vdb_grid->getAccessor(); + + auto set_node = [&](TData& voxel_value, bool& active) { setNodeToFree(voxel_value, active); }; + + + for (const PointT& pt : *cloud) + { + openvdb::Vec3d world_pt(pt.x, pt.y, pt.z); + openvdb::Coord index_pt = openvdb::Coord::floor(m_vdb_grid->worldToIndex(world_pt)); + acc.modifyValueAndActiveState(index_pt, set_node); + } + return true; +} + +template +bool VDBMapping::addPointsToGrid(const PointCloudT::ConstPtr& cloud) +{ + typename GridT::Accessor acc = m_vdb_grid->getAccessor(); + auto set_node = [&](TData& voxel_value, bool& active) { setNodeToOccupied(voxel_value, active); }; + for (const PointT& pt : *cloud) + { + openvdb::Vec3d world_pt(pt.x, pt.y, pt.z); + openvdb::Coord index_pt = openvdb::Coord::floor(m_vdb_grid->worldToIndex(world_pt)); + acc.modifyValueAndActiveState(index_pt, set_node); + } + return true; +} + + +template +void VDBMapping::accumulateUpdate(const PointCloudT::ConstPtr& cloud, + const Eigen::Matrix& origin, + const double& max_range) +{ + UpdateGridT::Accessor update_grid_acc = m_update_grid->getAccessor(); + if (max_range > 0) + { + raycastPointCloud(cloud, origin, max_range, update_grid_acc); + } + else + { + raycastPointCloud(cloud, origin, update_grid_acc); + } +} + +template +void VDBMapping::integrateUpdate(UpdateGridT::Ptr& update_grid, + UpdateGridT::Ptr& overwrite_grid) +{ + overwrite_grid = updateMap(m_update_grid); + update_grid = m_update_grid; +} + +template +void VDBMapping::resetUpdate() +{ + m_update_grid = UpdateGridT::create(false); +} + +template +bool VDBMapping::raycastPointCloud(const PointCloudT::ConstPtr& cloud, + const Eigen::Matrix& origin, + UpdateGridT::Accessor& update_grid_acc) +{ + return raycastPointCloud(cloud, origin, m_max_range, update_grid_acc); +} + + +template +bool VDBMapping::raycastPointCloud(const PointCloudT::ConstPtr& cloud, + const Eigen::Matrix& origin, + const double raycast_range, + UpdateGridT::Accessor& update_grid_acc) +{ + // Creating a temporary grid in which the new data is casted. This way we prevent the computation + // of redundant probability updates in the actual map + + // Check if a valid configuration was loaded + if (!m_config_set) + { + std::cerr << "Map not properly configured. Did you call setConfig method?" << std::endl; + return false; + } + + RayT ray; + DDAT dda; + + // Ray origin in world coordinates + openvdb::Vec3d ray_origin_world(origin.x(), origin.y(), origin.z()); + // Ray origin in index coordinates + openvdb::Coord buffer = openvdb::Coord::floor(m_vdb_grid->worldToIndex(ray_origin_world)); + Vec3T ray_origin_index(buffer.x(), buffer.y(), buffer.z()); + // Ray end point in world coordinates + openvdb::Vec3d ray_end_world; + + // Raycasting of every point in the input cloud + for (const PointT& pt : *cloud) + { + ray_end_world = openvdb::Vec3d(pt.x, pt.y, pt.z); + bool max_range_ray = false; + + + if (std::isnan(ray_end_world.x()) || std::isnan(ray_end_world.y()) || + std::isnan(ray_end_world.z()) || std::isnan(ray_origin_world.x()) || + std::isnan(ray_origin_world.y()) || std::isnan(ray_origin_world.z())) + { + continue; + } + + if (raycast_range > 0.0 && (ray_end_world - ray_origin_world).length() > raycast_range) + { + ray_end_world = ray_origin_world + (ray_end_world - ray_origin_world).unit() * raycast_range; + max_range_ray = true; + } + + openvdb::Coord ray_end_index = + castRayIntoGrid(ray_origin_world, ray_origin_index, ray_end_world, update_grid_acc); + + if (!max_range_ray) + { + update_grid_acc.setValueOn(ray_end_index, true); + } + } + return true; +} + +template +openvdb::Coord +VDBMapping::castRayIntoGrid(const openvdb::Vec3d& ray_origin_world, + const Vec3T& ray_origin_index, + const openvdb::Vec3d& ray_end_world, + UpdateGridT::Accessor& update_grid_acc) const +{ + // In case that a coodinate is placed directly on the grid a half the resolution is added as + // offset to push it within the grids boundaries Currently this does not support tranlational + // adjustments of the grid since it is not used within the vdb_mapping framework. + openvdb::Vec3d adjusted_ray_end_world = ray_end_world; + if (std::fmod(ray_end_world.x(), m_resolution)) + { + adjusted_ray_end_world.x() = ray_end_world.x() + (m_resolution / 2.0); + } + if (std::fmod(ray_end_world.y(), m_resolution)) + { + adjusted_ray_end_world.y() = ray_end_world.y() + (m_resolution / 2.0); + } + if (std::fmod(ray_end_world.z(), m_resolution)) + { + adjusted_ray_end_world.z() = ray_end_world.z() + (m_resolution / 2.0); + } + + openvdb::Coord ray_end_index = + openvdb::Coord::floor(m_vdb_grid->worldToIndex(adjusted_ray_end_world)); + + openvdb::Vec3d ray_direction = (ray_end_index.asVec3d() - ray_origin_index); + + // Starting the ray from the center of the voxel + RayT ray(ray_origin_index + 0.5, ray_direction, 0, 1); + DDAT dda(ray, 0); + if (ray_end_index.asVec3d() != ray_origin_index) + { + do + { + update_grid_acc.setActiveState(dda.voxel(), true); + } while (dda.step()); + } + return ray_end_index; +} + +template +bool VDBMapping::raytrace(const openvdb::Vec3d& ray_origin_world, + const openvdb::Vec3d& ray_direction, + const double max_ray_length, + openvdb::Vec3d& end_point) +{ + typename GridT::Accessor acc = m_vdb_grid->getAccessor(); + openvdb::Vec3d ray_origin_index = m_vdb_grid->worldToIndex(ray_origin_world); + RayT ray(ray_origin_index, ray_direction); + DDAT dda(ray); + while (true) + { + double distance = + m_resolution * std::sqrt(std::pow((dda.voxel().x() - ray_origin_index.x()), 2) + + std::pow((dda.voxel().y() - ray_origin_index.y()), 2) + + std::pow((dda.voxel().z() - ray_origin_index.z()), 2)); + if (distance < max_ray_length) + { + if (acc.isValueOn(dda.voxel())) + { + end_point = m_vdb_grid->indexToWorld(dda.voxel()); + return true; + } + dda.step(); + } + else + { + return false; + } + } +} + +template +VDBMapping::UpdateGridT::Ptr +VDBMapping::updateMap(const UpdateGridT::Ptr& temp_grid) +{ + UpdateGridT::Ptr change = UpdateGridT::create(false); + UpdateGridT::Accessor change_acc = change->getAccessor(); + if (temp_grid->empty()) + { + return change; + } + + bool state_changed = false; + typename GridT::Accessor acc = m_vdb_grid->getAccessor(); + // Probability update lambda for free space grid elements + auto miss = [&](TData& voxel_value, bool& active) { + bool last_state = active; + updateFreeNode(voxel_value, active); + if (last_state != active) + { + state_changed = true; + } + }; + + // Probability update lambda for occupied grid elements + auto hit = [&](TData& voxel_value, bool& active) { + bool last_state = active; + updateOccupiedNode(voxel_value, active); + if (last_state != active) + { + state_changed = true; + } + }; + + // Integrating the data of the temporary grid into the map using the probability update functions + for (UpdateGridT::ValueOnCIter iter = temp_grid->cbeginValueOn(); iter; ++iter) + { + state_changed = false; + if (*iter) + { + acc.modifyValueAndActiveState(iter.getCoord(), hit); + if (state_changed) + { + change_acc.setValueOn(iter.getCoord(), true); + } + } + else + { + acc.modifyValueAndActiveState(iter.getCoord(), miss); + if (state_changed) + { + change_acc.setActiveState(iter.getCoord(), true); + } + } + } + return change; +} + +template +void VDBMapping::overwriteMap(const UpdateGridT::Ptr& update_grid) +{ + typename GridT::Accessor acc = m_vdb_grid->getAccessor(); + for (UpdateGridT::ValueOnCIter iter = update_grid->cbeginValueOn(); iter; ++iter) + { + if (*iter) + { + acc.setActiveState(iter.getCoord(), true); + } + else + { + acc.setActiveState(iter.getCoord(), false); + } + } +} + +template +std::vector VDBMapping::compressString(const std::string& string) const +{ + auto uncompressed = std::vector(string.begin(), string.end()); + + // Create buffer with enough size for worst case scenario + size_t len = ZSTD_compressBound(uncompressed.size()); + std::vector compressed(len); + + int ret = ZSTD_compress( + compressed.data(), len, uncompressed.data(), uncompressed.size(), m_compression_level); + + + if (ZSTD_isError(ret)) + { + std::cerr << "Compression using ZSTD failed: " << ZSTD_getErrorName(ret) + << " , sending uncompressed byte array" << std::endl; + return uncompressed; + } + + // Resize compressed buffer to actual compressed size + compressed.resize(ret); + return compressed; +} + +template +std::string +VDBMapping::decompressByteArray(const std::vector& byte_array) const +{ + std::size_t len = ZSTD_getDecompressedSize(byte_array.data(), byte_array.size()); + std::vector uncompressed(len); + + std::size_t size = + ZSTD_decompress(uncompressed.data(), len, byte_array.data(), byte_array.size()); + + + std::string map_str; + if (ZSTD_isError(size)) + { + std::cerr << "Could not decompress map using ZSTD failed: " << ZSTD_getErrorName(size) + << " , returning raw data" << std::endl; + map_str = std::string(byte_array.begin(), byte_array.end()); + } + else + { + map_str = std::string(uncompressed.begin(), uncompressed.end()); + } + + return map_str; +} + +template +template +std::vector VDBMapping::gridToByteArray(const typename TGrid::Ptr grid) +{ + openvdb::GridPtrVec grids; + grids.push_back(grid); + std::ostringstream oss(std::ios_base::binary); + openvdb::io::Stream(oss).write(grids); + return compressString(oss.str()); +} + +template +template +typename TGrid::Ptr +VDBMapping::byteArrayToGrid(const std::vector byte_array) +{ + std::istringstream iss(decompressByteArray(byte_array)); + openvdb::io::Stream strm(iss); + openvdb::GridPtrVecPtr grids; + grids = strm.getGrids(); + // This cast might fail if different VDB versions are used. + // Corresponding error messages are generated by VDB directly + typename TGrid::Ptr update_grid = openvdb::gridPtrCast(grids->front()); + return update_grid; +} + +template +void VDBMapping::setConfig(const TConfig& config) +{ + if (config.max_range < 0.0) + { + std::cerr << "Max range of " << config.max_range << " invalid. Range cannot be negative." + << std::endl; + return; + } + m_max_range = config.max_range; + m_map_directory_path = config.map_directory_path; + m_config_set = true; +} diff --git a/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/index.html b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/index.html new file mode 100644 index 00000000..59e1cda2 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/index.html @@ -0,0 +1,2709 @@ + + + + + + + + + + + + + + + + + + + VDB Mapping Core Library - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

VDB Mapping Core Library

+

DISCLAIMER: This library is still under development. Be warned that some interfaces will be changed and/or extended in the future.

+

The VDB Mapping core library was primarily developed to be used in combination with the corresponding ROS wrapper or ROS2 wrapper

+

Getting Started

+

Requirements

+

This library requires OpenVDB as it is build around it. +This library was initially developed using Version 5.0 and should work with all versions above.

+

As the apt packages are quite outdated for most systems, we recommend building at least OpenVDB v9.0.0 from source using the provided build instructions

+

Build instructions

+

The library can be either used as plain c++ library or in combination with the afore mentioned ROS wrapper.

+

Dependencies

+

The library requires the following dependencies to build correctly +

apt-get install -y libeigen3-dev
+apt-get install -y libtbb-dev
+apt-get install -y libpcl-dev
+apt-get install -y libilmbase-dev
+

+

Plain cmake

+

To build this package as a standalone library, follow the usual cmake building steps: +

git clone https://github.com/fzi-forschungszentrum-informatik/vdb_mapping
+cd vdb_mapping
+mkdir build && cd build
+cmake ..
+make -j8
+make install
+

+

ROS Workspace

+

In case you want build this library inside of a ROS workspace in combination with VDB Mapping ROS, you cannot use catkin_make since this library is not a catkin package. +Instead you have to use catkin build or catkin_make_isolated to build the workspace.

+
# source global ros
+source /opt/ros/<your_ros_version>/setup.{zsh/bash}
+
+# create a catkin workspace
+mkdir -p ~/catkin_ws/src && cd catkin_ws
+
+# clone packages
+git clone https://github.com/fzi-forschungszentrum-informatik/vdb_mapping
+
+# install dependencies
+sudo apt update
+rosdep update
+rosdep install --from-paths src --ignore-src -y
+
+# build the workspace.
+catkin build
+
+# source the workspace
+source deve/setup.bash
+
+

ROS2 Workspace

+
# source global ros
+source /opt/ros/<your_ros_version>/setup.{zsh/bash}
+
+# create a catkin workspace
+mkdir -p ~/colcon_ws/src && cd ~/colcon_ws/src
+
+# clone packages
+git clone https://github.com/fzi-forschungszentrum-informatik/vdb_mapping
+git clone https://github.com/fzi-forschungszentrum-informatik/vdb_mapping_ros2
+
+# install dependencies
+sudo apt update
+rosdep update
+rosdep install --from-paths src --ignore-src -y
+
+# build the workspace.  
+colcon build
+
+# source the workspace
+source install/setup.bash
+
+

Citation

+

Thanks that you read until here and please let us know if you run into any issues or have suggestions for improvements. +If you use our work in your publications please feel free to cite our manuscript: +

  @inproceedings{besselmann2021vdb,
+  title={VDB-Mapping: a high resolution and real-time capable 3D mapping framework for versatile mobile robots},
+  author={Besselmann, M Grosse and Puck, Lennart and Steffen, Lea and Roennau, Arne and Dillmann, R{\"u}diger},
+  booktitle={2021 IEEE 17th International Conference on Automation Science and Engineering (CASE)},
+  pages={448--454},
+  year={2021},
+  organization={IEEE}
+  doi={10.1109/CASE49439.2021.9551430}}
+}
+
+or for the remote mapping case: +
@incollection{besselmann2022remote,
+  title={Remote VDB-Mapping: A Level-Based Data Reduction Framework for Distributed Mapping},
+  author={Besselmann, Marvin Grosse and R{\"o}nnau, Arne and Dillmann, R{\"u}diger},
+  booktitle={Robotics in Natural Settings: CLAWAR 2022},
+  pages={448--459},
+  year={2022},
+  publisher={Springer}
+  doi={10.1007/978-3-031-15226-9_42}
+}
+

+

Acknowledgement

+

The research leading to this package has received funding from the German Federal Ministry of Education and Research under grant agreement No. 13N14679:

+

+ bmbf +

+

ROBDEKON - Robotic systems for decontamination in hazardous environments
+More information: robdekon.de

+

+ robdekon_logo +

+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ +
+ + + +
+
+
+ +
+ Copyright © 2024 - 2030 AirLab CMU +
+ + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + +
+ +
+
+
+ +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/package.xml b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/package.xml new file mode 100644 index 00000000..be3954c5 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/package.xml @@ -0,0 +1,29 @@ + + + vdb_mapping + 0.1.0 + The vdb_mapping core package used for fast performant volumetric mapping. + + Marvin Große Besselmann + + Apache-2.0 + + + + + + + Marvin Große Besselmann + + cmake + + eigen + libopenvdb-dev + libpcl-all-dev + libpcl-all + tbb + + + cmake + + diff --git a/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/tests/CMakeLists.txt b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/tests/CMakeLists.txt new file mode 100644 index 00000000..ee34a1e8 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/tests/CMakeLists.txt @@ -0,0 +1,30 @@ +cmake_minimum_required(VERSION 3.0.2) +set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/../CMakeModules" ${CMAKE_MODULE_PATH}) + +project(vdb_mapping_tests CXX) + +# This will allow cmake 3.0 and above to fail on missing link targets (when specified with an double colon syntax) +# Note that not all packages are support the colon syntax, but if they do it is recomended to use this syntax, since it is less error prone. +if(POLICY CMP0028) + cmake_policy(SET CMP0028 NEW) +endif() + +## +## Find dependencies for testing +## +include(FindGTestPackage) +find_gtest_package(GTEST_LIBRARIES) +if (NOT TARGET vdb_mapping::vdb_mapping) + find_package(vdb_mapping REQUIRED) +endif() + +########### +## Build ## +########### + +add_executable(mapping_tests mapping.cpp) +target_compile_features(mapping_tests PUBLIC cxx_std_14) +target_include_directories(mapping_tests PRIVATE ${GTEST_INCLUDE_DIRS}) +target_link_libraries(mapping_tests PRIVATE vdb_mapping::vdb_mapping ${GTEST_LIBRARIES}) +add_test(NAME mapping.tests COMMAND mapping_tests) + diff --git a/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/tests/mapping.cpp b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/tests/mapping.cpp new file mode 100644 index 00000000..e63f60a2 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/tests/mapping.cpp @@ -0,0 +1,152 @@ +#include "gtest/gtest.h" +#include + +namespace vdb_mapping { + +TEST(Mapping, SetConfig) +{ + OccupancyVDBMapping map(1); + OccupancyVDBMapping::PointCloudT::Ptr cloud(new OccupancyVDBMapping::PointCloudT); + cloud->points.emplace_back(0, 0, 1); + Eigen::Matrix origin(0, 0, 0); + map.insertPointCloud(cloud, origin); + OccupancyVDBMapping::GridT::Accessor acc = map.getGrid()->getAccessor(); + openvdb::Coord coord(0, 0, 1); + EXPECT_EQ(acc.getValue(coord), 0.0); + Config conf; + conf.max_range = 10; + conf.prob_hit = 0.9; + conf.prob_miss = 0.1; + conf.prob_thres_max = 0.51; + conf.prob_thres_min = 0.49; + map.setConfig(conf); + auto log_hit = static_cast(log(conf.prob_hit) - log(1 - conf.prob_hit)); + auto log_miss = static_cast(log(conf.prob_miss) - log(1 - conf.prob_miss)); + map.insertPointCloud(cloud, origin); + EXPECT_EQ(acc.getValue(openvdb::Coord(0, 0, 0)), log_miss); + EXPECT_EQ(acc.getValue(openvdb::Coord(0, 0, 1)), log_hit); +} + +TEST(Mapping, InsertPositivePoint) +{ + double resolution = 0.1; + OccupancyVDBMapping map(resolution); + Config conf; + conf.max_range = 10; + conf.prob_hit = 0.9; + conf.prob_miss = 0.1; + conf.prob_thres_max = 0.51; + conf.prob_thres_min = 0.49; + map.setConfig(conf); + + auto log_hit = static_cast(log(conf.prob_hit) - log(1 - conf.prob_hit)); + auto log_miss = static_cast(log(conf.prob_miss) - log(1 - conf.prob_miss)); + + OccupancyVDBMapping::PointCloudT::Ptr cloud(new OccupancyVDBMapping::PointCloudT); + cloud->points.emplace_back(0, 0, 5 * resolution); + Eigen::Matrix origin(0, 0, 0); + map.insertPointCloud(cloud, origin); + OccupancyVDBMapping::GridT::Accessor acc = map.getGrid()->getAccessor(); + for (int i = 0; i < 5; ++i) + { + openvdb::Coord coord(0, 0, i); + EXPECT_EQ(acc.getValue(coord), log_miss); + EXPECT_FALSE(acc.isValueOn(coord)); + } + openvdb::Coord coord(0, 0, 5); + EXPECT_EQ(acc.getValue(coord), log_hit); + EXPECT_TRUE(acc.isValueOn(coord)); +} + +TEST(Mapping, InsertNegativePoint) +{ + double resolution = 0.1; + OccupancyVDBMapping map(resolution); + Config conf; + conf.max_range = 10; + conf.prob_hit = 0.9; + conf.prob_miss = 0.1; + conf.prob_thres_max = 0.51; + conf.prob_thres_min = 0.49; + map.setConfig(conf); + + auto log_hit = static_cast(log(conf.prob_hit) - log(1 - conf.prob_hit)); + auto log_miss = static_cast(log(conf.prob_miss) - log(1 - conf.prob_miss)); + + OccupancyVDBMapping::PointCloudT::Ptr cloud(new OccupancyVDBMapping::PointCloudT); + cloud->points.emplace_back(0, 0, -5 * resolution); + Eigen::Matrix origin(0, 0, 0); + map.insertPointCloud(cloud, origin); + OccupancyVDBMapping::GridT::Accessor acc = map.getGrid()->getAccessor(); + for (int i = 1; i < 5; ++i) + { + openvdb::Coord coord(0, 0, -1 * i); + EXPECT_EQ(acc.getValue(coord), log_miss); + EXPECT_FALSE(acc.isValueOn(coord)); + } + openvdb::Coord coord(0, 0, -5); + EXPECT_EQ(acc.getValue(coord), log_hit); + EXPECT_TRUE(acc.isValueOn(coord)); +} + +TEST(Mapping, InsertMaxRangePoint) +{ + double resolution = 0.1; + OccupancyVDBMapping map(resolution); + Config conf; + conf.max_range = 0.5; + conf.prob_hit = 0.9; + conf.prob_miss = 0.1; + conf.prob_thres_max = 0.51; + conf.prob_thres_min = 0.49; + map.setConfig(conf); + + auto log_miss = static_cast(log(conf.prob_miss) - log(1 - conf.prob_miss)); + + OccupancyVDBMapping::PointCloudT::Ptr cloud(new OccupancyVDBMapping::PointCloudT); + cloud->points.emplace_back(0, 0, 7 * resolution); + Eigen::Matrix origin(0, 0, 0); + map.insertPointCloud(cloud, origin); + OccupancyVDBMapping::GridT::Accessor acc = map.getGrid()->getAccessor(); + for (int i = 0; i <= (int)(conf.max_range / resolution); ++i) + { + openvdb::Coord coord(0, 0, i); + EXPECT_EQ(acc.getValue(coord), log_miss); + EXPECT_FALSE(acc.isValueOn(coord)); + } + openvdb::Coord coord(0, 0, (int)(conf.max_range / resolution) + 1); + EXPECT_EQ(acc.getValue(coord), 0); + EXPECT_FALSE(acc.isValueOn(coord)); +} + +TEST(Mapping, ResetMap) +{ + OccupancyVDBMapping map(1); + Config conf; + conf.max_range = 10; + conf.prob_hit = 0.9; + conf.prob_miss = 0.1; + conf.prob_thres_max = 0.51; + conf.prob_thres_min = 0.49; + map.setConfig(conf); + + auto log_hit = static_cast(log(conf.prob_hit) - log(1 - conf.prob_hit)); + + OccupancyVDBMapping::PointCloudT::Ptr cloud(new OccupancyVDBMapping::PointCloudT); + cloud->points.emplace_back(0, 0, 1); + Eigen::Matrix origin(0, 0, 0); + map.insertPointCloud(cloud, origin); + OccupancyVDBMapping::GridT::Accessor acc = map.getGrid()->getAccessor(); + EXPECT_EQ(acc.getValue(openvdb::Coord(0, 0, 1)), log_hit); + map.resetMap(); + acc = map.getGrid()->getAccessor(); + EXPECT_EQ(acc.getValue(openvdb::Coord(0, 0, 1)), 0.0); +} + +} // namespace vdb_mapping + +int main(int argc, char** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/vdb_mappingConfig.cmake b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/vdb_mappingConfig.cmake new file mode 100644 index 00000000..5c39c095 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/vdb_mappingConfig.cmake @@ -0,0 +1,10 @@ +set(CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}" ${CMAKE_MODULE_PATH}) + +include(CMakeFindDependencyMacro) +find_dependency(Eigen3) +find_dependency(OpenVDB) +find_dependency(Blosc) +find_dependency(PCL) +find_dependency(TBB) + +include("${CMAKE_CURRENT_LIST_DIR}/vdb_mappingTargets.cmake") diff --git a/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/index.html b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/index.html new file mode 100644 index 00000000..1dd76224 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/index.html @@ -0,0 +1,3307 @@ + + + + + + + + + + + + + + + + + + + + + + + VDB Mapping ROS2 Package - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

VDB Mapping ROS2 Package

+

DISCLAIMER: This library is still under development. Be warned that some interfaces will be changed and/or extended in the future.

+

The VDB Mapping ROS2 Package is a ROS2 wrapper around VDB Mapping

+

Getting Started

+

Requirements

+

This library requires OpenVDB as it is build around it. This library was initially developed using Version 5.0 and should work with all versions above.
+Either use the apt package which will be automatically installed via rosdep or compile the package from source using the provided build instructions

+

Build instructions

+

Since the required VDB Mapping library is a plain C++ package, you cannot use catkin_make directly. +Instead you have to use colcon build to build the workspace.

+
# source global ros
+source /opt/ros/<your_ros_version>/setup.bash
+
+# create a catkin workspace
+mkdir -p ~/colcon_ws/src && cd ~/colcon_ws/src
+
+# clone packages
+git clone https://github.com/fzi-forschungszentrum-informatik/vdb_mapping
+git clone https://github.com/fzi-forschungszentrum-informatik/vdb_mapping_ros2
+
+# install dependencies
+sudo apt update
+rosdep update
+rosdep install --from-paths src --ignore-src -y
+
+# build the workspace.  
+colcon build
+
+# source the workspace
+source install/setup.bash
+
+

Remote Mapping

+

The remote mapping module is at the moment mainly designed to provide an external access to the map, without saving and loading it in a different node. In our case we use it to visualize the map data on a remote base station which is then used to control the robots from far away and give the user an overview of the area the robot operates in. Basically it is a full additional mapping node which can access the data chunks generated by the main mapping on the robot. These can the be integrated into the remote mapping instance to generate the same or a similar map depending on the chosen parameter settings. +Diving deeper into the remote mapping functionality you might stumble over the term of updates, overwrites and sections. These are different mechanics to update a remote mapping instance. +The first two are just a byproduct of the normal mapping process. Therefore there is no additional computation needed to generate the data for a remote mapping instance. However all three mechanics use OpenVDB grids which are compressed and serialized as a bitstream before sending them out over the network.

+

Updates

+

Updates are grids generated during the raycasting process and are also used for the internal map update. Here all sensor data is previously accumulated into an update grid which specifies whether the occupancy probability in the map should be updated as occupied or free. This two step process helps to prevent discretization issues due to the larger resolution of the grid compared to the raw sensor data. It is also possible to accumulate multiple sensor sources and measurements into one update grid (see accumulation parameters). The update grid can then be applied to the local or remote instance to generate the same map on both sides.

+

Overwrites

+

Overwrite grids are similar to the update grid but only contain the voxel that have changed their occupancy state in the last iteration. Therefore they are more lightweight in terms of necessary bandwith. This can be useful in cases where the network is rather limited but has the downside, that the probabilistic framework is lost on the remote side.

+

Sections

+

In contrast to updates and overwrites, sections are not bound to the sensor data rate but are generated with a defined rate and create additional processing load. Here the binary occupancy state of a section around a specified frame is copied out of the map and transformed into an update grid that can be applied to a remote instance. Although this created additional overhead it is still highly useful in certain scenarios. In our case we had to handle transmission delay and package loss where both the update and overwrites would result in incomplete maps since grids were lost. Sections on the other hand contain the complete chunk of the map while still being small with respect to the bandwidth.

+

ROS API

+

ROS Parameters

+

VDB Mapping is highly configurable using ROS parameters. Below is a complete list of all available parameters. Internally we store them in a yaml configuration file. An example can be found here.

+

Basic Parameters

+

Listed below are the general parameters to configure the basic behavior of vdb_mapping

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Parameter NameTypeDefaultInformation
map_framestring' 'Coordinate frame of the map
robot_framestring' 'Coordinate frame of the robot
max_rangedouble15.0Global maximum raycasting range (can also be set for each sensor source individually)
resolutiondouble0.07Map resolution
prob_hitdouble0.7Probability update if a beam hits a voxel
prob_missdouble0.4Probability update if a beam misses a voxel
prob_thres_mindouble0.12Lower occupancy threshold of a voxel
prob_thres_maxdouble0.97Upper occupancy threshold of a voxel
map_save_dirstring' 'Storage location for saved maps
accumulate_updatesbooleanfalseSpecifies whether the data of multiple sensor measurement should be accumulated before integrating it into the map.
accumulation_perioddouble1Specifies how long updates should be accumulated before integration
visualization_ratedouble1Specifies in which rate the visualization is published
publish_pointcloudbooleantrueSpecifies whether the map should be published as pointcloud
publish_vis_markerbooleantrueSpecifies whether the map should be published as visual marker
apply_raw_sensor_databooleantrueSpecify whether raw sensor data (e.g. Pointclouds) should be integrated into the map. This flag becomes relevant when the user wants to use a pure remote instance. If set to true all sensor sources will be periodically integrated into the map.
sourcesstring[][]List of sensor sources
remote_sourcesstring[][]List of remote sources
+

Sensor Sources

+

VDB Mapping is able to integrate an arbitrary amount of different sensor sources (given they are available as PointCloud2 msg). To add a new sensor source the user has to add it to the list of sources in the basic parameters. Below are listed the parameters that can be used to configure the individual sensor source. These have to be put into the corresponding namespace in the yaml file. For an example see this configuration file.

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Parameter NameTypeDefaultInformation
topicstring' 'Topic name of the sensor msg
sensor_origin_framestring' 'Sensor frame of the measurement. This parameter is optional and in general the frame id of the msg is used. However in some cases like already frame aligned point clouds this info is no longer the origin of the sensor. For this purpose the user can specify here from which frame the raycasting should be performed.
max_rangedouble0Individual per sensor max raycasting range. This parameter is optional and per default the global max range is used.
+

Remote Sources

+

VDB Mapping provides a remote operation mode. Here the map data is shared between multiple instances to generate the same or similar maps (depending on the chosen parameter settings). For this remote sources can be specified over the parameter server.

+
Local side
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Parameter NameTypeDefaultInformation
publish_updatesbooleanfalseSpecifies whether updates are published
publish_overwritesbooleanfalseSpecifies whether overwrites are published
publish_sectionsbooleanfalseSpecifies whether sections are published
section_update/ratedouble1Rate in which the section update is published
section_update/framestringrobot_frameCenter of the update section
section_update/min_coord/{x,y,z}double-10Min coordinate of the section bounding box centered around the section frame
section_update/max_coord/{x,y,z}double10Max coordinate of the section bounding box centered around the section frame
+
Remote side
+

Below are listed the parameters that can be used to configure the individual remote source. These have to be put into the corresponding namespace in the yaml file. An example config file can be found here.

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Parameter NameTypeDefaultInformation
namespacestring' 'Namespace of the remote mapping instance
apply_remote_updatesbooleanfalseSpecifies whether remote updates should be applied to the map
apply_remote_overwritesbooleanfalseSpecifies whether remote overwrites should be applied to the map
apply_remote_sectionsbooleanfalseSpecifies whether remote sections should be applied to the map
+

Advertised ROS Topics

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Topic NameTypeInformation
~/vdb_map_visualizationvisualization_msgs/MarkerMap visualization topic as voxel marker
~/vdb_map_pointcloudsensor_msgs/PointCloud2Map visualization topic as pointcloud
~/vdb_map_updatesvdb_mapping_msgs/UpdateGridMap updates topic
~/vdb_map_overwritesvdb_mapping_msgs/UpdateGridMap overwrites topic
~/vdb_map_sectionsvdb_mapping_msgs/UpdateGridMap sections topic
+

Subscribed ROS Topics

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Topic NameTypeInformation
{Parameter:sensor_source}/{Parameter:topic}sensor_msgs/PointCloud2Pointcloud sensor message
{Parameter:remote_source}/vdb_map_updatesvdb_mapping_msgs/UpdateGridMap updates topic
{Parameter:remote_source}/vdb_map_overwritesvdb_mapping_msgs/UpdateGridMap overwrites topic
{Parameter:remote_source}/vdb_map_sectionsvdb_mapping_msgs/UpdateGridMap sections topic
+

ROS Services

+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
Service NameTypeInformation
~/load_mapvdb_mapping_msgs/LoadMapLoads the map specified in the msg
~/save_mapstd_srvs/TriggerSaves the current map in the destination specified in the map_save_dir parameter
~/reset_mapstd_srvs/TriggerResets the current map
~/raytracevdb_mapping_msgs/RaytraceRaytraces a point and returns coordinate where the ray first intersected the map
~/trigger_map_section_updatevdb_mapping_msgs/TriggerMapSectionUpdateTriggers a map section update on a remote instance
+

Citation

+

Thanks that you read until here and please let us know if you run into any issues or have suggestions for improvements. +If you use our work in your publications please feel free to cite our manuscript: +

  @inproceedings{besselmann2021vdb,
+  title={VDB-Mapping: a high resolution and real-time capable 3D mapping framework for versatile mobile robots},
+  author={Besselmann, M Grosse and Puck, Lennart and Steffen, Lea and Roennau, Arne and Dillmann, R{\"u}diger},
+  booktitle={2021 IEEE 17th International Conference on Automation Science and Engineering (CASE)},
+  pages={448--454},
+  year={2021},
+  organization={IEEE}
+  doi={10.1109/CASE49439.2021.9551430}}
+}
+
+or for the remote mapping case: +
@incollection{besselmann2022remote,
+  title={Remote VDB-Mapping: A Level-Based Data Reduction Framework for Distributed Mapping},
+  author={Besselmann, Marvin Grosse and R{\"o}nnau, Arne and Dillmann, R{\"u}diger},
+  booktitle={Robotics in Natural Settings: CLAWAR 2022},
+  pages={448--459},
+  year={2022},
+  publisher={Springer}
+  doi={10.1007/978-3-031-15226-9_42}
+}
+

+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ + + +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_interfaces/CMakeLists.txt b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_interfaces/CMakeLists.txt new file mode 100644 index 00000000..8990d72c --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_interfaces/CMakeLists.txt @@ -0,0 +1,50 @@ +cmake_minimum_required(VERSION 3.8) +project(vdb_mapping_interfaces) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/BoundingBox.msg" + "msg/UpdateGrid.msg" + "srv/AddPointsToGrid.srv" + "srv/GetMapSection.srv" + "srv/GetOccGrid.srv" + "srv/LoadMap.srv" + "srv/LoadMapFromPCD.srv" + "srv/Raytrace.srv" + "srv/RemovePointsFromGrid.srv" + "srv/TriggerMapSectionUpdate.srv" + DEPENDENCIES + geometry_msgs + nav_msgs + sensor_msgs + std_msgs +) + + +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_interfaces/LICENSE b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_interfaces/LICENSE new file mode 100644 index 00000000..8db0ba44 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_interfaces/LICENSE @@ -0,0 +1,178 @@ +Copyright 2022 FZI Forschungszentrum Informatik + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grantsto You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS diff --git a/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_interfaces/msg/BoundingBox.msg b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_interfaces/msg/BoundingBox.msg new file mode 100644 index 00000000..c440f5ee --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_interfaces/msg/BoundingBox.msg @@ -0,0 +1,2 @@ +geometry_msgs/Point min_corner +geometry_msgs/Point max_corner diff --git a/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_interfaces/msg/UpdateGrid.msg b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_interfaces/msg/UpdateGrid.msg new file mode 100644 index 00000000..f1f08f21 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_interfaces/msg/UpdateGrid.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +uint8[] map diff --git a/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_interfaces/package.xml b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_interfaces/package.xml new file mode 100644 index 00000000..56386593 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_interfaces/package.xml @@ -0,0 +1,25 @@ + + + + vdb_mapping_interfaces + 0.0.1 + Interface definitions of vdb_mapping ROS2 wrapper + Marvin Große Besselmann + Apache-2.0 + Marvin Große Besselmann + + ament_cmake + + ament_lint_auto + ament_lint_common + rosidl_default_generators + rosidl_default_runtime + geometry_msgs + nav_msgs + std_msgs + + rosidl_interface_packages + + ament_cmake + + diff --git a/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_interfaces/srv/AddPointsToGrid.srv b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_interfaces/srv/AddPointsToGrid.srv new file mode 100644 index 00000000..c7761c4d --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_interfaces/srv/AddPointsToGrid.srv @@ -0,0 +1,3 @@ +sensor_msgs/PointCloud2 points +--- +bool success diff --git a/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_interfaces/srv/GetMapSection.srv b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_interfaces/srv/GetMapSection.srv new file mode 100644 index 00000000..f2d64d85 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_interfaces/srv/GetMapSection.srv @@ -0,0 +1,5 @@ +std_msgs/Header header +BoundingBox bounding_box +--- +UpdateGrid section +bool success diff --git a/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_interfaces/srv/GetOccGrid.srv b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_interfaces/srv/GetOccGrid.srv new file mode 100644 index 00000000..f329ba58 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_interfaces/srv/GetOccGrid.srv @@ -0,0 +1,2 @@ +--- +nav_msgs/OccupancyGrid occupancy_grid diff --git a/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_interfaces/srv/LoadMap.srv b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_interfaces/srv/LoadMap.srv new file mode 100644 index 00000000..d84ab0a0 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_interfaces/srv/LoadMap.srv @@ -0,0 +1,3 @@ +string path +--- +bool success diff --git a/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_interfaces/srv/LoadMapFromPCD.srv b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_interfaces/srv/LoadMapFromPCD.srv new file mode 100644 index 00000000..4b1e1d27 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_interfaces/srv/LoadMapFromPCD.srv @@ -0,0 +1,5 @@ +string path +bool set_background +bool clear_map +--- +bool success diff --git a/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_interfaces/srv/Raytrace.srv b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_interfaces/srv/Raytrace.srv new file mode 100644 index 00000000..56bcbb6c --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_interfaces/srv/Raytrace.srv @@ -0,0 +1,8 @@ +std_msgs/Header header +geometry_msgs/Point origin +geometry_msgs/Vector3 direction +float32 max_ray_length +--- +std_msgs/Header header +bool success +geometry_msgs/Point end_point diff --git a/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_interfaces/srv/RemovePointsFromGrid.srv b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_interfaces/srv/RemovePointsFromGrid.srv new file mode 100644 index 00000000..c7761c4d --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_interfaces/srv/RemovePointsFromGrid.srv @@ -0,0 +1,3 @@ +sensor_msgs/PointCloud2 points +--- +bool success diff --git a/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_interfaces/srv/TriggerMapSectionUpdate.srv b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_interfaces/srv/TriggerMapSectionUpdate.srv new file mode 100644 index 00000000..fa8210de --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_interfaces/srv/TriggerMapSectionUpdate.srv @@ -0,0 +1,5 @@ +std_msgs/Header header +string remote_source +BoundingBox bounding_box +--- +bool success diff --git a/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_ros2/CMakeLists.txt b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_ros2/CMakeLists.txt new file mode 100644 index 00000000..0434d758 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_ros2/CMakeLists.txt @@ -0,0 +1,115 @@ +cmake_minimum_required(VERSION 3.8) +project(vdb_mapping_ros2) + +if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) + message("${PROJECT_NAME}: You did not request a specific build type: selecting 'Release'.") + set(CMAKE_BUILD_TYPE Release) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(vdb_mapping REQUIRED) +find_package(vdb_mapping_interfaces REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(pcl_conversions REQUIRED) +find_package(rclcpp_components REQUIRED) + +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +include_directories(include) + + +set(THIS_PACKAGE_INCLUDE_DEPENDS + rclcpp + rclcpp_components + sensor_msgs + std_msgs + std_srvs + tf2 + tf2_eigen + tf2_ros + vdb_mapping_interfaces + visualization_msgs + pcl_conversions +) + +add_library(${PROJECT_NAME} INTERFACE) +target_include_directories(${PROJECT_NAME} + INTERFACE + $ + $ +) +target_link_libraries(${PROJECT_NAME} INTERFACE + vdb_mapping::vdb_mapping +) +ament_target_dependencies(${PROJECT_NAME} INTERFACE + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) + +add_library(vdb_mapping_ros2_component SHARED + src/vdb_mapping_ros2_component.cpp +) +target_link_libraries(vdb_mapping_ros2_component + vdb_mapping::vdb_mapping +) + +ament_target_dependencies(vdb_mapping_ros2_component + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) +rclcpp_components_register_nodes(vdb_mapping_ros2_component + "vdb_mapping_ros2::vdb_mapping_ros2_component" +) + +install(TARGETS + vdb_mapping_ros2_component + DESTINATION lib +) + +install(TARGETS ${PROJECT_NAME} + EXPORT "export_${PROJECT_NAME}" + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib + INCLUDES DESTINATION include +) + +install(DIRECTORY include/ + DESTINATION include +) + +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}) + +install(DIRECTORY + config + DESTINATION share/${PROJECT_NAME}) + + +if(BUILD_TESTING) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) +endif() + +ament_export_dependencies( + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) +ament_export_targets("export_${PROJECT_NAME}") +ament_package() diff --git a/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_ros2/LICENSE b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_ros2/LICENSE new file mode 100644 index 00000000..8db0ba44 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_ros2/LICENSE @@ -0,0 +1,178 @@ +Copyright 2022 FZI Forschungszentrum Informatik + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grantsto You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS diff --git a/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_ros2/config/vdb_map_server_params.yaml b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_ros2/config/vdb_map_server_params.yaml new file mode 100644 index 00000000..be8dc5aa --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_ros2/config/vdb_map_server_params.yaml @@ -0,0 +1,22 @@ +/**: + ros__parameters: + # Basic setup + map_frame: map + robot_frame: base_link + map_save_dir: "" + + # Visualizations + publish_pointcloud: true + publish_vis_marker: false + visualization_rate: 5.0 + + # Sensor input + accumulate_updates: true + accumulation_period: 0.2 + + apply_raw_sensor_data: false + + map_server: + initial_map_file: "/home/ubuntu/bags/neuheim.pcd" + set_background: false + clear_map: true diff --git a/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_ros2/config/vdb_params.yaml b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_ros2/config/vdb_params.yaml new file mode 100644 index 00000000..7abdff70 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_ros2/config/vdb_params.yaml @@ -0,0 +1,44 @@ +/**: + ros__parameters: + # Basic setup + map_frame: map + robot_frame: base_link + max_range: 10.0 + resolution: 0.07 + prob_hit: 0.99 + prob_miss: 0.1 + thres_min: 0.49 + thres_max: 0.51 + map_save_dir: "" + + # Visualizations + publish_pointcloud: true + publish_vis_marker: true + visualization_rate: 2.0 + + # Sensor input + accumulate_updates: true + accumulation_period: 0.2 + + apply_raw_sensor_data: true + sources: [velodyne] + velodyne: + topic: scan_matched_points2 + sensor_origin_frame: velodyne + + # Remote mapping + publish_updates: true + publish_overwrites: true + publish_sections: true + section_update: + rate: 1.0 + min_coord: + x: -10.0 + y: -10.0 + z: -10.0 + max_coord: + x: 10.0 + y: 10.0 + z: 10.0 + + diff --git a/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_ros2/config/vdb_remote_params.yaml b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_ros2/config/vdb_remote_params.yaml new file mode 100644 index 00000000..b612747b --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_ros2/config/vdb_remote_params.yaml @@ -0,0 +1,33 @@ +/**: + ros__parameters: + # Basic setup + map_frame: map + robot_frame: base_link + max_range: 10.0 + resolution: 0.07 + prob_hit: 0.8 + prob_miss: 0.1 + thres_min: 0.12 + thres_max: 0.8 + map_save_dir: "" + + # Visualizations + publish_pointcloud: true + publish_vis_marker: true + visualization_rate: 2.0 + + # Sensor input + apply_raw_sensor_data: false + + # Remote mapping + publish_updates: false + publish_overwrites: false + publish_sections: false + + remote_sources: [vdb_mapping] + vdb_mapping: + namespace: vdb_mapping + apply_remote_updates: false + apply_remote_overwrites: true + apply_remote_sections: true + diff --git a/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_ros2/include/vdb_mapping_ros2/VDBMappingROS2.hpp b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_ros2/include/vdb_mapping_ros2/VDBMappingROS2.hpp new file mode 100644 index 00000000..68bba483 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_ros2/include/vdb_mapping_ros2/VDBMappingROS2.hpp @@ -0,0 +1,448 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// -- END LICENSE BLOCK ------------------------------------------------ +//---------------------------------------------------------------------- +/*!\file + * + * \author Marvin Große Besselmann grosse@fzi.de + * \date 2022-05-07 + * + */ +//---------------------------------------------------------------------- +#ifndef VDB_MAPPING_ROS2_VDBMAPPINGROS2_HPP_INCLUDED +#define VDB_MAPPING_ROS2_VDBMAPPINGROS2_HPP_INCLUDED + +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define BOOST_BIND_NO_PLACEHOLDERS +#include +#include + +#include + +struct RemoteSource +{ + rclcpp::Subscription::SharedPtr map_update_sub; + rclcpp::Subscription::SharedPtr map_overwrite_sub; + rclcpp::Subscription::SharedPtr map_section_sub; + rclcpp::Client::SharedPtr get_map_section_client; + bool apply_remote_updates; + bool apply_remote_overwrites; + bool apply_remote_sections; +}; + +struct SensorSource +{ + std::string topic; + std::string sensor_origin_frame; + double max_range; +}; + +template +class VDBMappingROS2 : public rclcpp::Node +{ +public: + /*! + * \brief Creates a new VDBMappingROS instance + */ + explicit VDBMappingROS2(const rclcpp::NodeOptions& options); + virtual ~VDBMappingROS2(){}; + + /*! + * \brief Resets the current map + */ + void resetMap(); + /*! + * \brief Saves the current map + */ + bool saveMap(const std::shared_ptr req, + const std::shared_ptr res); + /*! + * \brief Saves the active values of the current map as PCD file + */ + bool saveMapToPCD(const std::shared_ptr req, + const std::shared_ptr res); + /*! + * \brief Load stored map + */ + bool loadMap(const std::shared_ptr req, + const std::shared_ptr res); + + /*! + * \brief Load stored map + */ + bool + loadMapFromPCD(const std::shared_ptr req, + const std::shared_ptr res); + /*! + * \brief Sensor callback for Pointclouds + * + * If the sensor_origin_frame is not empty it will be used instead of the frame id + * of the input cloud as origin of the raycasting + * + * \param cloud_msg PointCloud message + * \param sensor_source Sensor source corresponding to the Pointcloud + */ + void cloudCallback(const std::shared_ptr cloud_msg, + const SensorSource& sensor_source); + /*! + * \brief Integrating the transformed pointcloud and sensor origins into the core mapping library + * + * + * \param cloud Point cloud transformed into map coordinates + * \param tf Sensor transform in map coordinates + */ + void insertPointCloud(const typename VDBMappingT::PointCloudT::Ptr cloud, + const geometry_msgs::msg::TransformStamped transform); + + void publishUpdates(typename VDBMappingT::UpdateGridT::Ptr update, + typename VDBMappingT::UpdateGridT::Ptr overwrite, + rclcpp::Time stamp); + /*! + * \brief Publishes a marker array and pointcloud representation of the map + */ + void publishMap() const; + /*! + * \brief Listens to map updates and creats a map from these + * + * \param update_msg Single map update from a remote mapping instance + */ + void mapUpdateCallback(const std::shared_ptr update_msg); + + /*! + * \brief Listens to map overwrites and creates a map from these + * + * \param update_msg Single map overwrite from a remote mapping instance + */ + void + mapOverwriteCallback(const std::shared_ptr update_msg); + + void + mapSectionCallback(const std::shared_ptr update_msg); + + /*! + * \brief Get the map frame name + * + * \returns Map frame name + */ + const std::string& getMapFrame() const; + + /*! + * \brief Returns the map + * + * \returns VDB map + */ + std::shared_ptr getMap(); + + /*! + * \brief Returns the map + * + * \returns VDB map + */ + const std::shared_ptr getMap() const; + + /*! + * \brief Callback for map reset service call + * + * \param res result of the map reset + * \returns result of map reset + */ + bool resetMapCallback(const std::shared_ptr req, + const std::shared_ptr res); + + /*! + * \brief Callback for requesting parts of the map + * + * \param req Coordinates and reference of the map section + * \param res Result of section request, which includes the returned map + * + * \returns Result of section request + */ + bool getMapSectionCallback( + const std::shared_ptr req, + const std::shared_ptr res); + + /*! + * \brief Callback for triggering a map section request on a remote source + * + * \param req Coordinates, reference frame and remote source identifier of the map section + * \param res Result of triggering section request + * + * \returns Result of triggering section request + */ + bool triggerMapSectionUpdateCallback( + const std::shared_ptr req, + const std::shared_ptr res); + + /*! + * \brief Callback for adding points directly into the grid + * + * \param req Pointcloud which should be added into the grid + * \param res Result of adding points request + * + * \returns Result of adding points request + */ + bool addPointsToGridCallback( + const std::shared_ptr req, + const std::shared_ptr res); + + /*! + * \brief Callback for removing points directly from the grid + * + * \param req Pointcloud which should be removed from the grid + * \param res Result of removing points request + * + * \returns Result of removing points request + */ + bool removePointsFromGridCallback( + const std::shared_ptr req, + const std::shared_ptr res); + + /*! + * \brief Callback for occupancy grid service call + * + * \param req Trigger request + * \param res current occupancy grid + * \returns current occupancy grid + */ + bool + occGridGenCallback(const std::shared_ptr req, + const std::shared_ptr res); + + /*! + * \brief Callback for raytrace service call + * + * \param req Origin and direction for raytracing + * \param res Resulting point of the raytrace + * + * \returns result of raytrace service + */ + bool raytraceCallback(const std::shared_ptr req, + const std::shared_ptr res); + + void visualizationTimerCallback(); + void accumulationUpdateTimerCallback(); + void sectionTimerCallback(); + +private: + std::vector::SharedPtr> m_cloud_subs; + /*! + * \brief Subscriber for raw pointclouds + */ + // rclcpp::Subscription::SharedPtr m_sensor_cloud_sub; + /*! + * \brief Subscriber for scan aligned pointclouds + */ + // rclcpp::Subscription::SharedPtr m_aligned_cloud_sub; + /*! + * \brief Publisher for the marker array + */ + rclcpp::Publisher::SharedPtr m_visualization_marker_pub; + /*! + * \brief Publisher for the point cloud + */ + rclcpp::Publisher::SharedPtr m_pointcloud_pub; + /*! + * \brief Publisher for map updates + */ + rclcpp::Publisher::SharedPtr m_map_update_pub; + /*! + * \brief Publisher for map overwrites + */ + rclcpp::Publisher::SharedPtr m_map_overwrite_pub; + /*! + * \brief Publisher for map sections + */ + rclcpp::Publisher::SharedPtr m_map_section_pub; + /*! + * \brief Saves map in specified path from parameter server + */ + rclcpp::Service::SharedPtr m_save_map_service; + /*! + * \brief Saves the active values of the map as PCD file in the specified path from the paramter + * server + */ + rclcpp::Service::SharedPtr m_save_map_to_pcd_service; + /*! + * \brief Loads a map from specified path from service + */ + rclcpp::Service::SharedPtr m_load_map_service; + /*! + * \brief Generates a map from a PCD file specified by the path from service + */ + rclcpp::Service::SharedPtr + m_load_map_from_pcd_service; + /*! + * \brief Service for reset map + */ + rclcpp::Service::SharedPtr m_reset_map_service; + /*! + * \brief Service for dynamic reconfigure of parameters + */ + // TODO + /*! + * \brief Service to request an occupancy grid based on the current VDB map + */ + rclcpp::Service::SharedPtr m_occupancy_grid_service; + /*! + * \brief Service for raytracing + */ + rclcpp::Service::SharedPtr m_raytrace_service; + /*! + * \brief Service for map section requests + */ + rclcpp::Service::SharedPtr m_get_map_section_service; + /*! + * \brief Service for triggering the map section request on a remote source + */ + rclcpp::Service::SharedPtr + m_trigger_map_section_update_service; + /*! + * \brief Service for adding points directly into the grid. + */ + rclcpp::Service::SharedPtr + m_add_points_to_grid_service; + /*! + * \brief Service for removing points directly from the grid. + */ + rclcpp::Service::SharedPtr + m_remove_points_from_grid_service; + /*! + * \brief Transformation buffer + */ + std::unique_ptr m_tf_buffer; + /*! + * \brief Transformation listener + */ + std::shared_ptr m_tf_listener{nullptr}; + /*! + * \brief Grid cell resolution + */ + double m_resolution; + /*! + * \brief Map Frame + */ + std::string m_map_frame; + /*! + * \brief Robot Frame + */ + std::string m_robot_frame; + /*! + * \brief Map pointer + */ + std::shared_ptr m_vdb_map; + /*! + * \brief Map configuration + */ + vdb_mapping::Config m_config; + /*! + * \brief Specifies whether a pointcloud should be published or not + */ + bool m_publish_pointcloud; + /*! + * \brief Specifies whether the map should be published as markers or not + */ + bool m_publish_vis_marker; + /*! + * \brief Specifies whether the mapping publishes map updates for remote use + */ + bool m_publish_updates; + /*! + * \brief Specifies whether the mapping publishes map overwrites for remote use + */ + bool m_publish_overwrites; + /*! + * \brief Specifies whether the mapping publishes map sections for remote use + */ + bool m_publish_sections; + /*! + * \brief Specifies whether the mapping applies raw sensor data + */ + bool m_apply_raw_sensor_data; + /*! + * \brief Map of remote mapping source connections + */ + std::map m_remote_sources; + /*! + * \brief Timer for map visualization + */ + rclcpp::TimerBase::SharedPtr m_visualization_timer; + /*! + * \brief Specifies whether the sensor data is accumulated before updating the map + */ + bool m_accumulate_updates; + /*! + * \brief Timer for integrating accumulated data + */ + rclcpp::TimerBase::SharedPtr m_accumulation_update_timer; + /*! + * \brief Timer for publishing map sections + */ + rclcpp::TimerBase::SharedPtr m_section_timer; + /*! + * \brief Min Coordinate of the section update bounding box + */ + Eigen::Matrix m_section_min_coord; + /*! + * \brief Max Coordinate of the section update bounding box + */ + Eigen::Matrix m_section_max_coord; + /*! + * \brief Reference Frame for the section update + */ + std::string m_section_update_frame; + /*! + * \brief Specifies the number of voxels which count as occupied for the occupancy grid + */ + int m_two_dim_projection_threshold; + + /*! + * \brief Compression level used for creating the byte array message. + */ + unsigned int m_compression_level = 1; + std::shared_ptr m_param_sub; + std::shared_ptr m_z_min_param_handle; + std::shared_ptr m_z_max_param_handle; + double m_lower_visualization_z_limit; + double m_upper_visualization_z_limit; +}; + + +#include "VDBMappingROS2_impl.hpp" +#endif /* VDB_MAPPING_ROS22_VDBMAPPINGROS2_HPP_INCLUDED */ diff --git a/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_ros2/include/vdb_mapping_ros2/VDBMappingROS2_impl.hpp b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_ros2/include/vdb_mapping_ros2/VDBMappingROS2_impl.hpp new file mode 100644 index 00000000..bf432385 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_ros2/include/vdb_mapping_ros2/VDBMappingROS2_impl.hpp @@ -0,0 +1,801 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// -- END LICENSE BLOCK ------------------------------------------------ +//---------------------------------------------------------------------- +/*!\file + * + * \author Marvin Große Besselmann grosse@fzi.de + * \date 2022-05-07 + * + */ +//---------------------------------------------------------------------- + + +template +VDBMappingROS2::VDBMappingROS2(const rclcpp::NodeOptions& options) + : Node("vdb_mapping_ros2", options) +{ + using namespace std::placeholders; + + m_tf_buffer = std::make_unique(this->get_clock()); + m_tf_listener = std::make_shared(*m_tf_buffer); + + this->declare_parameter("resolution", 0.1); + this->get_parameter("resolution", m_resolution); + m_vdb_map = std::make_shared(m_resolution); + + this->declare_parameter("max_range", 10.0); + this->get_parameter("max_range", m_config.max_range); + this->declare_parameter("prob_hit", 0.7); + this->get_parameter("prob_hit", m_config.prob_hit); + this->declare_parameter("prob_miss", 0.4); + this->get_parameter("prob_miss", m_config.prob_miss); + this->declare_parameter("prob_thres_min", 0.12); + this->get_parameter("prob_thres_min", m_config.prob_thres_min); + this->declare_parameter("prob_thres_max", 0.97); + this->get_parameter("prob_thres_max", m_config.prob_thres_max); + this->declare_parameter("map_directory_path", ""); + this->get_parameter("map_directory_path", m_config.map_directory_path); + this->declare_parameter("two_dim_projection_threshold", 5); + this->get_parameter("two_dim_projection_threshold", m_two_dim_projection_threshold); + + // Configuring the VDB map + m_vdb_map->setConfig(m_config); + + this->declare_parameter("publish_pointcloud", true); + this->get_parameter("publish_pointcloud", m_publish_pointcloud); + this->declare_parameter("publish_vis_marker", true); + this->get_parameter("publish_vis_marker", m_publish_vis_marker); + this->declare_parameter("publish_updates", true); + this->get_parameter("publish_updates", m_publish_updates); + this->declare_parameter("publish_overwrites", true); + this->get_parameter("publish_overwrites", m_publish_overwrites); + this->declare_parameter("publish_sections", true); + this->get_parameter("publish_sections", m_publish_sections); + this->declare_parameter("apply_raw_sensor_data", true); + this->get_parameter("apply_raw_sensor_data", m_apply_raw_sensor_data); + + + this->declare_parameter("z_limit_min", 0); + this->get_parameter("z_limit_min", m_lower_visualization_z_limit); + this->declare_parameter("z_limit_max", 0); + this->get_parameter("z_limit_max", m_upper_visualization_z_limit); + + m_param_sub = std::make_shared(this); + + auto min_z_cb = [this](const rclcpp::Parameter &p) { + m_lower_visualization_z_limit = p.as_double(); + }; + auto max_z_cb = [this](const rclcpp::Parameter &p) { + m_upper_visualization_z_limit = p.as_double(); + }; + + m_z_min_param_handle = m_param_sub->add_parameter_callback("z_limit_min", min_z_cb); + m_z_min_param_handle = m_param_sub->add_parameter_callback("z_limit_max", max_z_cb); + + + this->declare_parameter("map_frame", ""); + this->get_parameter("map_frame", m_map_frame); + if (m_map_frame.empty()) + { + RCLCPP_WARN(this->get_logger(), "No map frame specified"); + } + m_vdb_map->getGrid()->insertMeta("ros/map_frame", openvdb::StringMetadata(m_map_frame)); + this->declare_parameter("robot_frame", ""); + this->get_parameter("robot_frame", m_robot_frame); + if (m_robot_frame.empty()) + { + RCLCPP_WARN(this->get_logger(), "No robot frame specified"); + } + + // Setting up remote sources + std::vector source_ids; + this->declare_parameter >("remote_sources", std::vector()); + this->get_parameter("remote_sources", source_ids); + + for (auto& source_id : source_ids) + { + std::string remote_namespace; + this->declare_parameter(source_id + ".namespace", ""); + this->get_parameter(source_id + ".namespace", remote_namespace); + + RemoteSource remote_source; + this->declare_parameter(source_id + ".apply_remote_updates", false); + this->get_parameter(source_id + ".apply_remote_updates", remote_source.apply_remote_updates); + this->declare_parameter(source_id + ".apply_remote_overwrites", false); + this->get_parameter(source_id + ".apply_remote_overwrites", + remote_source.apply_remote_overwrites); + this->declare_parameter(source_id + ".apply_remote_sections", false); + this->get_parameter(source_id + ".apply_remote_sections", remote_source.apply_remote_sections); + if (remote_source.apply_remote_updates) + { + remote_source.map_update_sub = + this->create_subscription( + remote_namespace + "/vdb_map_updates", + rclcpp::QoS(10).durability_volatile().best_effort(), + std::bind(&VDBMappingROS2::mapUpdateCallback, this, _1)); + } + if (remote_source.apply_remote_overwrites) + { + remote_source.map_overwrite_sub = + this->create_subscription( + remote_namespace + "/vdb_map_overwrites", + rclcpp::QoS(10).durability_volatile().best_effort(), + std::bind(&VDBMappingROS2::mapOverwriteCallback, this, _1)); + } + if (remote_source.apply_remote_sections) + { + remote_source.map_section_sub = + this->create_subscription( + remote_namespace + "/vdb_map_sections", + rclcpp::QoS(10).durability_volatile().best_effort(), + std::bind(&VDBMappingROS2::mapSectionCallback, this, _1)); + } + remote_source.get_map_section_client = + this->create_client(remote_namespace + + "/get_map_section"); + m_remote_sources.insert(std::make_pair(source_id, remote_source)); + } + + if (m_publish_updates) + { + m_map_update_pub = this->create_publisher( + "~/vdb_map_updates", rclcpp::QoS(1).durability_volatile().best_effort()); + } + if (m_publish_overwrites) + { + m_map_overwrite_pub = this->create_publisher( + "~/vdb_map_overwrites", rclcpp::QoS(1).durability_volatile().best_effort()); + } + if (m_publish_sections) + { + m_map_section_pub = this->create_publisher( + "~/vdb_map_sections", rclcpp::QoS(1).durability_volatile().best_effort()); + + double section_update_rate; + this->declare_parameter("section_update.rate", 1); + this->get_parameter("section_update.rate", section_update_rate); + m_section_timer = + this->create_wall_timer(std::chrono::milliseconds((int)(1000.0 / section_update_rate)), + std::bind(&VDBMappingROS2::sectionTimerCallback, this)); + + this->declare_parameter("section_update.min_coord.x", -10); + this->get_parameter("section_update.min_coord.x", m_section_min_coord.x()); + this->declare_parameter("section_update.min_coord.y", -10); + this->get_parameter("section_update.min_coord.y", m_section_min_coord.y()); + this->declare_parameter("section_update.min_coord.z", -10); + this->get_parameter("section_update.min_coord.z", m_section_min_coord.z()); + this->declare_parameter("section_update.max_coord.x", 10); + this->get_parameter("section_update.max_coord.x", m_section_max_coord.x()); + this->declare_parameter("section_update.max_coord.y", 10); + this->get_parameter("section_update.max_coord.y", m_section_max_coord.y()); + this->declare_parameter("section_update.max_coord.z", 10); + this->get_parameter("section_update.max_coord.z", m_section_max_coord.z()); + this->declare_parameter("section_update.frame", m_robot_frame); + this->get_parameter("section_update.frame", m_section_update_frame); + } + + if (m_apply_raw_sensor_data) + { + this->declare_parameter >("sources", std::vector()); + this->get_parameter("sources", source_ids); + + for (auto& source_id : source_ids) + { + SensorSource sensor_source; + this->declare_parameter(source_id + ".topic", ""); + this->get_parameter(source_id + ".topic", sensor_source.topic); + this->declare_parameter(source_id + ".sensor_origin_frame", ""); + this->get_parameter(source_id + ".sensor_origin_frame", sensor_source.sensor_origin_frame); + this->declare_parameter(source_id + ".max_range", 0); + this->get_parameter(source_id + ".max_range", sensor_source.max_range); + RCLCPP_INFO_STREAM(this->get_logger(), "Setting up source: " << source_id); + + if (sensor_source.topic.empty()) + { + RCLCPP_ERROR_STREAM(this->get_logger(), + "No input topic specified for source: " << source_id); + continue; + } + RCLCPP_INFO_STREAM(this->get_logger(), "Topic: " << sensor_source.topic); + if (sensor_source.sensor_origin_frame.empty()) + { + RCLCPP_INFO(this->get_logger(), "Using frame id of topic as raycast origin"); + } + else + { + RCLCPP_INFO_STREAM(this->get_logger(), + "Using " << sensor_source.sensor_origin_frame << " as raycast origin"); + } + + + m_cloud_subs.push_back(this->create_subscription( + sensor_source.topic, + rclcpp::QoS(1).durability_volatile().best_effort(), + [&, sensor_source](const std::shared_ptr cloud_msg) { + cloudCallback(cloud_msg, sensor_source); + })); + } + } + + m_reset_map_service = this->create_service( + "~/reset_map", std::bind(&VDBMappingROS2::resetMapCallback, this, _1, _2)); + + m_save_map_service = this->create_service( + "~/save_map", std::bind(&VDBMappingROS2::saveMap, this, _1, _2)); + + m_load_map_service = this->create_service( + "~/load_map", std::bind(&VDBMappingROS2::loadMap, this, _1, _2)); + + m_load_map_from_pcd_service = this->create_service( + "~/load_map_from_pcd", std::bind(&VDBMappingROS2::loadMapFromPCD, this, _1, _2)); + + m_get_map_section_service = this->create_service( + "~/get_map_section", std::bind(&VDBMappingROS2::getMapSectionCallback, this, _1, _2)); + + m_trigger_map_section_update_service = + this->create_service( + "~/trigger_map_section_update", + std::bind(&VDBMappingROS2::triggerMapSectionUpdateCallback, this, _1, _2)); + + m_raytrace_service = this->create_service( + "~/raytrace", std::bind(&VDBMappingROS2::raytraceCallback, this, _1, _2)); + + m_add_points_to_grid_service = this->create_service( + "~/add_points_to_grid", std::bind(&VDBMappingROS2::addPointsToGridCallback, this, _1, _2)); + + m_remove_points_from_grid_service = + this->create_service( + "~/remove_points_from_grid", + std::bind(&VDBMappingROS2::removePointsFromGridCallback, this, _1, _2)); + + m_pointcloud_pub = + this->create_publisher("~/vdb_map_pointcloud", 1); + m_visualization_marker_pub = + this->create_publisher("~/vdb_map_visualization", 1); + + m_occupancy_grid_service = this->create_service( + "~/get_occupancy_grid", std::bind(&VDBMappingROS2::occGridGenCallback, this, _1, _2)); + + double visualization_rate; + this->declare_parameter("visualization_rate", 1.0); + this->get_parameter("visualization_rate", visualization_rate); + if (visualization_rate > 0.0) + { + m_visualization_timer = + this->create_wall_timer(std::chrono::milliseconds((int)(1000.0 / visualization_rate)), + std::bind(&VDBMappingROS2::visualizationTimerCallback, this)); + } + + this->declare_parameter("accumulate_updates", false); + this->get_parameter("accumulate_updates", m_accumulate_updates); + if (m_accumulate_updates) + { + double accumulation_period; + this->declare_parameter("accumulation_period", 1); + this->get_parameter("accumulation_period", accumulation_period); + m_accumulation_update_timer = + this->create_wall_timer(std::chrono::milliseconds((int)(1000 * accumulation_period)), + std::bind(&VDBMappingROS2::accumulationUpdateTimerCallback, this)); + } + + // Load initial map file + std::string initial_map_file; + bool set_background; + bool clear_map; + this->declare_parameter("map_server.initial_map_file", ""); + this->get_parameter("map_server.initial_map_file", initial_map_file); + RCLCPP_INFO_STREAM(this->get_logger(), "Loading intial Map " << initial_map_file); + this->declare_parameter("map_server.set_background", false); + this->get_parameter("map_server.set_background", set_background); + this->declare_parameter("map_server.clear_map", false); + this->get_parameter("map_server.clear_map", clear_map); + if (initial_map_file != "") + { + RCLCPP_INFO_STREAM(this->get_logger(), "Loading intial Map " << initial_map_file); + m_vdb_map->loadMapFromPCD(initial_map_file, set_background, clear_map); + publishMap(); + } +} + +template +void VDBMappingROS2::resetMap() +{ + RCLCPP_INFO(this->get_logger(), "Resetting Map"); + m_vdb_map->resetMap(); + publishMap(); +} + +template +bool VDBMappingROS2::resetMapCallback( + const std::shared_ptr req, + const std::shared_ptr res) +{ + (void)req; + resetMap(); + res->success = true; + res->message = "Reset map successful."; + return true; +} + +template +bool VDBMappingROS2::saveMap( + const std::shared_ptr req, + const std::shared_ptr res) +{ + (void)req; + RCLCPP_INFO(this->get_logger(), "Saving Map"); + res->success = m_vdb_map->saveMap(); + return res->success; +} + +template +bool VDBMappingROS2::saveMapToPCD( + const std::shared_ptr req, + const std::shared_ptr res) +{ + (void)req; + RCLCPP_INFO(this->get_logger(), "Saving Map to PCD"); + res->success = m_vdb_map->saveMapToPCD(); + return res->success; +} + +template +bool VDBMappingROS2::loadMap( + const std::shared_ptr req, + const std::shared_ptr res) +{ + RCLCPP_INFO(this->get_logger(), "Loading Map"); + bool success = m_vdb_map->loadMap(req->path); + publishMap(); + res->success = success; + return success; +} + +template +bool VDBMappingROS2::loadMapFromPCD( + const std::shared_ptr req, + const std::shared_ptr res) +{ + RCLCPP_INFO(this->get_logger(), "Loading Map from PCD file"); + bool success = m_vdb_map->loadMapFromPCD(req->path, req->set_background, req->clear_map); + publishMap(); + res->success = success; + return success; +} + + +template +const std::string& VDBMappingROS2::getMapFrame() const +{ + return m_map_frame; +} + +template +std::shared_ptr VDBMappingROS2::getMap() +{ + return m_vdb_map; +} + +template +const std::shared_ptr VDBMappingROS2::getMap() const +{ + return m_vdb_map; +} + +template +bool VDBMappingROS2::getMapSectionCallback( + const std::shared_ptr req, + const std::shared_ptr res) +{ + geometry_msgs::msg::TransformStamped source_to_map_tf; + try + { + source_to_map_tf = m_tf_buffer->lookupTransform( + m_map_frame, req->header.frame_id, rclcpp::Time(0), rclcpp::Duration(1, 0)); + } + catch (tf2::TransformException& ex) + { + RCLCPP_ERROR(this->get_logger(), + "Could not transform %s to %s: %s", + m_map_frame.c_str(), + req->header.frame_id.c_str(), + ex.what()); + res->success = false; + return true; + } + res->section.map = m_vdb_map->template gridToByteArray( + m_vdb_map->getMapSectionUpdateGrid(Eigen::Matrix(req->bounding_box.min_corner.x, + req->bounding_box.min_corner.y, + req->bounding_box.min_corner.z), + Eigen::Matrix(req->bounding_box.max_corner.x, + req->bounding_box.max_corner.y, + req->bounding_box.max_corner.z), + tf2::transformToEigen(source_to_map_tf).matrix())); + res->section.header.frame_id = m_map_frame; + res->section.header.stamp = this->now(); + res->success = true; + + return true; +} + +template +bool VDBMappingROS2::triggerMapSectionUpdateCallback( + const std::shared_ptr req, + const std::shared_ptr res) +{ + auto remote_source = m_remote_sources.find(req->remote_source); + if (remote_source == m_remote_sources.end()) + { + std::stringstream ss; + ss << "Key " << req->remote_source << " not found. Available sources are: "; + for (auto& source : m_remote_sources) + { + ss << source.first << ", "; + } + RCLCPP_WARN(this->get_logger(), ss.str().c_str()); + res->success = false; + return true; + } + + auto request = std::make_shared(); + + request->header = req->header; + request->bounding_box = req->bounding_box; + auto result = remote_source->second.get_map_section_client->async_send_request(request); + if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) == + rclcpp::FutureReturnCode::SUCCESS) + { + auto response = result.get(); + if (response->success) + { + m_vdb_map->overwriteMap( + m_vdb_map->template byteArrayToGrid( + response->section.map)); + } + res->success = response->success; + } + else + { + RCLCPP_ERROR(this->get_logger(), "Failed to call servcie get_map_section"); + res->success = false; + } + + return true; +} + +template +bool VDBMappingROS2::raytraceCallback( + const std::shared_ptr req, + const std::shared_ptr res) +{ + geometry_msgs::msg::TransformStamped reference_tf; + try + { + reference_tf = + m_tf_buffer->lookupTransform(m_map_frame, req->header.frame_id.c_str(), req->header.stamp); + + Eigen::Matrix m = tf2::transformToEigen(reference_tf).matrix(); + Eigen::Matrix origin, direction; + origin << req->origin.x, req->origin.y, req->origin.z, 1; + direction << req->direction.x, req->direction.y, req->direction.z, 0; + + origin = m * origin; + direction = m * direction; + + openvdb::Vec3d end_point; + + res->success = m_vdb_map->raytrace(openvdb::Vec3d(origin.x(), origin.y(), origin.z()), + openvdb::Vec3d(direction.x(), direction.y(), direction.z()), + req->max_ray_length, + end_point); + + res->header.frame_id = m_map_frame; + res->header.stamp = req->header.stamp; + res->end_point.x = end_point.x(); + res->end_point.y = end_point.y(); + res->end_point.z = end_point.z(); + } + catch (tf2::TransformException& ex) + { + RCLCPP_ERROR_STREAM(this->get_logger(), "Transform to map frame failed: " << ex.what()); + res->success = false; + } + return true; +} + +template +bool VDBMappingROS2::addPointsToGridCallback( + const std::shared_ptr req, + const std::shared_ptr res) +{ + typename VDBMappingT::PointCloudT::Ptr cloud(new typename VDBMappingT::PointCloudT); + pcl::fromROSMsg(req->points, *cloud); + res->success = m_vdb_map->addPointsToGrid(cloud); + return true; +} + +template +bool VDBMappingROS2::removePointsFromGridCallback( + const std::shared_ptr req, + const std::shared_ptr res) +{ + typename VDBMappingT::PointCloudT::Ptr cloud(new typename VDBMappingT::PointCloudT); + pcl::fromROSMsg(req->points, *cloud); + res->success = m_vdb_map->removePointsFromGrid(cloud); + return true; +} + +template +void VDBMappingROS2::visualizationTimerCallback() +{ + publishMap(); +} + +template +void VDBMappingROS2::accumulationUpdateTimerCallback() +{ + typename VDBMappingT::UpdateGridT::Ptr update; + typename VDBMappingT::UpdateGridT::Ptr overwrite; + m_vdb_map->integrateUpdate(update, overwrite); + + publishUpdates(update, overwrite, this->now()); + m_vdb_map->resetUpdate(); +} + +template +void VDBMappingROS2::sectionTimerCallback() +{ + geometry_msgs::msg::TransformStamped map_to_robot_tf; + try + { + // Get sensor origin transform in map coordinates + map_to_robot_tf = m_tf_buffer->lookupTransform( + m_map_frame, m_section_update_frame, rclcpp::Time(0), rclcpp::Duration(1, 0)); + } + catch (tf2::TransformException& ex) + { + RCLCPP_ERROR(this->get_logger(), + "Could not transform %s to %s: %s", + m_map_frame.c_str(), + m_section_update_frame.c_str(), + ex.what()); + return; + } + + typename VDBMappingT::UpdateGridT::Ptr section = m_vdb_map->getMapSectionUpdateGrid( + m_section_min_coord, m_section_max_coord, tf2::transformToEigen(map_to_robot_tf).matrix()); + vdb_mapping_interfaces::msg::UpdateGrid msg; + msg.header.frame_id = m_map_frame; + msg.header.stamp = map_to_robot_tf.header.stamp; + msg.map = m_vdb_map->template gridToByteArray(section); + m_map_section_pub->publish(msg); +} + +template +void VDBMappingROS2::mapUpdateCallback( + const std::shared_ptr update_msg) +{ + m_vdb_map->updateMap( + m_vdb_map->template byteArrayToGrid(update_msg->map)); +} + +template +void VDBMappingROS2::mapOverwriteCallback( + const std::shared_ptr update_msg) +{ + m_vdb_map->overwriteMap( + m_vdb_map->template byteArrayToGrid(update_msg->map)); +} + +template +void VDBMappingROS2::mapSectionCallback( + const std::shared_ptr update_msg) +{ + m_vdb_map->applyMapSectionUpdateGrid( + m_vdb_map->template byteArrayToGrid(update_msg->map)); +} + +template +bool VDBMappingROS2::occGridGenCallback( + const std::shared_ptr req, + const std::shared_ptr res) +{ + (void)req; + nav_msgs::msg::OccupancyGrid grid; + openvdb::CoordBBox curr_bbox = m_vdb_map->getGrid()->evalActiveVoxelBoundingBox(); + grid.header.frame_id = m_map_frame; + grid.header.stamp = this->now(); + grid.info.height = curr_bbox.dim().y(); + grid.info.width = curr_bbox.dim().x(); + grid.info.resolution = m_resolution; + std::vector voxel_projection_grid; + grid.data.resize(grid.info.width * grid.info.height); + voxel_projection_grid.resize(grid.info.width * grid.info.height); + + int x_offset = abs(curr_bbox.min().x()); + int y_offset = abs(curr_bbox.min().y()); + + geometry_msgs::msg::Pose origin_pose; + origin_pose.position.x = curr_bbox.min().x() * m_resolution; + origin_pose.position.y = curr_bbox.min().y() * m_resolution; + origin_pose.position.z = 0.00; + + grid.info.origin = origin_pose; + int world_to_index = 0; + for (openvdb::FloatGrid::ValueOnCIter iter = m_vdb_map->getGrid()->cbeginValueOn(); iter; ++iter) + { + if (iter.isValueOn()) + { + world_to_index = + (iter.getCoord().y() + y_offset) * curr_bbox.dim().x() + (iter.getCoord().x() + x_offset); + voxel_projection_grid[world_to_index] += 1; + } + } + + for (size_t i = 0; i < voxel_projection_grid.size(); i++) + { + if (voxel_projection_grid[i] > m_two_dim_projection_threshold) + { + grid.data[i] = 100; + } + else if (voxel_projection_grid[i] == 0) + { + grid.data[i] = -1; + } + else + { + grid.data[i] = 0; + } + } + res->occupancy_grid = grid; + return true; +} + + +template +void VDBMappingROS2::cloudCallback( + const std::shared_ptr cloud_msg, const SensorSource& sensor_source) +{ + typename VDBMappingT::PointCloudT::Ptr cloud(new typename VDBMappingT::PointCloudT); + pcl::fromROSMsg(*cloud_msg, *cloud); + geometry_msgs::msg::TransformStamped cloud_origin_tf; + + std::string sensor_frame = sensor_source.sensor_origin_frame.empty() + ? cloud_msg->header.frame_id + : sensor_source.sensor_origin_frame; + + // Get the origin of the sensor used as a starting point of the ray cast + try + { + cloud_origin_tf = m_tf_buffer->lookupTransform( + m_map_frame, sensor_frame, cloud_msg->header.stamp, rclcpp::Duration(0, 100000000)); + } + catch (tf2::TransformException& ex) + { + RCLCPP_ERROR(this->get_logger(), + "Could not transform %s to %s: %s", + m_map_frame.c_str(), + sensor_frame.c_str(), + ex.what()); + return; + } + // If aligned map is not already in correct map frame, transform it + if (m_map_frame != cloud_msg->header.frame_id) + { + geometry_msgs::msg::TransformStamped origin_to_map_tf; + try + { + origin_to_map_tf = m_tf_buffer->lookupTransform( + m_map_frame, cloud_msg->header.frame_id, cloud_msg->header.stamp); + } + catch (tf2::TransformException& ex) + { + RCLCPP_ERROR(this->get_logger(), + "Could not transform %s to %s: %s", + m_map_frame.c_str(), + cloud_msg->header.frame_id.c_str(), + ex.what()); + return; + } + pcl::transformPointCloud(*cloud, *cloud, tf2::transformToEigen(origin_to_map_tf).matrix()); + cloud->header.frame_id = m_map_frame; + } + m_vdb_map->accumulateUpdate( + cloud, tf2::transformToEigen(cloud_origin_tf).translation(), sensor_source.max_range); + if (!m_accumulate_updates) + { + typename VDBMappingT::UpdateGridT::Ptr update; + typename VDBMappingT::UpdateGridT::Ptr overwrite; + m_vdb_map->integrateUpdate(update, overwrite); + m_vdb_map->resetUpdate(); + publishUpdates(update, overwrite, cloud_msg->header.stamp); + } +} + +template +void VDBMappingROS2::insertPointCloud( + const typename VDBMappingT::PointCloudT::Ptr cloud, + const geometry_msgs::msg::TransformStamped transform) +{ + Eigen::Matrix sensor_to_map_eigen = tf2::transformToEigen(transform).translation(); + typename VDBMappingT::UpdateGridT::Ptr update; + typename VDBMappingT::UpdateGridT::Ptr overwrite; + // Integrate data into vdb grid + m_vdb_map->insertPointCloud(cloud, sensor_to_map_eigen, update, overwrite); + publishUpdates(update, overwrite, transform.header.stamp); +} + +template +void VDBMappingROS2::publishUpdates(typename VDBMappingT::UpdateGridT::Ptr update, + typename VDBMappingT::UpdateGridT::Ptr overwrite, + rclcpp::Time stamp) +{ + std_msgs::msg::Header header; + header.frame_id = m_map_frame; + header.stamp = stamp; + if (m_publish_updates) + { + vdb_mapping_interfaces::msg::UpdateGrid msg; + msg.map = m_vdb_map->template gridToByteArray(update); + msg.header = header; + m_map_update_pub->publish(msg); + } + if (m_publish_overwrites) + { + vdb_mapping_interfaces::msg::UpdateGrid msg; + msg.map = m_vdb_map->template gridToByteArray(overwrite); + msg.header = header; + m_map_update_pub->publish(msg); + } +} + +template +void VDBMappingROS2::publishMap() const +{ + if (!(m_publish_pointcloud || m_publish_vis_marker)) + { + return; + } + bool publish_vis_marker; + publish_vis_marker = + (m_publish_vis_marker && this->count_subscribers("~/vdb_map_visualization") > 0); + bool publish_pointcloud; + publish_pointcloud = + (m_publish_pointcloud && this->count_subscribers("~/vdb_map_pointcloud") > 0); + + visualization_msgs::msg::Marker visualization_marker_msg; + sensor_msgs::msg::PointCloud2 cloud_msg; + VDBMappingTools::createMappingOutput(m_vdb_map->getGrid(), + m_map_frame, + visualization_marker_msg, + cloud_msg, + m_publish_vis_marker, + m_publish_pointcloud, + m_lower_visualization_z_limit, + m_upper_visualization_z_limit); + if (publish_vis_marker) + { + visualization_marker_msg.header.stamp = this->now(); + m_visualization_marker_pub->publish(visualization_marker_msg); + } + if (publish_pointcloud) + { + cloud_msg.header.stamp = this->now(); + m_pointcloud_pub->publish(cloud_msg); + } +} diff --git a/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_ros2/include/vdb_mapping_ros2/VDBMappingTools.hpp b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_ros2/include/vdb_mapping_ros2/VDBMappingTools.hpp new file mode 100644 index 00000000..225883f6 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_ros2/include/vdb_mapping_ros2/VDBMappingTools.hpp @@ -0,0 +1,73 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// -- END LICENSE BLOCK ------------------------------------------------ +//---------------------------------------------------------------------- +/*!\file + * + * \author Marvin Große Besselmann grosse@fzi.de + * \date 2022-05-09 + * + */ +//---------------------------------------------------------------------- +#ifndef VDB_MAPPING_ROS2_VDBMAPPINGTOOLS_H_INCLUDED +#define VDB_MAPPING_ROS2_VDBMAPPINGTOOLS_H_INCLUDED +#include +#include +#include +#include +#include +#include + +/*! + * \brief Collection of VDBMapping helper functions and tools + */ +template +class VDBMappingTools +{ +public: + VDBMappingTools(){}; + virtual ~VDBMappingTools(){}; + /*! + * \brief Creates output msgs for pointcloud and marker arrays + * + * \param grid Map grid + * \param resolution Resolution of the grid + * \param frame_id Frame ID of the grid + * \param marker_msg Output Marker message + * \param cloud_msg Output Pointcloud message + * \param create_marker Flag specifying to create a marker message + * \param create_pointcloud Flag specifying to create a pointcloud message + */ + static void createMappingOutput(const typename VDBMappingT::GridT::Ptr gri, + const std::string& frame_id, + visualization_msgs::msg::Marker& marker_msg, + sensor_msgs::msg::PointCloud2& cloud_msg, + bool create_marker, + bool create_pointcloud, + double lower_z_limit = 0.0, + double upper_z_limit = 0.0); + /*! + * \brief Calculates a height correlating color coding using HSV color space + * + * \param height Gridcell height relativ to the min and max height of the complete grid. Parameter + * can take values between 0 and 1 + * + * \returns RGBA color of the grid cell + */ + static std_msgs::msg::ColorRGBA heightColorCoding(const double height); +}; +#include "VDBMappingTools_impl.hpp" +#endif /* VDB_MAPPING_ROS2_VDBMAPPINGTOOLS_H_INCLUDED */ diff --git a/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_ros2/include/vdb_mapping_ros2/VDBMappingTools_impl.hpp b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_ros2/include/vdb_mapping_ros2/VDBMappingTools_impl.hpp new file mode 100644 index 00000000..baa1e349 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_ros2/include/vdb_mapping_ros2/VDBMappingTools_impl.hpp @@ -0,0 +1,148 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2022 FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// -- END LICENSE BLOCK ------------------------------------------------ +//---------------------------------------------------------------------- +/*!\file + * + * \author Marvin Große Besselmann grosse@fzi.de + * \date 2022-05-09 + * + */ +//---------------------------------------------------------------------- +template +void VDBMappingTools::createMappingOutput(const typename VDBMappingT::GridT::Ptr grid, + const std::string& frame_id, + visualization_msgs::msg::Marker& marker_msg, + sensor_msgs::msg::PointCloud2& cloud_msg, + const bool create_marker, + const bool create_pointcloud, + double lower_z_limit, + double upper_z_limit) +{ + typename VDBMappingT::PointCloudT::Ptr cloud(new typename VDBMappingT::PointCloudT); + openvdb::CoordBBox bbox = grid->evalActiveVoxelBoundingBox(); + double min_z, max_z; + openvdb::Vec3d min_world_coord = grid->indexToWorld(bbox.getStart()); + openvdb::Vec3d max_world_coord = grid->indexToWorld(bbox.getEnd()); + min_z = min_world_coord.z(); + max_z = max_world_coord.z(); + + if (lower_z_limit != upper_z_limit && lower_z_limit < upper_z_limit) + { + min_z = min_z < lower_z_limit ? lower_z_limit : min_z; + max_z = max_z > upper_z_limit ? upper_z_limit : max_z; + } + + for (typename VDBMappingT::GridT::ValueOnCIter iter = grid->cbeginValueOn(); iter; ++iter) + { + openvdb::Vec3d world_coord = grid->indexToWorld(iter.getCoord()); + + if( world_coord.z() < min_z || world_coord.z() > max_z) + { + continue; + } + + if (create_marker) + { + geometry_msgs::msg::Point cube_center; + cube_center.x = world_coord.x(); + cube_center.y = world_coord.y(); + cube_center.z = world_coord.z(); + marker_msg.points.push_back(cube_center); + // Calculate the relative height of each voxel. + double h = (1.0 - ((world_coord.z() - min_z) / (max_z - min_z))); + marker_msg.colors.push_back(heightColorCoding(h)); + } + if (create_pointcloud) + { + cloud->points.push_back( + typename VDBMappingT::PointT(world_coord.x(), world_coord.y(), world_coord.z())); + } + } + if (create_marker) + { + double size = grid->transform().voxelSize()[0]; + marker_msg.header.frame_id = frame_id; + // marker_msg.header.stamp = ros::Time::now(); + marker_msg.id = 0; + marker_msg.type = visualization_msgs::msg::Marker::CUBE_LIST; + marker_msg.scale.x = size; + marker_msg.scale.y = size; + marker_msg.scale.z = size; + marker_msg.color.a = 1.0; + marker_msg.pose.orientation.w = 1.0; + marker_msg.frame_locked = true; + if (marker_msg.points.size() > 0) + { + marker_msg.action = visualization_msgs::msg::Marker::ADD; + } + else + { + marker_msg.action = visualization_msgs::msg::Marker::DELETE; + } + } + if (create_pointcloud) + { + cloud->width = cloud->points.size(); + cloud->height = 1; + pcl::toROSMsg(*cloud, cloud_msg); + cloud_msg.header.frame_id = frame_id; + // cloud_msg.header.stamp = ros::Time::now(); + } +} +// Conversion from Hue to RGB Value +template +std_msgs::msg::ColorRGBA VDBMappingTools::heightColorCoding(const double height) +{ + // The factor of 0.8 is only for a nicer color range + double h = height * 0.8; + int i = (int)(h * 6.0); + double f = (h * 6.0) - i; + double q = (1.0 - f); + i %= 6; + auto toMsg = [](double v1, double v2, double v3) { + std_msgs::msg::ColorRGBA rgba; + rgba.a = 1.0; + rgba.r = v1; + rgba.g = v2; + rgba.b = v3; + return rgba; + }; + switch (i) + { + case 0: + return toMsg(1.0, f, 0.0); + break; + case 1: + return toMsg(q, 1.0, 0.0); + break; + case 2: + return toMsg(0.0, 1.0, f); + break; + case 3: + return toMsg(0.0, q, 1.0); + break; + case 4: + return toMsg(f, 0.0, 1.0); + break; + case 5: + return toMsg(1.0, 0.0, q); + break; + default: + return toMsg(1.0, 0.5, 0.5); + break; + } +} diff --git a/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_ros2/launch/vdb_map_server_ros2.py b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_ros2/launch/vdb_map_server_ros2.py new file mode 100644 index 00000000..50d7875b --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_ros2/launch/vdb_map_server_ros2.py @@ -0,0 +1,37 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch_ros.actions import SetParametersFromFile + +def generate_launch_description(): + ld = LaunchDescription() + + config = os.path.join( + get_package_share_directory('vdb_mapping_ros2'), + 'config', + 'vdb_map_server_params.yaml' + ) + + print(config) + + container = ComposableNodeContainer( + name='Container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + ComposableNode( + package='vdb_mapping_ros2', + plugin='vdb_mapping_ros2::vdb_mapping_ros2_component', + name='vdb_mapping', + parameters=[config], + ) + ], + output='screen', + ) + + ld.add_action(container) + + return ld diff --git a/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_ros2/launch/vdb_mapping_ros2.py b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_ros2/launch/vdb_mapping_ros2.py new file mode 100644 index 00000000..3cca4188 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_ros2/launch/vdb_mapping_ros2.py @@ -0,0 +1,45 @@ +import os +from launch.actions import DeclareLaunchArgument + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch_ros.actions import SetParametersFromFile +from launch.substitutions import LaunchConfiguration, TextSubstitution + + +def generate_launch_description(): + ld = LaunchDescription() + + config_launch_arg = DeclareLaunchArgument( + "config", + default_value=TextSubstitution( + text=os.path.join( + get_package_share_directory("vdb_mapping_ros2"), + "config", + "vdb_params.yaml", + ) + ), + ) + + container = ComposableNodeContainer( + name="Container", + namespace="", + package="rclcpp_components", + executable="component_container", + composable_node_descriptions=[ + ComposableNode( + package="vdb_mapping_ros2", + plugin="vdb_mapping_ros2::vdb_mapping_ros2_component", + name="vdb_mapping", + parameters=[LaunchConfiguration("config")], + ) + ], + output="screen", + ) + + ld.add_action(config_launch_arg) + ld.add_action(container) + + return ld diff --git a/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_ros2/launch/vdb_remote_mapping_ros2.py b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_ros2/launch/vdb_remote_mapping_ros2.py new file mode 100644 index 00000000..30c15203 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_ros2/launch/vdb_remote_mapping_ros2.py @@ -0,0 +1,37 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +from launch_ros.actions import SetParametersFromFile + +def generate_launch_description(): + ld = LaunchDescription() + + config = os.path.join( + get_package_share_directory('vdb_mapping_ros2'), + 'config', + 'vdb_remote_params.yaml' + ) + + print(config) + + container = ComposableNodeContainer( + name='Container', + namespace='', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + ComposableNode( + package='vdb_mapping_ros2', + plugin='vdb_mapping_ros2::vdb_mapping_ros2_component', + name='vdb_remote_mapping', + parameters=[config], + ) + ], + output='screen', + ) + + ld.add_action(container) + + return ld diff --git a/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_ros2/package.xml b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_ros2/package.xml new file mode 100644 index 00000000..881c5f35 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_ros2/package.xml @@ -0,0 +1,32 @@ + + + + vdb_mapping_ros2 + 0.0.1 + The vdb_mapping_ros2 package is a ROS2 wrapper for the vdb_mapping packages used for fast volumetric mapping + Marvin Große Besselmann + Marvin Große Besselmann + + Apache-2.0 + + ament_cmake + + rclcpp + rclcpp_components + sensor_msgs + std_msgs + std_srvs + tf2 + tf2_eigen + tf2_ros + vdb_mapping + vdb_mapping_interfaces + visualization_msgs + pcl_conversions + + + + + ament_cmake + + diff --git a/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_ros2/src/vdb_mapping_ros2_component.cpp b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_ros2/src/vdb_mapping_ros2_component.cpp new file mode 100644 index 00000000..90c8296d --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_ros2/src/vdb_mapping_ros2_component.cpp @@ -0,0 +1,10 @@ +#include "rclcpp/rclcpp.hpp" + +#include "vdb_mapping/OccupancyVDBMapping.h" +#include "vdb_mapping_ros2/VDBMappingROS2.hpp" + +namespace vdb_mapping_ros2 { +using vdb_mapping_ros2_component = VDBMappingROS2; +} +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(vdb_mapping_ros2::vdb_mapping_ros2_component) diff --git a/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_ros2/src/vdb_mapping_ros_node.cpp b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_ros2/src/vdb_mapping_ros_node.cpp new file mode 100644 index 00000000..69bac867 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/vdb_mapping_ros2/src/vdb_mapping_ros_node.cpp @@ -0,0 +1,18 @@ +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" +#include "vdb_mapping/OccupancyVDBMapping.h" +#include "vdb_mapping_ros2/VDBMappingROS2.hpp" + + +int main(int argc, char* argv[]) +{ + rclcpp::init(argc, argv); + std::cout << "well hello there" << std::endl; + std::shared_ptr > vdb_mapping = + std::make_shared >(); + + rclcpp::spin(vdb_mapping); + + rclcpp::shutdown(); + return 0; +} diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/ensemble_planner/CMakeLists.txt b/robot/ros_ws/src/autonomy/4_global/b_planners/ensemble_planner/CMakeLists.txt new file mode 100644 index 00000000..9de81c1b --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/ensemble_planner/CMakeLists.txt @@ -0,0 +1,75 @@ +cmake_minimum_required(VERSION 3.5) +project(ensemble_global_planner) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +# Find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) + +################################### +## ament specific configuration ## +################################### + +ament_package() + +########### +## Build ## +########### + +# Specify additional locations of header files + +# Declare a C++ executable +add_executable(ensemble_global_planner src/ensemble_global_planner_node.cpp) + +# Link libraries +ament_target_dependencies(ensemble_global_planner + rclcpp + rclcpp_action + std_msgs + std_srvs + geometry_msgs + nav_msgs + visualization_msgs + tf2 + tf2_ros +) + +# Install executable +install(TARGETS + ensemble_global_planner + DESTINATION lib/${PROJECT_NAME} +) +# install(DIRECTORY +# launch +# DESTINATION share/${PROJECT_NAME}) + +install(DIRECTORY + config + DESTINATION share/${PROJECT_NAME}) + +install(DIRECTORY + src + DESTINATION share/${PROJECT_NAME}) + + +############# +## Testing ## +############# + +# Add gtest based cpp test target and link libraries +# ament_add_gtest(${PROJECT_NAME}-test test/test_ensemble_global_planner.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}_node) +# endif() \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/ensemble_planner/config/ensemble_global_planner_config.yaml b/robot/ros_ws/src/autonomy/4_global/b_planners/ensemble_planner/config/ensemble_global_planner_config.yaml new file mode 100644 index 00000000..f38ff465 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/ensemble_planner/config/ensemble_global_planner_config.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + srv_random_walk_toggle_topic: "/robot_1/behavior/global_plan_toggle" + way_point_planners: + - name: "random_walk" + config: + frequency: 1.0 #hz + diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/ensemble_planner/include/ensemble_global_planner_node.hpp b/robot/ros_ws/src/autonomy/4_global/b_planners/ensemble_planner/include/ensemble_global_planner_node.hpp new file mode 100644 index 00000000..80ff9b2e --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/ensemble_planner/include/ensemble_global_planner_node.hpp @@ -0,0 +1,58 @@ + +#ifndef RANDOM_WALK_NODE_H +#define RANDOM_WALK_NODE_H + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +class EnsembleGlobalPlannerNode : public rclcpp::Node { + private: + + // String constants + std::string srv_global_plan_toggle_topic_; + + void globalPlannnerToggleCallback(const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response); + + // Other functions + void readParameters(); + + bool enable_global_planner = false; + + public: + // explicit RandomWalkNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + EnsembleGlobalPlannerNode(); + ~EnsembleGlobalPlannerNode() = default; + + // ROS subscribers + rclcpp::Subscription::SharedPtr sub_map; + rclcpp::Subscription::SharedPtr sub_robot_tf; + + // ROS publishers + // rclcpp::Publisher::SharedPtr pub_global_path; + rclcpp::Publisher::SharedPtr pub_goal_point; + rclcpp::Publisher::SharedPtr pub_trajectory_lines; + + // ROS services + rclcpp::Service::SharedPtr srv_global_planner_toggle; + + // ROS timers + rclcpp::TimerBase::SharedPtr timer; +}; + +#endif // RANDOM_WALK_NODE_H diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/ensemble_planner/package.xml b/robot/ros_ws/src/autonomy/4_global/b_planners/ensemble_planner/package.xml new file mode 100644 index 00000000..fafdacbc --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/ensemble_planner/package.xml @@ -0,0 +1,46 @@ + + + ensemble_global_planner + 0.0.0 + Ensemble planner to coordinate different groups of planners + + + + + todo + + + + + + TODO + + + + + + + + + + + + + + ament_cmake + rclcpp + rclcpp_action + std_msgs + std_srvs + base + geometry_msgs + nav_msgs + visualization_msgs + message_generation + tf2 + tf2_ros + + + ament_cmake + + diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/ensemble_planner/src/ensemble_global_planner_node.cpp b/robot/ros_ws/src/autonomy/4_global/b_planners/ensemble_planner/src/ensemble_global_planner_node.cpp new file mode 100644 index 00000000..bf8b10f8 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/ensemble_planner/src/ensemble_global_planner_node.cpp @@ -0,0 +1,41 @@ + +#include "../include/ensemble_global_planner_node.hpp" + +void EnsembleGlobalPlannerNode::readParameters() { + this->declare_parameter("srv_global_plan_toggle_topic"); + if (!this->get_parameter("srv_global_plan_toggle_topic", this->srv_global_plan_toggle_topic_)) { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: srv_global_plan_toggle_topic"); + } +} + +EnsembleGlobalPlannerNode::EnsembleGlobalPlannerNode() : Node("ensemble_global_planner_node") { + // Initialize the Global Planner planner + EnsembleGlobalPlannerNode::readParameters(); + this->srv_global_planner_toggle = this->create_service( + srv_global_plan_toggle_topic_, + std::bind(&EnsembleGlobalPlannerNode::globalPlannnerToggleCallback, this, std::placeholders::_1, + std::placeholders::_2)); +} + +void EnsembleGlobalPlannerNode::globalPlannnerToggleCallback( + const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response) { + if (this->enable_global_planner == false) { + this->enable_global_planner = true; + response->success = true; + response->message = "Global Planner enabled"; + RCLCPP_INFO(this->get_logger(), "Global Planner enabled"); + } else { + this->enable_global_planner = false; + response->success = true; + response->message = "Global Planer disabled"; + RCLCPP_INFO(this->get_logger(), "Global Planner disabled"); + } +} + +int main(int argc, char *argv[]) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/global_planner_bringup/CMakeLists.txt b/robot/ros_ws/src/autonomy/4_global/b_planners/global_planner_bringup/CMakeLists.txt new file mode 100644 index 00000000..50e46d65 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/global_planner_bringup/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.8) +project(global_planner_bringup) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +# Install files. +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) + +ament_package() diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/global_planner_bringup/launch/global_planner.launch.xml b/robot/ros_ws/src/autonomy/4_global/b_planners/global_planner_bringup/launch/global_planner.launch.xml new file mode 100644 index 00000000..939903f8 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/global_planner_bringup/launch/global_planner.launch.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/global_planner_bringup/package.xml b/robot/ros_ws/src/autonomy/4_global/b_planners/global_planner_bringup/package.xml new file mode 100644 index 00000000..1aab4c36 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/global_planner_bringup/package.xml @@ -0,0 +1,18 @@ + + + + global_planner_bringup + 0.0.0 + TODO: Package description + andrew + Apache-2.0 + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/CMakeLists.txt b/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/CMakeLists.txt new file mode 100644 index 00000000..6ba75c20 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/CMakeLists.txt @@ -0,0 +1,76 @@ +cmake_minimum_required(VERSION 3.5) +project(random_walk_planner) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +# Find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) + +################################### +## ament specific configuration ## +################################### + +ament_package() + +########### +## Build ## +########### + +# Specify additional locations of header files + +# Declare a C++ executable +add_executable(random_walk_planner src/random_walk_node.cpp src/random_walk_logic.cpp) + +# Link libraries +ament_target_dependencies(random_walk_planner + rclcpp + rclcpp_action + std_msgs + std_srvs + geometry_msgs + nav_msgs + visualization_msgs + tf2 + tf2_ros +) + +# Install executable +install(TARGETS + random_walk_planner + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}) + +install(DIRECTORY + config + DESTINATION share/${PROJECT_NAME}) + +install(DIRECTORY + src + DESTINATION share/${PROJECT_NAME}) + + +############# +## Testing ## +############# + +# Add gtest based cpp test target and link libraries +# ament_add_gtest(${PROJECT_NAME}-test test/test_random_walk_planner.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}_node) +# endif() \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/config/random_walk_config.yaml b/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/config/random_walk_config.yaml new file mode 100644 index 00000000..b2e059b2 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/config/random_walk_config.yaml @@ -0,0 +1,21 @@ +/**: + ros__parameters: + robot_frame_id: "base_link" + pub_global_plan_topic: "~/global_plan" + pub_goal_point_viz_topic: "~/goal_point_viz" + pub_trajectory_viz_topic: "~/traj_viz" + sub_map_topic: "vdb_map_visualization" + sub_robot_tf_topic: "/tf" + srv_random_walk_toggle_topic: "/robot_1/behavior/global_plan_toggle" + + publish_visualizations: false # should trajectory visualizations be published + num_paths_to_generate: 5 # how many paths to generate + + # Constants + max_start_to_goal_dist_m: 5.0 # how max distance the goal can be from the current robot position + checking_point_cnt: 100 # how many points to collision check between the start and goal points + max_z_change_m: 0.1 # max height that the goal can deviate from the current height + collision_padding_m: 0.1 # how much space should the path have to any obstacles + path_end_threshold_m: 0.1 # how close to the goal will the path be considered complete + max_z_angle_change_rad: 1.5708 # how much the goal can deviate from the current orientation + diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/include/random_walk_logic.hpp b/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/include/random_walk_logic.hpp new file mode 100644 index 00000000..345d390a --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/include/random_walk_logic.hpp @@ -0,0 +1,70 @@ + +#ifndef RANDOM_WALK_LOGIC_H +#define RANDOM_WALK_LOGIC_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +typedef std::vector> Path; // x, y, z, yaw + +struct init_params { + float max_start_to_goal_dist_m; + int checking_point_cnt; + float max_z_change_m; + float collision_padding_m; + float path_end_threshold_m; + float max_z_angle_change_rad; + std::tuple voxel_size_m; +}; + +class RandomWalkPlanner { + public: + RandomWalkPlanner() = default; + RandomWalkPlanner(init_params params); + ~RandomWalkPlanner() = default; + + std::optional generate_straight_rand_path( + std::tuple start_point, float timeout_duration); // x, y, z, yaw + + float path_end_threshold_m; + + std::vector> voxel_points; + + private: + // Numerical constants + float max_start_to_goal_dist_m_; + int checking_point_cnt; + float max_z_change_m_; + float collision_padding_m; + float max_z_angle_change_rad; + + // Variables + std::tuple voxel_size_m; + + std::mutex mutex; + + // Functions + bool check_if_collided_single_voxel(const std::tuple& point, + const std::tuple& voxel_center); + + bool check_if_collided(const std::tuple& point); + + std::tuple generate_goal_point( + std::tuple start_point); +}; + +double get_point_distance(const std::tuple& point1, + const std::tuple& point2); + +double deg2rad(double deg); + +double rad2deg(double rad); + +#endif // RANDOM_WALK_LOGIC_H diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/include/random_walk_node.hpp b/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/include/random_walk_node.hpp new file mode 100644 index 00000000..d6baf12f --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/include/random_walk_node.hpp @@ -0,0 +1,99 @@ + +#ifndef RANDOM_WALK_NODE_H +#define RANDOM_WALK_NODE_H + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "random_walk_logic.hpp" +#include "rclcpp/rclcpp.hpp" + +class RandomWalkNode : public rclcpp::Node { + private: + // Planner + // RandomWalkPlanner random_walk_planner; + std::unique_ptr random_walk_planner; + + // String constants + std::string world_frame_id_; + std::string robot_frame_id_; + std::string pub_global_plan_topic_; + std::string pub_goal_point_viz_topic_; + std::string pub_trajectory_viz_topic_; + std::string sub_map_topic_; + std::string sub_robot_tf_topic_; + std::string srv_random_walk_toggle_topic_; + + // Variables + init_params params; + // nav_msgs::msg::Path generated_path; + int num_paths_to_generate_; + std::vector generated_paths; + bool publish_visualizations = false; + bool received_first_map = false; + bool received_first_robot_tf = false; + bool enable_random_walk = false; + bool is_path_executing = false; + + geometry_msgs::msg::Transform current_location; // x, y, z, yaw + geometry_msgs::msg::Transform current_goal_location; // x, y, z, yaw + + // Callbacks + void mapCallback(const visualization_msgs::msg::Marker::SharedPtr msg); + + void tfCallback(const tf2_msgs::msg::TFMessage::SharedPtr msg); + + void randomWalkToggleCallback(const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response); + + void timerCallback(); + + void generate_plan(); + + void publish_plan(); + + // Other functions + std::optional readParameters(); + + public: + // explicit RandomWalkNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + RandomWalkNode(); + ~RandomWalkNode() = default; + + // TF buffer + std::shared_ptr tf_buffer; + std::shared_ptr tf_listener; + + // ROS subscribers + rclcpp::Subscription::SharedPtr sub_map; + // rclcpp::Subscription::SharedPtr sub_robot_tf; + + // ROS publishers + rclcpp::Publisher::SharedPtr pub_global_plan; + rclcpp::Publisher::SharedPtr pub_goal_point; + rclcpp::Publisher::SharedPtr pub_trajectory_lines; + + // ROS services + rclcpp::Service::SharedPtr srv_random_walk_toggle; + + // ROS timers + rclcpp::TimerBase::SharedPtr timer; +}; + +#endif // RANDOM_WALK_NODE_H diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/index.html b/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/index.html new file mode 100644 index 00000000..3aade784 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/index.html @@ -0,0 +1,2669 @@ + + + + + + + + + + + + + + + + + + + + + + + Random Walk Global Planner Baseline - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

Random Walk Global Planner Baseline

+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ + + +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/launch/random_walk_launch.xml b/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/launch/random_walk_launch.xml new file mode 100644 index 00000000..174bc53f --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/launch/random_walk_launch.xml @@ -0,0 +1,11 @@ + + + + + + \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/package.xml b/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/package.xml new file mode 100644 index 00000000..019be21c --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/package.xml @@ -0,0 +1,46 @@ + + + random_walk_planner + 0.0.0 + The random walk planner package + + + + + todo + + + + + + TODO + + + + + + + + + + + + + + ament_cmake + rclcpp + rclcpp_action + std_msgs + std_srvs + base + geometry_msgs + nav_msgs + visualization_msgs + message_generation + tf2 + tf2_ros + + + ament_cmake + + diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/src/random_walk_logic.cpp b/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/src/random_walk_logic.cpp new file mode 100644 index 00000000..c7299f22 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/src/random_walk_logic.cpp @@ -0,0 +1,186 @@ + +#include "../include/random_walk_logic.hpp" + +#include "rclcpp/rclcpp.hpp" + +RandomWalkPlanner::RandomWalkPlanner(init_params params) +{ + this->max_start_to_goal_dist_m_ = params.max_start_to_goal_dist_m; + this->checking_point_cnt = params.checking_point_cnt; + this->max_z_change_m_ = params.max_z_change_m; + this->voxel_size_m = params.voxel_size_m; + this->collision_padding_m = params.collision_padding_m; + this->path_end_threshold_m = params.path_end_threshold_m; + this->max_z_angle_change_rad = params.max_z_angle_change_rad; +} + +std::optional RandomWalkPlanner::generate_straight_rand_path( + std::tuple start_point, float timeout_duration) +{ + // Locking mutex to prevent crashing when access the voxel map + std::optional path; + bool is_goal_point_valid = false; + // get start ti + const std::chrono::steady_clock::time_point start_time = std::chrono::steady_clock::now(); + // RCLCPP_INFO(rclcpp::get_logger("random_walk_planner"), "Starting Path Search..."); + + while (!is_goal_point_valid && std::chrono::duration_cast( + std::chrono::steady_clock::now() - start_time) + .count() < timeout_duration) + { + std::tuple goal_point = generate_goal_point(start_point); + // RCLCPP_INFO(rclcpp::get_logger("random_walk_planner"), "Point Generated..."); + if (std::get<2>(goal_point) == -1) + { + RCLCPP_INFO(rclcpp::get_logger("random_walk_planner"), + "No valid goal point found in timeout duration"); + break; + } + + float x_diff = std::get<0>(goal_point) - std::get<0>(start_point); + float y_diff = std::get<1>(goal_point) - std::get<1>(start_point); + float z_diff = std::get<2>(goal_point) - std::get<2>(start_point); + float yaw = std::atan2(y_diff, x_diff); + + path = Path(); + std::tuple first_point( + std::get<0>(start_point), std::get<1>(start_point), std::get<2>(start_point), yaw); + path.value().push_back(first_point); + + std::tuple current_point = std::make_tuple( + std::get<0>(start_point), std::get<1>(start_point), std::get<2>(start_point)); + + // start moving in the direction of the goal point + while (get_point_distance(current_point, goal_point) > 0.001) + { + if (!check_if_collided(current_point)) + { + // add small delay to stop crashing + float new_x = std::get<0>(current_point) + x_diff / float(this->checking_point_cnt); + float new_y = std::get<1>(current_point) + y_diff / float(this->checking_point_cnt); + float new_z = std::get<2>(current_point) + z_diff / float(this->checking_point_cnt); + std::tuple new_point(new_x, new_y, new_z); + current_point = new_point; + std::tuple new_point_with_yaw( + std::get<0>(new_point), std::get<1>(new_point), std::get<2>(new_point), yaw); + path.value().push_back(new_point_with_yaw); + } + else + { + RCLCPP_INFO(rclcpp::get_logger("random_walk_planner"), "Collision detected"); + is_goal_point_valid = false; + break; + } + is_goal_point_valid = true; + } + } + if (!is_goal_point_valid) + { + RCLCPP_INFO(rclcpp::get_logger("random_walk_planner"), "No Path found in time limit"); + return std::nullopt; + } + else + { + return path; + } +} + +bool RandomWalkPlanner::check_if_collided_single_voxel( + const std::tuple& point, const std::tuple& voxel_center) +{ + // Check if the point is within the voxel + float x_max = std::get<0>(voxel_center) + std::get<0>(this->voxel_size_m) + this->collision_padding_m; + float x_min = std::get<0>(voxel_center) - std::get<0>(this->voxel_size_m) - this->collision_padding_m; + float y_max = std::get<1>(voxel_center) + std::get<1>(this->voxel_size_m) + this->collision_padding_m; + float y_min = std::get<1>(voxel_center) - std::get<1>(this->voxel_size_m) - this->collision_padding_m; + float z_max = std::get<2>(voxel_center) + std::get<2>(this->voxel_size_m) + this->collision_padding_m; + float z_min = std::get<2>(voxel_center) - std::get<2>(this->voxel_size_m) - this->collision_padding_m; + bool in_x = std::get<0>(point) < x_max && std::get<0>(point) > x_min; + bool in_y = std::get<1>(point) < y_max && std::get<1>(point) > y_min; + bool in_z = std::get<2>(point) < z_max && std::get<2>(point) > z_min; + if (in_x && in_y && in_z) + { + return true; + } + else + { + return false; + } +} + +bool RandomWalkPlanner::check_if_collided(const std::tuple& point) +{ + std::lock_guard lock(this->mutex); + for (const std::tuple& voxel : this->voxel_points) + { + if (voxel == std::tuple(0, 0, 0)) + { + RCLCPP_INFO(rclcpp::get_logger("random_walk_planner"), "Voxel is 0,0,0"); + } + if (check_if_collided_single_voxel(point, voxel)) + { + return true; + } + } + return false; +} + +std::tuple RandomWalkPlanner::generate_goal_point( + std::tuple start_point) +{ + float time_out_duration = 1.0; + const clock_t start_time = clock(); + + while ((clock() - start_time) / CLOCKS_PER_SEC < time_out_duration) + { + std::random_device rd; + std::mt19937 gen(rd()); + std::uniform_real_distribution distribution_xy(-this->max_start_to_goal_dist_m_, + this->max_start_to_goal_dist_m_); + std::uniform_real_distribution distribution_z(-this->max_z_change_m_, this->max_z_change_m_); + float delta_x = distribution_xy(gen); + float delta_y = distribution_xy(gen); + float delta_z = distribution_z(gen); + float rand_x = std::get<0>(start_point) + delta_x; + float rand_y = std::get<1>(start_point) + delta_y; + float rand_z = std::get<2>(start_point) + delta_z; + std::tuple rand_point(rand_x, rand_y, rand_z); + std::tuple start_point_wo_yaw( + std::get<0>(start_point), std::get<1>(start_point), std::get<2>(start_point)); + float dist = get_point_distance(start_point_wo_yaw, rand_point); + float new_angle = std::atan2(std::get<1>(rand_point) - std::get<1>(start_point_wo_yaw), + std::get<0>(rand_point) - std::get<0>(start_point_wo_yaw)); + float angle_diff = std::abs(std::get<3>(start_point) - new_angle); + // if the z value of the point is low enough + if (angle_diff < this->max_z_angle_change_rad) + { + // if the point is close enough to the start point + if (dist < this->max_start_to_goal_dist_m_) + { + // if the point doesnt collide with any of the voxels + if (!(check_if_collided(rand_point))) + { + return rand_point; + } + } + } + } + return std::tuple(0, 0, -1); +} + +double get_point_distance(const std::tuple& point1, + const std::tuple& point2) +{ + float x_diff = std::get<0>(point1) - std::get<0>(point2); + float y_diff = std::get<1>(point1) - std::get<1>(point2); + float z_diff = std::get<2>(point1) - std::get<2>(point2); + float dist = std::sqrt(x_diff * x_diff + y_diff * y_diff + z_diff * z_diff); + if (std::isnan(dist) || std::isinf(dist)) { + RCLCPP_ERROR(rclcpp::get_logger("random_walk_planner"), "Distance is nan or inf"); + } + return dist; +} + +double deg2rad(double deg) { return deg * M_PI / 180.0; } + +double rad2deg(double rad) { return rad * 180.0 / M_PI; } diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/src/random_walk_node.cpp b/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/src/random_walk_node.cpp new file mode 100644 index 00000000..c89f2ef9 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/src/random_walk_node.cpp @@ -0,0 +1,306 @@ + +#include "../include/random_walk_node.hpp" + +#include "../include/random_walk_logic.hpp" + +std::optional RandomWalkNode::readParameters() { + // Read in parameters based off the default yaml file + init_params params; + this->declare_parameter("robot_frame_id"); + if (!this->get_parameter("robot_frame_id", this->robot_frame_id_)) { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: robot_frame_id"); + return std::optional{}; + } + this->declare_parameter("pub_global_plan_topic"); + if (!this->get_parameter("pub_global_plan_topic", this->pub_global_plan_topic_)) { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: pub_global_plan_topic"); + return std::optional{}; + } + this->declare_parameter("pub_goal_point_viz_topic"); + if (!this->get_parameter("pub_goal_point_viz_topic", this->pub_goal_point_viz_topic_)) { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: pub_goal_point_viz_topic"); + return std::optional{}; + } + this->declare_parameter("pub_trajectory_viz_topic"); + if (!this->get_parameter("pub_trajectory_viz_topic", this->pub_trajectory_viz_topic_)) { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: pub_trajectory_viz_topic"); + return std::optional{}; + } + this->declare_parameter("sub_map_topic"); + if (!this->get_parameter("sub_map_topic", this->sub_map_topic_)) { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: sub_map_topic"); + return std::optional{}; + } + this->declare_parameter("sub_robot_tf_topic"); + if (!this->get_parameter("sub_robot_tf_topic", this->sub_robot_tf_topic_)) { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: sub_robot_tf_topic"); + return std::optional{}; + } + this->declare_parameter("srv_random_walk_toggle_topic"); + if (!this->get_parameter("srv_random_walk_toggle_topic", this->srv_random_walk_toggle_topic_)) { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: srv_random_walk_toggle_topic"); + return std::optional{}; + } + this->declare_parameter("publish_visualizations"); + if (!this->get_parameter("publish_visualizations", this->publish_visualizations)) { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: publish_visualizations"); + return std::optional{}; + } + this->declare_parameter("num_paths_to_generate"); + if (!this->get_parameter("num_paths_to_generate", this->num_paths_to_generate_)) { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: num_paths_to_generate"); + return std::optional{}; + } + this->declare_parameter("max_start_to_goal_dist_m"); + if (!this->get_parameter("max_start_to_goal_dist_m", params.max_start_to_goal_dist_m)) { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: max_start_to_goal_dist_m"); + return std::optional{}; + } + this->declare_parameter("checking_point_cnt"); + if (!this->get_parameter("checking_point_cnt", params.checking_point_cnt)) { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: checking_point_cnt"); + return std::optional{}; + } + this->declare_parameter("max_z_change_m"); + if (!this->get_parameter("max_z_change_m", params.max_z_change_m)) { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: max_z_change_m"); + return std::optional{}; + } + this->declare_parameter("collision_padding_m"); + if (!this->get_parameter("collision_padding_m", params.collision_padding_m)) { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: collision_padding_m"); + return std::optional{}; + } + this->declare_parameter("path_end_threshold_m"); + if (!this->get_parameter("path_end_threshold_m", params.path_end_threshold_m)) { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: path_end_threshold_m"); + return std::optional{}; + } + this->declare_parameter("max_z_angle_change_rad"); + if (!this->get_parameter("max_z_angle_change_rad", params.max_z_angle_change_rad)) { + RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: max_z_angle_change_rad"); + return std::optional{}; + } + return params; +} + +RandomWalkNode::RandomWalkNode() : Node("random_walk_node") { + // Initialize the random walk planner + std::optional params_opt = RandomWalkNode::readParameters(); + if (params_opt.has_value()) { + this->params = params_opt.value(); + } else + RCLCPP_ERROR(this->get_logger(), "Failed to initialize random walk planner"); + + this->sub_map = this->create_subscription( + sub_map_topic_, 10, std::bind(&RandomWalkNode::mapCallback, this, std::placeholders::_1)); + + //TF buffer and listener + tf_buffer = std::make_shared(this->get_clock()); + tf_listener = std::make_shared(*tf_buffer); + + this->pub_global_plan = this->create_publisher(pub_global_plan_topic_, 10); + this->pub_goal_point = + this->create_publisher(pub_goal_point_viz_topic_, 10); + this->pub_trajectory_lines = + this->create_publisher(pub_trajectory_viz_topic_, 10); + + // Set up the timer + this->timer = this->create_wall_timer(std::chrono::seconds(1), + std::bind(&RandomWalkNode::timerCallback, this)); + // Set up the service + this->srv_random_walk_toggle = this->create_service( + srv_random_walk_toggle_topic_, std::bind(&RandomWalkNode::randomWalkToggleCallback, this, + std::placeholders::_1, std::placeholders::_2)); + RCLCPP_INFO(this->get_logger(), "Random walk node initialized"); +} + +void RandomWalkNode::randomWalkToggleCallback( + const std_srvs::srv::Trigger::Request::SharedPtr request, + std_srvs::srv::Trigger::Response::SharedPtr response) { + if (this->enable_random_walk == false) { + this->enable_random_walk = true; + response->success = true; + response->message = "Random walk enabled"; + RCLCPP_INFO(this->get_logger(), "Random walk enabled"); + } else { + this->enable_random_walk = false; + response->success = true; + response->message = "Random walk disabled"; + RCLCPP_INFO(this->get_logger(), "Random walk disabled"); + } +} + +void RandomWalkNode::mapCallback(const visualization_msgs::msg::Marker::SharedPtr msg) { + // updating the local voxel points and generating a path only if the path is not executing + if (!this->received_first_map) { + this->received_first_map = true; + this->world_frame_id_ = msg->header.frame_id; + RCLCPP_INFO(this->get_logger(), "Received first map"); + this->params.voxel_size_m = + std::tuple(msg->scale.x, msg->scale.y, msg->scale.z); + // this->random_walk_planner = RandomWalkPlanner(this->params); + this->random_walk_planner = std::make_unique(this->params); + + RCLCPP_INFO(this->get_logger(), "Initialized random walk planner logic"); + } + this->random_walk_planner->voxel_points.clear(); + for (int i = 0; i < msg->points.size(); i++) { + this->random_walk_planner->voxel_points.push_back( + std::tuple(msg->points[i].x, msg->points[i].y, msg->points[i].z)); + } +} + +void RandomWalkNode::tfCallback(const tf2_msgs::msg::TFMessage::SharedPtr msg) { + // get the current location + for (int i = 0; i < msg->transforms.size(); i++) { + if (msg->transforms[i].child_frame_id.c_str() == this->robot_frame_id_) { + this->current_location = msg->transforms[i].transform; + if (!this->received_first_robot_tf) { + this->received_first_robot_tf = true; + RCLCPP_INFO(this->get_logger(), "Received first robot_tf"); + } + } + } +} + +void RandomWalkNode::generate_plan() { + RCLCPP_INFO(this->get_logger(), "Starting to generate plan..."); + + std::tuple start_loc; + if (this->generated_paths.size() == 0) { + geometry_msgs::msg::Quaternion orientation = this->current_location.rotation; + tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w); + q.normalize(); + double roll, pitch, yaw; + tf2::Matrix3x3 m(q); + m.getRPY(roll, pitch, yaw); + start_loc = std::make_tuple(this->current_location.translation.x, + this->current_location.translation.y, + this->current_location.translation.z, yaw); + } else { + geometry_msgs::msg::Quaternion orientation = + this->generated_paths.back().poses.back().pose.orientation; + tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w); + q.normalize(); + double roll, pitch, yaw; + tf2::Matrix3x3 m(q); + m.getRPY(roll, pitch, yaw); + start_loc = std::make_tuple(this->generated_paths.back().poses.back().pose.position.x, + this->generated_paths.back().poses.back().pose.position.y, + this->generated_paths.back().poses.back().pose.position.z, yaw); + } + + float timeout_duration = 5.0; + std::optional gen_path_opt = + this->random_walk_planner->generate_straight_rand_path(start_loc, timeout_duration); + if (gen_path_opt.has_value() && gen_path_opt.value().size() > 0) { + RCLCPP_INFO(this->get_logger(), "Generated path with %ld points", + gen_path_opt.value().size()); + + // set the current goal location + this->current_goal_location = geometry_msgs::msg::Transform(); + this->current_goal_location.translation.x = std::get<0>(gen_path_opt.value().back()); + this->current_goal_location.translation.y = std::get<1>(gen_path_opt.value().back()); + this->current_goal_location.translation.z = std::get<2>(gen_path_opt.value().back()); + float z_rot = std::get<3>(gen_path_opt.value().back()); + tf2::Quaternion q; + q.setRPY(0, 0, z_rot); // Roll = 0, Pitch = 0, Yaw = yaw + q.normalize(); + this->current_goal_location.rotation.x = q.x(); + this->current_goal_location.rotation.y = q.y(); + this->current_goal_location.rotation.z = q.z(); + this->current_goal_location.rotation.w = q.w(); + + // publish the path + nav_msgs::msg::Path generated_single_path; + generated_single_path = nav_msgs::msg::Path(); + generated_single_path.header.stamp = this->now(); + generated_single_path.header.frame_id = world_frame_id_; + for (auto point : gen_path_opt.value()) { + geometry_msgs::msg::PoseStamped point_msg; + point_msg.pose.position.x = std::get<0>(point); + point_msg.pose.position.y = std::get<1>(point); + point_msg.pose.position.z = std::get<2>(point); + // convert yaw rotation to quaternion + tf2::Quaternion q; + q.setRPY(0, 0, std::get<3>(point)); // Roll = 0, Pitch = 0, Yaw = yaw + q.normalize(); + point_msg.pose.orientation.x = q.x(); + point_msg.pose.orientation.y = q.y(); + point_msg.pose.orientation.z = q.z(); + point_msg.pose.orientation.w = q.w(); + point_msg.header.stamp = this->now(); + generated_single_path.poses.push_back(point_msg); + } + geometry_msgs::msg::PoseStamped last_goal_loc = generated_single_path.poses.back(); + this->current_goal_location.translation.x = last_goal_loc.pose.position.x; + this->current_goal_location.translation.y = last_goal_loc.pose.position.y; + this->current_goal_location.translation.z = last_goal_loc.pose.position.z; + this->current_goal_location.rotation.z = last_goal_loc.pose.orientation.z; + this->generated_paths.push_back(generated_single_path); + + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to generate path, size was 0"); + } +} + +void RandomWalkNode::publish_plan() { + nav_msgs::msg::Path full_path; + for (auto path : this->generated_paths) { + for (auto point : path.poses) { + full_path.poses.push_back(point); + } + } + full_path.header.stamp = this->now(); + full_path.header.frame_id = this->world_frame_id_; + this->pub_global_plan->publish(full_path); + RCLCPP_INFO(this->get_logger(), "Published full path"); +} +void RandomWalkNode::timerCallback() { + // get current TF to world + try { + geometry_msgs::msg::TransformStamped transform_stamped = + this->tf_buffer->lookupTransform(this->world_frame_id_, this->robot_frame_id_, + rclcpp::Time(0)); + this->current_location = transform_stamped.transform; + if (!this->received_first_robot_tf) { + this->received_first_robot_tf = true; + RCLCPP_INFO(this->get_logger(), "Received first robot_tf"); + } + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR(this->get_logger(), "Could not get robot tf: %s", ex.what()); + } + + if (this->enable_random_walk && !this->is_path_executing) { + if (this->received_first_map && this->received_first_robot_tf) { + for (int i = 0; i < this->num_paths_to_generate_; i++) { + this->generate_plan(); + } + this->publish_plan(); + this->is_path_executing = true; + } else { + RCLCPP_INFO(this->get_logger(), "Waiting for map and robot tf to be received..."); + } + } else if (this->enable_random_walk && this->is_path_executing) { + std::tuple current_point = std::make_tuple( + this->current_location.translation.x, this->current_location.translation.y, + this->current_location.translation.z); + std::tuple goal_point = std::make_tuple( + this->current_goal_location.translation.x, this->current_goal_location.translation.y, + this->current_goal_location.translation.z); + if (get_point_distance(current_point, goal_point) < + this->random_walk_planner->path_end_threshold_m) { + this->is_path_executing = false; + this->generated_paths.clear(); + RCLCPP_INFO(this->get_logger(), "Reached goal point"); + } + } +} + +int main(int argc, char *argv[]) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/4_global/global_bringup/CMakeLists.txt b/robot/ros_ws/src/autonomy/4_global/global_bringup/CMakeLists.txt new file mode 100644 index 00000000..77d07941 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/global_bringup/CMakeLists.txt @@ -0,0 +1,32 @@ +cmake_minimum_required(VERSION 3.8) +project(global_bringup) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +# Install files. +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +# install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) +# install(DIRECTORY params DESTINATION share/${PROJECT_NAME}) + +ament_package() diff --git a/robot/ros_ws/src/autonomy/4_global/global_bringup/LICENSE b/robot/ros_ws/src/autonomy/4_global/global_bringup/LICENSE new file mode 100644 index 00000000..d6456956 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/global_bringup/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/robot/ros_ws/src/autonomy/4_global/global_bringup/config/vdb_params.yaml b/robot/ros_ws/src/autonomy/4_global/global_bringup/config/vdb_params.yaml new file mode 100644 index 00000000..dc9cfe09 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/global_bringup/config/vdb_params.yaml @@ -0,0 +1,44 @@ +/**: + ros__parameters: + # Basic setup + map_frame: map + robot_frame: base_link + max_range: 10.0 + resolution: 0.07 + prob_hit: 0.99 + prob_miss: 0.1 + thres_min: 0.49 + thres_max: 0.51 + map_save_dir: "" + + # Visualizations + publish_pointcloud: true + publish_vis_marker: true + visualization_rate: 2.0 + + # Sensor input + accumulate_updates: true + accumulation_period: 0.2 + + apply_raw_sensor_data: true + sources: [ouster] + ouster: + topic: sensors/ouster/point_cloud + sensor_origin_frame: ouster + + # Remote mapping + publish_updates: true + publish_overwrites: true + publish_sections: true + section_update: + rate: 1.0 + min_coord: + x: -10.0 + y: -10.0 + z: -10.0 + max_coord: + x: 10.0 + y: 10.0 + z: 10.0 + + diff --git a/robot/ros_ws/src/autonomy/4_global/global_bringup/launch/global.launch.xml b/robot/ros_ws/src/autonomy/4_global/global_bringup/launch/global.launch.xml new file mode 100644 index 00000000..69544b86 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/global_bringup/launch/global.launch.xml @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/4_global/global_bringup/package.xml b/robot/ros_ws/src/autonomy/4_global/global_bringup/package.xml new file mode 100644 index 00000000..a7588894 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/global_bringup/package.xml @@ -0,0 +1,18 @@ + + + + global_bringup + 0.0.0 + TODO: Package description + andrew + Apache-2.0 + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/robot/ros_ws/src/autonomy/4_global/waypoint_interface/CMakeLists.txt b/robot/ros_ws/src/autonomy/4_global/waypoint_interface/CMakeLists.txt new file mode 100644 index 00000000..3d389fde --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/waypoint_interface/CMakeLists.txt @@ -0,0 +1,41 @@ +cmake_minimum_required(VERSION 3.8) +project(waypoint_interface) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) + +add_executable(waypoint_interface_node src/waypoint_interface_node.cpp) + +# target_link_libraries(waypoint_interface_node rclcpp nav_msgs geometry_msgs) + +ament_target_dependencies(waypoint_interface_node + rclcpp + std_msgs + geometry_msgs + nav_msgs +) + +install(TARGETS + waypoint_interface_node + DESTINATION lib/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/robot/ros_ws/src/autonomy/4_global/waypoint_interface/package.xml b/robot/ros_ws/src/autonomy/4_global/waypoint_interface/package.xml new file mode 100644 index 00000000..9ea09c96 --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/waypoint_interface/package.xml @@ -0,0 +1,22 @@ + + + + waypoint_interface + 0.0.0 + TODO: Package description + airstation-04 + TODO: License declaration + + ament_cmake + + rclcpp + nav_msgs + geometry_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/robot/ros_ws/src/autonomy/4_global/waypoint_interface/src/waypoint_interface_node.cpp b/robot/ros_ws/src/autonomy/4_global/waypoint_interface/src/waypoint_interface_node.cpp new file mode 100644 index 00000000..02544ecf --- /dev/null +++ b/robot/ros_ws/src/autonomy/4_global/waypoint_interface/src/waypoint_interface_node.cpp @@ -0,0 +1,119 @@ +#include +#include + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav_msgs/msg/path.hpp" +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/int32.hpp" + +using std::placeholders::_1; + +class WaypointInterfaceNode : public rclcpp::Node { + public: + WaypointInterfaceNode() : Node("waypoint_interface") { + this->declare_parameter("lookahead_time", 5.0); + lookahead_time_ = this->get_parameter("lookahead_time").as_double(); + + subscription_ref_ = this->create_subscription( + "global_plan_reference", 10, + std::bind(&WaypointInterfaceNode::global_plan_reference_callback, this, _1)); + subscription_eta_ = this->create_subscription( + "global_plan_eta", 10, + std::bind(&WaypointInterfaceNode::global_plan_eta_callback, this, _1)); + + // Publisher for the current waypoint index + waypoint_index_publisher_ = + this->create_publisher("current_waypoint_index", 10); + + timer_ = this->create_wall_timer(std::chrono::seconds(1), + std::bind(&WaypointInterfaceNode::update_position, this)); + } + + private: + void global_plan_reference_callback(const nav_msgs::msg::Path::SharedPtr msg) { + global_plan_reference_ = msg->poses; + RCLCPP_INFO(this->get_logger(), "Received global plan reference with sparse waypoints."); + } + + void global_plan_eta_callback(const nav_msgs::msg::Path::SharedPtr msg) { + global_plan_eta_ = msg->poses; + RCLCPP_INFO(this->get_logger(), "Received global plan eta with dense waypoints."); + } + + void update_position() { + this->get_parameter("lookahead_time", lookahead_time_); + RCLCPP_INFO(this->get_logger(), "Checking waypoints within lookahead time = %f", + lookahead_time_); + track_waypoints(lookahead_time_); + } + + void track_waypoints(double lookahead_time) { + if (global_plan_reference_.empty() || global_plan_eta_.empty()) { + RCLCPP_WARN(this->get_logger(), "Global plans not yet received."); + return; + } + + rclcpp::Time current_time = this->get_clock()->now(); + rclcpp::Time target_time = current_time + rclcpp::Duration::from_seconds(lookahead_time); + + int last_close_waypoint_idx = -1; + + for (const auto &pose_eta : global_plan_eta_) { + if (rclcpp::Time(pose_eta.header.stamp) > target_time) { + break; + } + + for (size_t i = 0; i < global_plan_reference_.size(); ++i) { + if (is_close(pose_eta.pose.position, global_plan_reference_[i].pose.position)) { + last_close_waypoint_idx = i; + } + } + } + + std_msgs::msg::Int32 waypoint_index_msg; + + if (last_close_waypoint_idx == -1 || + last_close_waypoint_idx >= static_cast(global_plan_reference_.size()) - 1) { + RCLCPP_INFO(this->get_logger(), + "Robot is not within close range of any reference waypoints."); + waypoint_index_msg.data = -1; // No waypoint in range + } else { + RCLCPP_INFO(this->get_logger(), "Robot is traveling between waypoints %d and %d.", + last_close_waypoint_idx, last_close_waypoint_idx + 1); + waypoint_index_msg.data = last_close_waypoint_idx; + } + + // Publish the current waypoint index + waypoint_index_publisher_->publish(waypoint_index_msg); + } + + bool is_close(const geometry_msgs::msg::Point &p1, const geometry_msgs::msg::Point &p2) const { + double dist = distance(p1, p2); + return dist < close_range_threshold_; + } + + double distance(const geometry_msgs::msg::Point &p1, + const geometry_msgs::msg::Point &p2) const { + return std::sqrt(std::pow(p1.x - p2.x, 2) + std::pow(p1.y - p2.y, 2) + + std::pow(p1.z - p2.z, 2)); + } + + rclcpp::Subscription::SharedPtr subscription_ref_; + rclcpp::Subscription::SharedPtr subscription_eta_; + rclcpp::Publisher::SharedPtr waypoint_index_publisher_; + rclcpp::TimerBase::SharedPtr timer_; + + std::vector global_plan_reference_; + std::vector global_plan_eta_; + + double lookahead_time_; + const double close_range_threshold_ = 0.5; // Distance threshold for "close range" in meters +}; + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_bringup/CMakeLists.txt b/robot/ros_ws/src/autonomy/5_behavior/behavior_bringup/CMakeLists.txt new file mode 100644 index 00000000..b62906b6 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_bringup/CMakeLists.txt @@ -0,0 +1,32 @@ +cmake_minimum_required(VERSION 3.8) +project(behavior_bringup) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +# Install files. +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +# install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME}) +# install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) +# install(DIRECTORY params DESTINATION share/${PROJECT_NAME}) + +ament_package() diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_bringup/LICENSE b/robot/ros_ws/src/autonomy/5_behavior/behavior_bringup/LICENSE new file mode 100644 index 00000000..d6456956 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_bringup/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_bringup/launch/behavior.launch.xml b/robot/ros_ws/src/autonomy/5_behavior/behavior_bringup/launch/behavior.launch.xml new file mode 100644 index 00000000..4fac12e5 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_bringup/launch/behavior.launch.xml @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_bringup/package.xml b/robot/ros_ws/src/autonomy/5_behavior/behavior_bringup/package.xml new file mode 100644 index 00000000..fbdc2cd6 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_bringup/package.xml @@ -0,0 +1,18 @@ + + + + behavior_bringup + 0.0.0 + TODO: Package description + andrew + Apache-2.0 + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_executive/CMakeLists.txt b/robot/ros_ws/src/autonomy/5_behavior/behavior_executive/CMakeLists.txt new file mode 100644 index 00000000..c3bba336 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_executive/CMakeLists.txt @@ -0,0 +1,54 @@ +cmake_minimum_required(VERSION 3.8) +project(behavior_executive) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(behavior_tree REQUIRED) +find_package(behavior_tree_msgs REQUIRED) +find_package(airstack_msgs REQUIRED) +find_package(airstack_common REQUIRED) +find_package(mavros_msgs REQUIRED) + +add_executable(behavior_executive src/behavior_executive.cpp) +target_include_directories(behavior_executive PUBLIC + $ + $) +target_compile_features(behavior_executive PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +ament_target_dependencies(behavior_executive + rclcpp + rclcpp_action + std_msgs + std_srvs + nav_msgs + behavior_tree + behavior_tree_msgs + airstack_msgs + airstack_common + mavros_msgs +) + +install(TARGETS behavior_executive + DESTINATION lib/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_executive/include/behavior_executive/behavior_executive.hpp b/robot/ros_ws/src/autonomy/5_behavior/behavior_executive/include/behavior_executive/behavior_executive.hpp new file mode 100644 index 00000000..8d1f6aa0 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_executive/include/behavior_executive/behavior_executive.hpp @@ -0,0 +1,82 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rclcpp_action/rclcpp_action.hpp" + +class BehaviorExecutive : public rclcpp::Node { +private: + // parameters + bool ascent_takeoff; + + // variables + std::string takeoff_state, landing_state; + + // Condition variables + bt::Condition* auto_takeoff_commanded_condition; + bt::Condition* takeoff_commanded_condition; + bt::Condition* armed_condition; + bt::Condition* offboard_mode_condition; + bt::Condition* stationary_condition; + bt::Condition* land_commanded_condition; + bt::Condition* pause_commanded_condition; + bt::Condition* rewind_commanded_condition; + bt::Condition* fixed_trajectory_condition; + bt::Condition* global_plan_condition; + bt::Condition* offboard_commanded_condition; + bt::Condition* arm_commanded_condition; + bt::Condition* disarm_commanded_condition; + bt::Condition* takeoff_complete_condition; + bt::Condition* landing_complete_condition; + std::vector conditions; + + // Action variables + bt::Action* arm_action; + bt::Action* takeoff_action; + bt::Action* land_action; + bt::Action* pause_action; + bt::Action* rewind_action; + bt::Action* follow_fixed_trajectory_action; + bt::Action* global_plan_action; + bt::Action* request_control_action; + bt::Action* disarm_action; + std::vector actions; + + // subscribers + rclcpp::Subscription::SharedPtr behavior_tree_commands_sub; + rclcpp::Subscription::SharedPtr is_armed_sub; + rclcpp::Subscription::SharedPtr has_control_sub; + rclcpp::Subscription::SharedPtr takeoff_state_sub; + rclcpp::Subscription::SharedPtr landing_state_sub; + + // publishers + + // services + rclcpp::CallbackGroup::SharedPtr service_callback_group; + rclcpp::Client::SharedPtr robot_command_client; + rclcpp::Client::SharedPtr trajectory_mode_client; + rclcpp::Client::SharedPtr takeoff_landing_command_client; + rclcpp::Client::SharedPtr global_planner_toggle_client; + + // timers + rclcpp::TimerBase::SharedPtr timer; + + // callbacks + void timer_callback(); + void bt_commands_callback(behavior_tree_msgs::msg::BehaviorTreeCommands msg); + void is_armed_callback(const std_msgs::msg::Bool::SharedPtr msg); + void has_control_callback(const std_msgs::msg::Bool::SharedPtr msg); + void takeoff_state_callback(const std_msgs::msg::String::SharedPtr msg); + void landing_state_callback(const std_msgs::msg::String::SharedPtr msg); + + + public: + BehaviorExecutive(); + +}; diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_executive/package.xml b/robot/ros_ws/src/autonomy/5_behavior/behavior_executive/package.xml new file mode 100644 index 00000000..8633fd1c --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_executive/package.xml @@ -0,0 +1,29 @@ + + + + behavior_executive + 0.0.0 + TODO: Package description + uav + TODO: License declaration + + ament_cmake + + rclcpp + rclcpp_action + std_msgs + std_srvs + nav_msgs + behavior_tree + behavior_tree_msgs + airstack_msgs + airstack_common + mavros_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_executive/src/behavior_executive.cpp b/robot/ros_ws/src/autonomy/5_behavior/behavior_executive/src/behavior_executive.cpp new file mode 100644 index 00000000..e6bb9b04 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_executive/src/behavior_executive.cpp @@ -0,0 +1,345 @@ +#include + +BehaviorExecutive::BehaviorExecutive() : Node("behavior_executive") { + // conditions + auto_takeoff_commanded_condition = new bt::Condition("Auto Takeoff Commanded", this); + takeoff_commanded_condition = new bt::Condition("Takeoff Commanded", this); + armed_condition = new bt::Condition("Armed", this); + offboard_mode_condition = new bt::Condition("Offboard Mode", this); + stationary_condition = new bt::Condition("Stationary", this); + land_commanded_condition = new bt::Condition("Land Commanded", this); + pause_commanded_condition = new bt::Condition("Pause Commanded", this); + rewind_commanded_condition = new bt::Condition("Rewind Commanded", this); + fixed_trajectory_condition = new bt::Condition("Fixed Trajectory Commanded", this); + global_plan_condition = new bt::Condition("Global Plan Commanded", this); + offboard_commanded_condition = new bt::Condition("Offboard Commanded", this); + arm_commanded_condition = new bt::Condition("Arm Commanded", this); + disarm_commanded_condition = new bt::Condition("Disarm Commanded", this); + takeoff_complete_condition = new bt::Condition("Takeoff Complete", this); + landing_complete_condition = new bt::Condition("Landing Complete", this); + conditions.push_back(auto_takeoff_commanded_condition); + conditions.push_back(takeoff_commanded_condition); + conditions.push_back(armed_condition); + conditions.push_back(offboard_mode_condition); + conditions.push_back(stationary_condition); + conditions.push_back(land_commanded_condition); + conditions.push_back(pause_commanded_condition); + conditions.push_back(rewind_commanded_condition); + conditions.push_back(fixed_trajectory_condition); + conditions.push_back(global_plan_condition); + conditions.push_back(offboard_commanded_condition); + conditions.push_back(arm_commanded_condition); + conditions.push_back(disarm_commanded_condition); + conditions.push_back(takeoff_complete_condition); + conditions.push_back(landing_complete_condition); + + // actions + arm_action = new bt::Action("Arm", this); + takeoff_action = new bt::Action("Takeoff", this); + land_action = new bt::Action("Land", this); + pause_action = new bt::Action("Pause", this); + rewind_action = new bt::Action("Rewind", this); + follow_fixed_trajectory_action = new bt::Action("Follow Fixed Trajectory", this); + global_plan_action = new bt::Action("Follow Global Plan", this); + request_control_action = new bt::Action("Request Control", this); + disarm_action = new bt::Action("Disarm", this); + actions.push_back(arm_action); + actions.push_back(takeoff_action); + actions.push_back(land_action); + actions.push_back(pause_action); + actions.push_back(rewind_action); + actions.push_back(follow_fixed_trajectory_action); + actions.push_back(global_plan_action); + actions.push_back(request_control_action); + actions.push_back(disarm_action); + + // subscribers + behavior_tree_commands_sub = + this->create_subscription( + "behavior_tree_commands", 1, + std::bind(&BehaviorExecutive::bt_commands_callback, this, std::placeholders::_1)); + is_armed_sub = this->create_subscription( + "is_armed", 1, + std::bind(&BehaviorExecutive::is_armed_callback, this, std::placeholders::_1)); + has_control_sub = this->create_subscription( + "has_control", 1, + std::bind(&BehaviorExecutive::has_control_callback, this, std::placeholders::_1)); + takeoff_state_sub = this->create_subscription("takeoff_state", 1, + std::bind(&BehaviorExecutive::takeoff_state_callback, + this, std::placeholders::_1)); + landing_state_sub = this->create_subscription("landing_state", 1, + std::bind(&BehaviorExecutive::landing_state_callback, + this, std::placeholders::_1)); + + + // publishers + + // services + service_callback_group = + this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + robot_command_client = this->create_client( + "robot_command", rmw_qos_profile_services_default, service_callback_group); + trajectory_mode_client = this->create_client( + "set_trajectory_mode", rmw_qos_profile_services_default, service_callback_group); + takeoff_landing_command_client = this->create_client( + "set_takeoff_landing_command", rmw_qos_profile_services_default, service_callback_group); + global_planner_toggle_client = this->create_client( + "global_plan_toggle", rmw_qos_profile_services_default, service_callback_group); + + // timers + timer = rclcpp::create_timer(this, this->get_clock(), rclcpp::Duration::from_seconds(1. / 20.), + std::bind(&BehaviorExecutive::timer_callback, this)); +} + +void BehaviorExecutive::timer_callback() { + if (request_control_action->is_active()) { + if (request_control_action->active_has_changed()) { + airstack_msgs::srv::RobotCommand::Request::SharedPtr request = + std::make_shared(); + request->command = airstack_msgs::srv::RobotCommand::Request::REQUEST_CONTROL; + + auto result = robot_command_client->async_send_request(request); + std::cout << "waiting be rc" << std::endl; + result.wait(); + std::cout << "done be rc" << std::endl; + if (result.get()->success) + request_control_action->set_success(); + else + request_control_action->set_failure(); + } + } + + if (arm_action->is_active()) { + if (arm_action->active_has_changed()) { + airstack_msgs::srv::RobotCommand::Request::SharedPtr request = + std::make_shared(); + request->command = airstack_msgs::srv::RobotCommand::Request::ARM; + + auto result = robot_command_client->async_send_request(request); + std::cout << "waiting be arm" << std::endl; + result.wait(); + std::cout << "done be arm" << std::endl; + if (result.get()->success) + arm_action->set_success(); + else + arm_action->set_failure(); + } + } + + if (disarm_action->is_active()) { + if (disarm_action->active_has_changed()) { + airstack_msgs::srv::RobotCommand::Request::SharedPtr request = + std::make_shared(); + request->command = airstack_msgs::srv::RobotCommand::Request::DISARM; + + auto result = robot_command_client->async_send_request(request); + std::cout << "waiting be arm" << std::endl; + result.wait(); + std::cout << "done be arm" << std::endl; + if (result.get()->success) + disarm_action->set_success(); + else + disarm_action->set_failure(); + } + } + + if (takeoff_action->is_active()) { + // std::cout << "takeoff" << std::endl; + takeoff_action->set_running(); + if (takeoff_action->active_has_changed()) { + // put trajectory controller in track mode + airstack_msgs::srv::TrajectoryMode::Request::SharedPtr mode_request = + std::make_shared(); + mode_request->mode = airstack_msgs::srv::TrajectoryMode::Request::TRACK; + auto mode_result = trajectory_mode_client->async_send_request(mode_request); + std::cout << "mode 1" << std::endl; + mode_result.wait(); + std::cout << "mode 2" << std::endl; + + if (mode_result.get()->success) { + // send a takeoff command to ardupilot + // TODO clean up variable names + airstack_msgs::srv::RobotCommand::Request::SharedPtr request = + std::make_shared(); + request->command = airstack_msgs::srv::RobotCommand::Request::TAKEOFF; + + auto result = robot_command_client->async_send_request(request); + std::cout << "waiting robot command takeoff" << std::endl; + result.wait(); + std::cout << "done robot command takeoff" << std::endl; + + // send the takeoff trajectory + if (result.get()->success) { + airstack_msgs::srv::TakeoffLandingCommand::Request::SharedPtr takeoff_request = + std::make_shared(); + takeoff_request->command = + airstack_msgs::srv::TakeoffLandingCommand::Request::TAKEOFF; + auto takeoff_result = + takeoff_landing_command_client->async_send_request(takeoff_request); + std::cout << "takeoff 1" << std::endl; + takeoff_result.wait(); + std::cout << "takeoff 2" << std::endl; + if (takeoff_result.get()->accepted) + takeoff_action->set_success(); + else + takeoff_action->set_failure(); + } else + takeoff_action->set_failure(); + } else + takeoff_action->set_failure(); + } + + if(takeoff_state == "COMPLETE"){ + takeoff_complete_condition->set(true); + takeoff_action->set_success(); + } + } + + if (land_action->is_active()) { + // std::cout << "land" << std::endl; + land_action->set_running(); + if (land_action->active_has_changed()) { + // put trajectory controller in track mode + airstack_msgs::srv::TrajectoryMode::Request::SharedPtr mode_request = + std::make_shared(); + mode_request->mode = airstack_msgs::srv::TrajectoryMode::Request::TRACK; + auto mode_result = trajectory_mode_client->async_send_request(mode_request); + std::cout << "mode 1" << std::endl; + mode_result.wait(); + std::cout << "mode 2" << std::endl; + + if (mode_result.get()->success) { + airstack_msgs::srv::TakeoffLandingCommand::Request::SharedPtr land_request = + std::make_shared(); + land_request->command = airstack_msgs::srv::TakeoffLandingCommand::Request::LAND; + auto land_result = takeoff_landing_command_client->async_send_request(land_request); + std::cout << "land 1" << std::endl; + land_result.wait(); + std::cout << "land 2" << std::endl; + if (land_result.get()->accepted) + land_action->set_success(); + else + land_action->set_failure(); + } else + land_action->set_failure(); + } + + + if(landing_state == "COMPLETE"){ + landing_complete_condition->set(true); + land_action->set_success(); + } + } + + if (pause_action->is_active()) { + pause_action->set_running(); + if (pause_action->active_has_changed()) { + airstack_msgs::srv::TrajectoryMode::Request::SharedPtr mode_request = + std::make_shared(); + mode_request->mode = airstack_msgs::srv::TrajectoryMode::Request::PAUSE; + auto mode_result = trajectory_mode_client->async_send_request(mode_request); + mode_result.wait(); + } + } + + if (rewind_action->is_active()) { + rewind_action->set_running(); + if (rewind_action->active_has_changed()) { + airstack_msgs::srv::TrajectoryMode::Request::SharedPtr mode_request = + std::make_shared(); + mode_request->mode = airstack_msgs::srv::TrajectoryMode::Request::REWIND; + auto mode_result = trajectory_mode_client->async_send_request(mode_request); + mode_result.wait(); + } + } + + // follow fixed trajectory action + if (follow_fixed_trajectory_action->is_active()) { + follow_fixed_trajectory_action->set_running(); + + if (follow_fixed_trajectory_action->active_has_changed()) { + // put trajectory controller in track mode + airstack_msgs::srv::TrajectoryMode::Request::SharedPtr mode_request = + std::make_shared(); + mode_request->mode = airstack_msgs::srv::TrajectoryMode::Request::TRACK; + auto mode_result = trajectory_mode_client->async_send_request(mode_request); + std::cout << "mode 1" << std::endl; + mode_result.wait(); + std::cout << "mode 2" << std::endl; + } + } + + if (global_plan_action->is_active()) { + global_plan_action->set_running(); + if (global_plan_action->active_has_changed()) { + // put trajectory controller in ADD_SEGMENT mode + airstack_msgs::srv::TrajectoryMode::Request::SharedPtr mode_request = + std::make_shared(); + mode_request->mode = airstack_msgs::srv::TrajectoryMode::Request::ADD_SEGMENT; + auto mode_result = trajectory_mode_client->async_send_request(mode_request); + std::cout << "mode 1" << std::endl; + mode_result.wait(); + std::cout << "mode 2" << std::endl; + + std_srvs::srv::Trigger::Request::SharedPtr request = + std::make_shared(); + auto result = global_planner_toggle_client->async_send_request(request); + result.wait(); + if (result.get()->success) + global_plan_action->set_success(); + else + global_plan_action->set_failure(); + }; + } + + for (bt::Condition* condition : conditions) condition->publish(); + for (bt::Action* action : actions) action->publish(); +} + +// callbacks + +void BehaviorExecutive::bt_commands_callback(behavior_tree_msgs::msg::BehaviorTreeCommands msg) { + for (int i = 0; i < msg.commands.size(); i++) { + std::string condition_name = msg.commands[i].condition_name; + int status = msg.commands[i].status; + + for (int j = 0; j < conditions.size(); j++) { + bt::Condition* condition = conditions[j]; + if (condition_name == condition->get_label()) { + if (status == behavior_tree_msgs::msg::Status::SUCCESS) + condition->set(true); + else if (status == behavior_tree_msgs::msg::Status::FAILURE) + condition->set(false); + } + } + } +} + +void BehaviorExecutive::is_armed_callback(const std_msgs::msg::Bool::SharedPtr msg) { + armed_condition->set(msg->data); +} + +void BehaviorExecutive::has_control_callback(const std_msgs::msg::Bool::SharedPtr msg) { + offboard_mode_condition->set(msg->data); +} + + +void BehaviorExecutive::takeoff_state_callback(const std_msgs::msg::String::SharedPtr msg){ + takeoff_state = msg->data; +} + +void BehaviorExecutive::landing_state_callback(const std_msgs::msg::String::SharedPtr msg){ + landing_state = msg->data; +} + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + std::shared_ptr node = std::make_shared(); + + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.spin(); + rclcpp::shutdown(); + + return 0; +} diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree/CMakeLists.txt b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree/CMakeLists.txt new file mode 100644 index 00000000..00372fad --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree/CMakeLists.txt @@ -0,0 +1,75 @@ +cmake_minimum_required(VERSION 3.5) +project(behavior_tree) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(behavior_tree_msgs REQUIRED) +find_package(Boost REQUIRED COMPONENTS iostreams) + +include_directories(include ${rclcpp_INCLUDE_DIRS} ${behavior_tree_msgs_INCLUDE_DIRS}) + +add_executable(behavior_tree_implementation src/behavior_tree_implementation.cpp) +target_compile_features(behavior_tree_implementation PUBLIC c_std_99 cxx_std_17) +target_link_libraries(behavior_tree_implementation + ${Boost_LIBRARIES} + ${rclcpp_LIBRARIES} + ${std_msgs_LIBRARIES} + ${behavior_tree_msgs_LIBRARIES}) +#ament_target_dependencies(behavior_tree_implementation rclcpp std_msgs behavior_tree_msgs) + +add_library(behavior_tree src/behavior_tree.cpp) +ament_target_dependencies(behavior_tree rclcpp std_msgs behavior_tree_msgs) +ament_export_targets(behavior_tree HAS_LIBRARY_TARGET) + +install( + DIRECTORY include/behavior_tree + DESTINATION include + ) + +install( + TARGETS behavior_tree + EXPORT behavior_tree + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + +install(TARGETS + behavior_tree_implementation + DESTINATION lib/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +# Install files. +# install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +# install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) +# install(DIRECTORY params DESTINATION share/${PROJECT_NAME}) + +ament_package() diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree/config/drone.tree b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree/config/drone.tree new file mode 100644 index 00000000..cd527ed8 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree/config/drone.tree @@ -0,0 +1,56 @@ +? + -> + (Auto Takeoff Commanded) + -> + ? + (Armed) + (Offboard Mode) + [Request Control] + ? + (Armed) + [Arm] + ? + (Takeoff Complete) + [Takeoff] + -> + (Takeoff Commanded) + ? + (Takeoff Complete) + [Takeoff] + -> + (Land Commanded) + ? + (Landing Complete) + [Land] + -> + (Pause Commanded) + ? + [Pause] + -> + (Rewind Commanded) + ? + [Rewind] + -> + (Fixed Trajectory Commanded) + ? + [Follow Fixed Trajectory] + -> + (Global Plan Commanded) + ? + [Follow Global Plan] + -> + (Offboard Commanded) + ? + (Offboard Mode) + [Request Control] + -> + (Arm Commanded) + ? + (Armed) + [Arm] + -> + (Disarm Commanded) + ? + + (Armed) + [Disarm] diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree/include/behavior_tree/behavior_tree.h b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree/include/behavior_tree/behavior_tree.h new file mode 100644 index 00000000..deff832b --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree/include/behavior_tree/behavior_tree.h @@ -0,0 +1,123 @@ +#ifndef _BEHAVIOR_TREE_H_ +#define _BEHAVIOR_TREE_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace bt { + + class Condition { + private: + std_msgs::msg::Bool success; + std::string label; + + rclcpp::Publisher::SharedPtr success_pub; + + public: + Condition(std::string label, rclcpp::Node* node); + void set(bool b); + void publish(); + bool get(); + std::string get_label(); + + static std::string get_published_topic_name(std::string label); + }; + + + class Action { + private: + bool active, prev_active, active_changed; + uint64_t current_id; + behavior_tree_msgs::msg::Status status; + std::string label; + + rclcpp::Subscription::SharedPtr active_sub; + rclcpp::Publisher::SharedPtr status_pub; + + void active_callback(const behavior_tree_msgs::msg::Active::SharedPtr msg); + + std::function active_callback_func; + std::function inactive_callback_func; + + void init(std::string label, rclcpp::Node* node); + + public: + Action(std::string label, rclcpp::Node* node); + + // constructor for active and inactive callback that are class members + template + Action(std::string label, rclcpp::Node* node, + void(ActiveClassType::*active_callback)(), ActiveClassType* active_class_object, + void(InactiveClassType::*inactive_callback)(), InactiveClassType* inactive_class_object){ + init(label, node); + active_callback_func = std::bind(active_callback, active_class_object); + inactive_callback_func = std::bind(inactive_callback, inactive_class_object); + } + + // constructor for an active callback that is a class member + template + Action(std::string label, rclcpp::Node* node, + void(ActiveClassType::*active_callback)(), ActiveClassType* active_class_object){ + init(label, node); + active_callback_func = std::bind(active_callback, active_class_object); + } + + // constructor for active and inactive callbacks that are not class members. The inactive callback is optional + Action(std::string label, rclcpp::Node* node, + void(*active_callback)(), + void(*inactive_callback)()=NULL){ + init(label, node); + active_callback_func = active_callback; + inactive_callback_func = inactive_callback; + } + + // constructor for an active callback that is not a class member and inactive callback that is a class member + template + Action(std::string label, rclcpp::Node* node, + void(*active_callback)(), + void(InactiveClassType::*inactive_callback)(), InactiveClassType* inactive_class_object){ + init(label, node); + active_callback_func = active_callback; + inactive_callback_func = std::bind(inactive_callback, inactive_class_object); + } + + // constructor for an active callback that is a class member and inactive callback that is not a class member + template + Action(std::string label, rclcpp::Node* node, + void(ActiveClassType::*active_callback)(), ActiveClassType* active_class_object, + void(*inactive_callback)()){ + init(label, node); + active_callback_func = std::bind(active_callback, active_class_object); + inactive_callback_func = inactive_callback; + } + + static std::string get_published_topic_name(std::string label); + static std::string get_subscribed_topic_name(std::string label); + + void set_success(); + void set_running(); + void set_failure(); + + bool is_active(); + bool active_has_changed(); + + bool is_success(); + bool is_running(); + bool is_failure(); + + std::string get_label(); + + void publish(); + }; + +}; + + +#endif diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree/include/behavior_tree/behavior_tree_implementation.h b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree/include/behavior_tree/behavior_tree_implementation.h new file mode 100644 index 00000000..c3ac77fe --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree/include/behavior_tree/behavior_tree_implementation.h @@ -0,0 +1,176 @@ +#ifndef _BEHAVIOR_TREE_IMPLEMENTATION_H_ +#define _BEHAVIOR_TREE_IMPLEMENTATION_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +//=============================================================================================== +// ------------------------------------- Base Node Class ---------------------------------------- +//=============================================================================================== + +class Node { +private: +public: + static double max_wait_time; + + std::string label; + bool is_active; + uint8_t status; + std::vector children; + + virtual void add_child(Node*); + virtual bool tick(bool active, int traversal_count)=0; +}; + +//=============================================================================================== +// ------------------------------------ Control Flow Nodes ------------------------------------- +//=============================================================================================== + +class ControlFlowNode : public Node { +private: +public: +}; + +class FallbackNode : public ControlFlowNode { +private: +public: + FallbackNode(); + bool tick(bool active, int traversal_count) override; +}; + +class SequenceNode : public ControlFlowNode { +private: +public: + SequenceNode(); + bool tick(bool active, int traversal_count) override; +}; + +class ParallelNode : public ControlFlowNode { +private: +public: + int child_success_threshold; + ParallelNode(int child_success_threshold); + bool tick(bool active, int traversal_count) override; +}; + +//=============================================================================================== +// -------------------------------------- Execution Nodes -------------------------------------- +//=============================================================================================== + +class ExecutionNode : public Node { +private: +public: + rclcpp::Time status_modification_time; + int current_traversal_count; + rclcpp::Node* node; + //rclcpp::Subscription subscriber; + //rclcpp::Publisher publisher; + + ExecutionNode(rclcpp::Node* node); + void init_ros(); + virtual std::string get_publisher_name()=0; + virtual void init_publisher()=0; + virtual std::string get_subscriber_name()=0; + virtual void init_subscriber()=0; + void set_status(uint8_t status); + double get_status_age(); +}; + +class ConditionNode : public ExecutionNode { +private: + bool callback_status_updated; + //rclcpp::Node* node; + +public: + rclcpp::Subscription::SharedPtr subscriber; + //rclcpp::Publisher<>::SharedPtr publisher; + + ConditionNode(std::string label, rclcpp::Node* node); + bool tick(bool active, int traversal_count) override; + std::string get_publisher_name() override; + void init_publisher() override; + std::string get_subscriber_name() override; + void init_subscriber() override; + void callback(const std_msgs::msg::Bool::SharedPtr msg); +}; + +class ActionNode : public ExecutionNode { +private: + int current_id; + bool publisher_initialized; + bool callback_status_updated; + +public: + bool is_newly_active; + + //rclcpp::Node* node; + + rclcpp::Subscription::SharedPtr subscriber; + rclcpp::Publisher::SharedPtr publisher; + + ActionNode(std::string label, rclcpp::Node* node); + bool tick(bool active, int traversal_count) override; + std::string get_publisher_name() override; + void init_publisher() override; + std::string get_subscriber_name() override; + void init_subscriber() override; + void callback(const behavior_tree_msgs::msg::Status::SharedPtr msg); + void publish_active_msg(int active_id); +}; + + +//=============================================================================================== +// -------------------------------------- Decorator Nodes -------------------------------------- +//=============================================================================================== + +class DecoratorNode : public Node { +private: +public: + void add_child(Node* node) override; +}; + + +class NotNode : public DecoratorNode { +private: +public: + NotNode(); + void add_child(Node* node) override; + bool tick(bool active, int traversal_count) override; +}; + +//=============================================================================================== +// --------------------------------------- Behavior Tree --------------------------------------- +//=============================================================================================== + +class BehaviorTree { +public: + rclcpp::Node* ros2_node; + std::string config_filename; + std::vector nodes; + Node* root; + int traversal_count; + std::unordered_map active_ids; + rclcpp::Publisher::SharedPtr active_actions_pub; + rclcpp::Publisher::SharedPtr graphviz_pub; + rclcpp::Publisher::SharedPtr compressed_pub; + bool first_tick; + + void parse_config(); + int count_tabs(std::string str); + std::string strip_space(std::string str); + std::string strip_brackets(std::string str); + std::vector get_arguments(std::string str); + std::string get_graphviz(); + //public: + BehaviorTree(std::string config_filename, rclcpp::Node* node); + bool tick(); +}; + +#endif diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree/package.xml b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree/package.xml new file mode 100644 index 00000000..a99a5382 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree/package.xml @@ -0,0 +1,22 @@ + + + + behavior_tree + 0.0.0 + TODO: Package description + john + TODO: License declaration + + ament_cmake + + behavior_tree_msgs + std_msgs + rclcpp + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree/src/behavior_tree.cpp b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree/src/behavior_tree.cpp new file mode 100644 index 00000000..3895ecd8 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree/src/behavior_tree.cpp @@ -0,0 +1,143 @@ +#include + +namespace bt { + + // ============================================================================== + // -------------------------------- Condition ----------------------------------- + // ============================================================================== + + + std::string Condition::get_published_topic_name(std::string label){ + std::string topic_name = label; + std::transform(topic_name.begin(), topic_name.end(), topic_name.begin(), ::tolower); + std::replace(topic_name.begin(), topic_name.end(), ' ', '_'); + topic_name += "_success"; + return topic_name; + } + + + Condition::Condition(std::string label, rclcpp::Node* node){ + //ros::NodeHandle nh; + this->label = label; + //success_pub = nh.advertise(Condition::get_published_topic_name(label), 10); + success_pub = node->create_publisher(Condition::get_published_topic_name(label), 10); + success.data = false; + } + + std::string Condition::get_label(){ + return label; + } + + void Condition::set(bool b){ + success.data = b; + } + + bool Condition::get(){ + return success.data; + } + + void Condition::publish(){ + success_pub->publish(success); + } + + // ============================================================================== + // --------------------------------- Action ------------------------------------- + // ============================================================================== + + std::string Action::get_published_topic_name(std::string label){ + std::string topic_name = label; + std::transform(topic_name.begin(), topic_name.end(), topic_name.begin(), ::tolower); + std::replace(topic_name.begin(), topic_name.end(), ' ', '_'); + topic_name += "_status"; + return topic_name; + } + + std::string Action::get_subscribed_topic_name(std::string label){ + std::string topic_name = label; + std::transform(topic_name.begin(), topic_name.end(), topic_name.begin(), ::tolower); + std::replace(topic_name.begin(), topic_name.end(), ' ', '_'); + topic_name += "_active"; + return topic_name; + } + + + Action::Action(std::string label, rclcpp::Node* node){ + init(label, node); + } + + void Action::init(std::string label, rclcpp::Node* node){ + //ros::NodeHandle nh; + active_sub = node->create_subscription(Action::get_subscribed_topic_name(label), 10, + std::bind(&Action::active_callback, this, + std::placeholders::_1)); + status_pub = node->create_publisher(Action::get_published_topic_name(label), 1); + + active = false; + prev_active = false; + active_changed = false; + status.status = behavior_tree_msgs::msg::Status::FAILURE; + + active_callback_func = NULL; + inactive_callback_func = NULL; + + this->label = label; + } + + void Action::active_callback(const behavior_tree_msgs::msg::Active::SharedPtr msg){ + bool active_changed = active != msg->active; + + active = msg->active; + current_id = msg->id; + + if(active_changed && active_callback_func != NULL){ + if(active) + active_callback_func(); + else + inactive_callback_func(); + } + } + + void Action::set_success(){ + status.status = behavior_tree_msgs::msg::Status::SUCCESS; + } + + void Action::set_running(){ + status.status = behavior_tree_msgs::msg::Status::RUNNING; + } + + void Action::set_failure(){ + status.status = behavior_tree_msgs::msg::Status::FAILURE; + } + + bool Action::is_active(){ + active_changed = prev_active != active; + prev_active = active; + return active; + } + + bool Action::active_has_changed(){ + return active_changed; + } + + bool Action::is_success(){ + return status.status == behavior_tree_msgs::msg::Status::SUCCESS; + } + + bool Action::is_running(){ + return status.status == behavior_tree_msgs::msg::Status::RUNNING; + } + + bool Action::is_failure(){ + return status.status == behavior_tree_msgs::msg::Status::FAILURE; + } + + std::string Action::get_label(){ + return label; + } + + void Action::publish(){ + status.id = current_id; + status_pub->publish(status); + } + +}; diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree/src/behavior_tree_implementation.cpp b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree/src/behavior_tree_implementation.cpp new file mode 100644 index 00000000..91985e92 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree/src/behavior_tree_implementation.cpp @@ -0,0 +1,780 @@ +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +//=============================================================================================== +// ------------------------------------- Base Node Class ---------------------------------------- +//=============================================================================================== + +double Node::max_wait_time = 1.0; + +void Node::add_child(Node* node){ + children.push_back(node); +} + +//=============================================================================================== +// ------------------------------------ Control Flow Nodes ------------------------------------- +//=============================================================================================== + +FallbackNode::FallbackNode(){ + label = "?"; + is_active = false; + status = behavior_tree_msgs::msg::Status::FAILURE; +} + +bool FallbackNode::tick(bool active, int traversal_count){ + uint8_t prev_status = status; + bool child_changed = false; + + if(!active){ + // TODO: is ticking children necessary if inactive? + for(int i = 0; i < children.size(); i++) + child_changed |= children[i]->tick(false, traversal_count); + } + else{ + status = behavior_tree_msgs::msg::Status::FAILURE; + for(int i = 0; i < children.size(); i++){ + Node* child = children[i]; + if(status == behavior_tree_msgs::msg::Status::FAILURE){ + child_changed |= child->tick(true, traversal_count); + status = child->status; + } + else + child_changed |= child->tick(false, traversal_count); + } + } + + bool status_changed = (status != prev_status) || child_changed; + bool active_changed = (is_active != active); + + is_active = active; + + return status_changed || active_changed; +} + + +SequenceNode::SequenceNode(){ + label = "->";//"\u2192"; // unicode arrow character + is_active = false; + status = behavior_tree_msgs::msg::Status::FAILURE; +} + +bool SequenceNode::tick(bool active, int traversal_count){ + uint8_t prev_status = status; + bool child_changed = false; + + if(!active){ + for(int i = 0; i < children.size(); i++) + child_changed |= children[i]->tick(false, traversal_count); + + } + else{ + status = behavior_tree_msgs::msg::Status::SUCCESS; + for(int i = 0; i < children.size(); i++){ + Node* child = children[i]; + if(status == behavior_tree_msgs::msg::Status::SUCCESS){ + child_changed |= child->tick(true, traversal_count); + status = child->status; + } + else + child_changed |= child->tick(false, traversal_count); + } + } + + bool status_changed = (status != prev_status) || child_changed; + bool active_changed = (is_active != active); + + is_active = active; + + return status_changed || active_changed; +} + + + +ParallelNode::ParallelNode(int child_success_threshold){ + label = "||"; + is_active = false; + status = behavior_tree_msgs::msg::Status::FAILURE; + this->child_success_threshold = child_success_threshold; +} + +bool ParallelNode::tick(bool active, int traversal_count){ + uint8_t prev_status = status; + bool child_changed = false; + + if(!active){ + for(int i = 0; i < children.size(); i++) + child_changed |= children[i]->tick(false, traversal_count); + } + else{ + int child_success_count = 0; + int child_failure_count = 0; + for(int i = 0; i < children.size(); i++){ + Node* child = children[i]; + child_changed |= child->tick(true, traversal_count); + + if(child->status == behavior_tree_msgs::msg::Status::SUCCESS) + child_success_count++; + else if(child->status == behavior_tree_msgs::msg::Status::FAILURE) + child_failure_count++; + } + + if(child_success_count >= child_success_threshold) + status = behavior_tree_msgs::msg::Status::SUCCESS; + else if(child_failure_count >= (children.size() - child_success_threshold + 1)) + status = behavior_tree_msgs::msg::Status::FAILURE; + else + status = behavior_tree_msgs::msg::Status::RUNNING; + } + + bool status_changed = (status != prev_status) || child_changed; + bool active_changed = (is_active != active); + + is_active = active; + + return status_changed || active_changed; +} + +//=============================================================================================== +// -------------------------------------- Execution Nodes -------------------------------------- +//=============================================================================================== + +ExecutionNode::ExecutionNode(rclcpp::Node* node) + : node(node){ + +} + + +void ExecutionNode::init_ros(){ + init_publisher(); + init_subscriber(); +} + +void ExecutionNode::set_status(uint8_t status){ + this->status = status; + status_modification_time = node->get_clock()->now();//rclcpp::Time::now(); +} + +double ExecutionNode::get_status_age(){ + //return (rclcpp::Time::now() - status_modification_time).toSec(); + return (node->get_clock()->now() - status_modification_time).seconds(); +} + + +ConditionNode::ConditionNode(std::string label, rclcpp::Node* node) + : ExecutionNode(node){ + this->label = label; + status_modification_time = node->get_clock()->now();//rclcpp::Time::now(); + is_active = false; + status = behavior_tree_msgs::msg::Status::FAILURE; + current_traversal_count = -1; + callback_status_updated = false; +} + +bool ConditionNode::tick(bool active, int traversal_count){ + //if(active) + // ROS_INFO_STREAM("Condition " << label << " active"); + + uint8_t prev_status = status; + bool child_changed = false; + + if(current_traversal_count != traversal_count) + current_traversal_count = traversal_count; + else if(is_active) + return false; // TODO: is this correct? + + if(!is_active && active && status == behavior_tree_msgs::msg::Status::FAILURE) + set_status(behavior_tree_msgs::msg::Status::FAILURE); + + if(get_status_age() > Node::max_wait_time) + set_status(behavior_tree_msgs::msg::Status::FAILURE); + + bool status_changed = (status != prev_status) || callback_status_updated; + bool active_changed = (is_active != active); + + is_active = active; + callback_status_updated = false; + + return status_changed || active_changed; +} + +std::string ConditionNode::get_publisher_name(){ + return ""; +} + +void ConditionNode::init_publisher(){ + +} + +std::string ConditionNode::get_subscriber_name(){ + std::string name = label; + std::transform(name.begin(), name.end(), name.begin(), + [](unsigned char c){ return std::tolower(c); }); + std::replace(name.begin(), name.end(), ' ', '_'); + name = name + "_success"; + //ROS_INFO_STREAM("condition get subscriber name: " << name << " " << label); + return name; +} + +void ConditionNode::init_subscriber(){ + //ros::NodeHandle nh; + subscriber = node->create_subscription(get_subscriber_name(), 1, + std::bind(&ConditionNode::callback, this, std::placeholders::_1)); +} + +void ConditionNode::callback(const std_msgs::msg::Bool::SharedPtr msg){ + //ROS_INFO_STREAM(label << ": " << (int)msg.data); + // TODO: bug where if status is set here the changed flag won't be set correctly in tick + uint8_t prev_status = status; + + if(msg->data) + set_status(behavior_tree_msgs::msg::Status::SUCCESS); + else + set_status(behavior_tree_msgs::msg::Status::FAILURE); + + if(status != prev_status) + callback_status_updated = true; +} + + +ActionNode::ActionNode(std::string label, rclcpp::Node* node) + : ExecutionNode(node){ + this->label = label; + status_modification_time = node->get_clock()->now();//rclcpp::Time::now(); + is_active = false; + status = behavior_tree_msgs::msg::Status::FAILURE; + current_traversal_count = -1; + is_newly_active = false; + current_id = 0; + publisher_initialized = false; + callback_status_updated = false; +} + +bool ActionNode::tick(bool active, int traversal_count){ + //if(active) + // ROS_INFO_STREAM("Action " << label << " active"); + + uint8_t prev_status = status; + + if(current_traversal_count != traversal_count) + current_traversal_count = traversal_count; + else if(is_active) + return false; // TOOD: is this correct? + + if(!is_active and active){ + set_status(behavior_tree_msgs::msg::Status::RUNNING); + is_newly_active = true; + } + else + is_newly_active = false; + + if(get_status_age() > Node::max_wait_time) + set_status(behavior_tree_msgs::msg::Status::FAILURE); + + + bool status_changed = (status != prev_status) || callback_status_updated; + bool active_changed = (is_active != active); + + is_active = active; + callback_status_updated = false; + + return status_changed || active_changed; +} + +std::string ActionNode::get_publisher_name(){ + std::string name = label; + std::transform(name.begin(), name.end(), name.begin(), + [](unsigned char c){ return std::tolower(c); }); + std::replace(name.begin(), name.end(), ' ', '_'); + name = name + "_active"; + //ROS_INFO_STREAM("action get publisher name: " << name << " " << label); + return name; +} + +void ActionNode::init_publisher(){ + publisher = node->create_publisher(get_publisher_name(), 1); + publisher_initialized = true; +} + +std::string ActionNode::get_subscriber_name(){ + std::string name = label; + std::transform(name.begin(), name.end(), name.begin(), + [](unsigned char c){ return std::tolower(c); }); + std::replace(name.begin(), name.end(), ' ', '_'); + name = name + "_status"; + //ROS_INFO_STREAM("action get subscriber name: " << name << " " << label); + return name; +} + +void ActionNode::init_subscriber(){ + subscriber = node->create_subscription(get_subscriber_name(), 1, + std::bind(&ActionNode::callback, this, + std::placeholders::_1)); +} + +void ActionNode::callback(const behavior_tree_msgs::msg::Status::SharedPtr msg){ + uint8_t prev_status = status; + if(is_active){ + if(msg->id != current_id){ + //ROS_ERROR_STREAM(label << " Incorrect ID " << msg->id << " " << current_id << " " << is_active); + std::cout << label << " Incorrect ID " << msg->id << " " << current_id << " " << is_active << std::endl; + return; + } + set_status(msg->status); + } + + if(status != prev_status) + callback_status_updated = true; +} + +void ActionNode::publish_active_msg(int active_id){ + if(publisher_initialized){ + behavior_tree_msgs::msg::Active active_msg; + active_msg.active = is_active; + active_msg.id = active_id; + current_id = active_id; + publisher->publish(active_msg); + } +} + + +//=============================================================================================== +// -------------------------------------- Decorator Nodes -------------------------------------- +//=============================================================================================== + +void DecoratorNode::add_child(Node* node){ + if(children.size() == 0) + children.push_back(node); + else{ + //ROS_ERROR_STREAM("A decorator node can only have one child."); + std::cout << "A decorator node can only have one child."<< std::endl; + exit(1); + } +} + +NotNode::NotNode(){ + label = "!"; + is_active = false; + status = behavior_tree_msgs::msg::Status::FAILURE; +} + +void NotNode::add_child(Node* node){ + ConditionNode* condition_node = dynamic_cast(node); + if(condition_node != NULL) + DecoratorNode::add_child(condition_node); + else{ + //ROS_ERROR_STREAM("A not decorator node can only have a condition node as a child."); + std::cout << "A not decorator node can only have a condition node as a child." << std::endl; + exit(1); + } +} + +bool NotNode::tick(bool active, int traversal_count){ + uint8_t prev_status = status; + bool child_changed = false; + + if(children.size() > 0){ + Node* child = children[0]; + child_changed |= child->tick(active, traversal_count); + + if(child->status == behavior_tree_msgs::msg::Status::SUCCESS) + status = behavior_tree_msgs::msg::Status::FAILURE; + else if(child->status == behavior_tree_msgs::msg::Status::FAILURE) + status = behavior_tree_msgs::msg::Status::SUCCESS; + } + + bool status_changed = (status != prev_status) || child_changed; + bool active_changed = (is_active != active); + + is_active = active; + + return status_changed || active_changed; +} + +//=============================================================================================== +// --------------------------------------- Behavior Tree --------------------------------------- +//=============================================================================================== + +BehaviorTree::BehaviorTree(std::string config_filename, rclcpp::Node* node) + : ros2_node(node){ + this->config_filename = config_filename; + root = NULL; + traversal_count = 0; + first_tick = true; + + active_actions_pub = node->create_publisher("active_actions", 1); + graphviz_pub = node->create_publisher("behavior_tree_graphviz", 1); + compressed_pub = node->create_publisher("behavior_tree_graphviz_compressed", 1); + + parse_config(); + for(int i = 0; i < nodes.size(); i++){ + ActionNode* action_node = dynamic_cast(nodes[i]); + if(action_node != NULL) + action_node->init_ros(); + ConditionNode* condition_node = dynamic_cast(nodes[i]); + if(condition_node != NULL) + condition_node->init_ros(); + } +} + +void BehaviorTree::parse_config(){ + std::ifstream in(config_filename); + + //ROS_INFO_STREAM("Config file: " << config_filename); + + if(in.is_open()){ + std::vector nodes_worklist; + int prev_tabs = 0; + + std::string line; + while(getline(in, line)){ + // skip empty lines + if(line.size() == 0) + continue; + + //ROS_INFO_STREAM("Line: " << line); + + // initialization + int curr_tabs = count_tabs(line); + std::string label = strip_space(line); + Node* node = NULL; + + //ROS_INFO_STREAM("curr tabs: " << curr_tabs << " label: " << label); + + // create a node of the right type + if(label.compare(0, 2, "->") == 0) + node = new SequenceNode(); + else if(label.compare(0, 1, "?") == 0) + node = new FallbackNode(); + else if(label.compare(0, 2, "||") == 0){ + std::vector arguments = get_arguments(label); + int child_success_threshold = 0; + if(arguments.size() > 0) + child_success_threshold = arguments[0]; + else{ + //ROS_ERROR_STREAM("Arguments not provided to parallel node"); + std::cout << "Arguments not provided to parallel node" << std::endl; + exit(1); + } + node = new ParallelNode(child_success_threshold); + } + else if(label.compare(0, 1, "(") == 0) + node = new ConditionNode(strip_brackets(label), ros2_node); + else if(label.compare(0, 1, "[") == 0){ + node = new ActionNode(strip_brackets(label), ros2_node); + active_ids[node->label] = 0; + } + else if(label.compare(0, 1, "<") == 0){ + std::string decorator_label = strip_brackets(label); + if(decorator_label.compare(0, 1, "!") == 0) + node = new NotNode(); + } + + if(node != NULL){ + nodes.push_back(node); + + if(root == NULL){ + root = node; + nodes_worklist.push_back(node); + continue; + } + + if(curr_tabs == prev_tabs+1){ + Node* parent = nodes_worklist[nodes_worklist.size()-1]; + parent->add_child(node); + } + else{ + for(int i = 0; i < prev_tabs - curr_tabs + 1; i++) + nodes_worklist.pop_back(); + Node* parent = nodes_worklist[nodes_worklist.size()-1]; + parent->add_child(node); + } + + nodes_worklist.push_back(node); + prev_tabs = curr_tabs; + } + } + in.close(); + } + else + std::cout << "Failed to open behavior tree config file: " << config_filename << std::endl; + //ROS_ERROR_STREAM("Failed to open behavior tree config file: " << config_filename); +} + +int BehaviorTree::count_tabs(std::string str){ + int count = 0; + for(int i = 0; i < str.size(); i++) + if(str[i] == '\t') + count++; + else + break; + + return count; +} + +std::string BehaviorTree::strip_space(std::string str){ + std::string result = str; + while(result.size() > 0) + if(std::isspace(result[0])) + result.erase(0, 1); + else + break; + + while(result.size() > 0) + if(std::isspace(result[result.size()-1])) + result.erase(result.size()-1, 1); + else + break; + + return result; +} + +std::string BehaviorTree::strip_brackets(std::string str){ + std::string result = str; + while(result.size() > 0) + if(result[0] == '(' || result[0] == '[' || result[0] == '<') + result.erase(0, 1); + else + break; + + while(result.size() > 0) + if(result[result.size()-1] == ')' || result[result.size()-1] == ']' || result[result.size()-1] == '>') + result.erase(result.size()-1, 1); + else + break; + + return result; +} + + +std::vector BehaviorTree::get_arguments(std::string str){ + std::vector arguments; + + std::istringstream ss(str); + std::string first; + ss >> first; + + int i; + while(ss >> i) + arguments.push_back(i); + + return arguments; +} + +bool BehaviorTree::tick(){ + bool changed = false || first_tick; + first_tick = false; + + if(root != NULL){ + changed = root->tick(true, traversal_count); + traversal_count++; + + std_msgs::msg::String active_actions; + + std::unordered_map unique_action_nodes; + for(int i = 0; i < nodes.size(); i++){ + ActionNode* action_node = dynamic_cast(nodes[i]); + if(action_node != NULL){ + if(unique_action_nodes.count(action_node->label) == 0 || action_node->is_active){ + unique_action_nodes[action_node->label] = action_node; + if(action_node->is_newly_active) + active_ids[action_node->label]++; + } + } + } + + for(auto it = unique_action_nodes.begin(); it != unique_action_nodes.end(); it++){ + ActionNode* action_node = dynamic_cast(it->second); + if(action_node != NULL){ + action_node->publish_active_msg(active_ids[it->first]); + + if(it->second->is_active) + active_actions.data += action_node->label + ", "; + } + else{ + std::cout << "action node is NULL, something went wrong." << std::endl; + //ROS_ERROR_STREAM("action node is NULL, something went wrong."); + } + } + if(active_actions.data.length() >= 2){ + active_actions.data.pop_back(); + active_actions.data.pop_back(); + } + active_actions_pub->publish(active_actions); + } + + return changed; +} + +std::string BehaviorTree::get_graphviz(){ + std::string gv = "digraph G {\n"; + + std::vector nodes_worklist; + nodes_worklist.push_back(root); + int count = 0; + std::unordered_map node_names; + + while(!nodes_worklist.empty()){ + Node* node = nodes_worklist.back(); + nodes_worklist.pop_back(); + std::string name = "node_" + std::to_string(count); + count++; + + std::string style = ""; + if(node->is_active){ + style += "penwidth=2 color=black style=filled "; + if(node->status == behavior_tree_msgs::msg::Status::SUCCESS) + style += "fillcolor=green"; + else if(node->status == behavior_tree_msgs::msg::Status::RUNNING) + style += "fillcolor=blue"; + else if(node->status == behavior_tree_msgs::msg::Status::FAILURE) + style += "fillcolor=red"; + } + else{ + style += "penwidth=2 "; + if(node->status == behavior_tree_msgs::msg::Status::SUCCESS) + style += "color=green"; + else if(node->status == behavior_tree_msgs::msg::Status::RUNNING) + style += "color=blue"; + else if(node->status == behavior_tree_msgs::msg::Status::FAILURE) + style += "color=red"; + } + + if(dynamic_cast(node) != NULL){ + gv += "\t" + name + " [label=\"" + node->label + "\" " + style + "]\n"; + } + else if(dynamic_cast(node) != NULL){ + gv += "\t" + name + " [label=\"" + node->label + "\" shape=square " + style + "]\n"; + } + else if(dynamic_cast(node) != NULL){ + gv += "\t" + name + " [label=\"" + node->label + "\" shape=square " + style + "]\n"; + } + else if(dynamic_cast(node) != NULL){ + gv += "\t" + name + " [label=\"" + node->label + "\" shape=square " + style + "]\n"; + } + else if(dynamic_cast(node) != NULL){ + gv += "\t" + name + " [label=\"" + node->label + "\" shape=parallelogram " + style + "]\n"; + } + else if(dynamic_cast(node) != NULL){ + gv += "\t" + name + " [label=\"" + node->label + "\" shape=diamond " + style + "]\n"; + } + + node_names[node] = name; + + for(int i = 0; i < node->children.size(); i++) + nodes_worklist.push_back(node->children[i]); + } + + gv += "\n\tordering=out;\n\n"; + + nodes_worklist.push_back(root); + while(!nodes_worklist.empty()){ + Node* node = nodes_worklist.back(); + nodes_worklist.pop_back(); + + for(int i = 0; i < node->children.size(); i++){ + gv += "\t" + node_names[node] + " -> "+ node_names[node->children[i]] + ";\n"; + nodes_worklist.push_back(node->children[i]); + } + } + + gv += "}\n"; + + return gv; +} + +std::string string_compress_encode(const std::string &data) +{ + std::stringstream compressed; + std::stringstream original; + original << data; + boost::iostreams::filtering_streambuf out; + out.push(boost::iostreams::zlib_compressor()); + out.push(original); + boost::iostreams::copy(out, compressed); + + /**need to encode here **/ + //std::string compressed_encoded = base64_encode(reinterpret_cast(compressed.c_str()), compressed.length()); + + return compressed.str();//compressed_encoded; +} + +BehaviorTree* bt; +std_msgs::msg::String graphviz_msg; +std_msgs::msg::String compressed_msg; +bool only_publish_on_change; +void timer_callback(){//const rclcpp::TimerEvent& te){ + bool changed = bt->tick(); + //ROS_INFO_STREAM("Changed: " << changed); + + if(changed){ + graphviz_msg.data = bt->get_graphviz(); + compressed_msg.data = string_compress_encode(graphviz_msg.data); + + if(only_publish_on_change){ + bt->graphviz_pub->publish(graphviz_msg); + bt->compressed_pub->publish(compressed_msg); + } + } + + if(!only_publish_on_change){ + bt->graphviz_pub->publish(graphviz_msg); + bt->compressed_pub->publish(compressed_msg); + } +} + +class BehaviorTreeNode : public rclcpp::Node +{ +private: + rclcpp::TimerBase::SharedPtr timer; + +public: + BehaviorTreeNode() + : Node("behavior_tree_node") + { + //timer = this->create_timer(std::chrono::milliseconds(50), std::bind(&MinimalPublisher::timer_callback, this)); + + + this->declare_parameter("config", ""); + std::string config_filename = this->get_parameter("config").as_string(); + + this->declare_parameter("timeout", 1.0); + ::Node::max_wait_time = this->get_parameter("timeout").as_double(); + + bt = new BehaviorTree(config_filename, this); + + timer = rclcpp::create_timer(this, this->get_clock(), std::chrono::milliseconds(50), &timer_callback); + } +}; + +int main(int argc, char** argv){ + /* + ros::init(argc, argv, "behavior_tree"); + + ros::NodeHandle nh; + ros::NodeHandle pnh("~"); + + std::string config_filename = pnh.param("config", std::string("")); + only_publish_on_change = pnh.param("only_publish_on_change", false); + Node::max_wait_time = pnh.param("timeout", 1.0); + + rclcpp::Timer timer = nh.createTimer(ros::Duration(0.05), timer_callback); + + bt = new BehaviorTree(config_filename); + + ros::spin(); + */ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + + return 0; +} diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/CMakeLists.txt b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/CMakeLists.txt new file mode 100644 index 00000000..21d815ef --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/CMakeLists.txt @@ -0,0 +1,69 @@ +cmake_minimum_required(VERSION 3.5) +project(behavior_tree_example) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(behavior_tree_msgs REQUIRED) +find_package(behavior_tree REQUIRED) +find_package(tf2 REQUIRED) + +#add_executable(behavior_tree_example src/behavior_tree_example.cpp) +#ament_target_dependencies(talker rclcpp std_msgs) + +add_executable(behavior_tree_example src/behavior_tree_example.cpp) +ament_target_dependencies(behavior_tree_example rclcpp std_msgs geometry_msgs visualization_msgs nav_msgs behavior_tree_msgs behavior_tree tf2) + +add_executable(robot_node src/robot_node.cpp) +ament_target_dependencies(robot_node rclcpp std_msgs geometry_msgs visualization_msgs nav_msgs behavior_tree_msgs tf2) + +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}/ + ) + +install(DIRECTORY + config + DESTINATION share/${PROJECT_NAME}/ + ) + +install(DIRECTORY + rviz + DESTINATION share/${PROJECT_NAME}/ + ) + +install(TARGETS + behavior_tree_example + robot_node + DESTINATION lib/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/config/example.tree b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/config/example.tree new file mode 100644 index 00000000..4903ecf6 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/config/example.tree @@ -0,0 +1,22 @@ +? + -> + (Visited Destination) + (At Home) + ? + (On Ground) + [Land] + -> + ? + (In No Fly Zone) + (At Flight Altitude) + [Takeoff] + ? + (In Fly Zone) + (On Ground) + [Land] + ? + (Visited Destination) + [Go To Destination] + ? + (At Home) + [Go Home] diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/config/example2.tree b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/config/example2.tree new file mode 100644 index 00000000..53570490 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/config/example2.tree @@ -0,0 +1,27 @@ +|| 2 + ? + (At Home) + [Spin] + ? + -> + (Visited Destination) + (At Home) + ? + (On Ground) + [Land] + -> + ? + (In No Fly Zone) + (At Flight Altitude) + [Takeoff] + ? + + (In No Fly Zone) + (On Ground) + [Land] + ? + (Visited Destination) + [Go To Destination] + ? + (At Home) + [Go Home] diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/config/example3.tree b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/config/example3.tree new file mode 100644 index 00000000..19229d0a --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/config/example3.tree @@ -0,0 +1,12 @@ +|| 2 + ? + (At Home) + [Spin] + ? + -> + (Visited Destination) + (At Home) + ? + (On Ground) + [Land] + include $(find behavior_tree_example)/config/example3_subtree.tree diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/config/example3_nested_subtree.tree b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/config/example3_nested_subtree.tree new file mode 100644 index 00000000..92390827 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/config/example3_nested_subtree.tree @@ -0,0 +1,3 @@ +? + (Visited Destination) + [Go To Destination] \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/config/example3_subtree.tree b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/config/example3_subtree.tree new file mode 100644 index 00000000..a311d1d9 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/config/example3_subtree.tree @@ -0,0 +1,14 @@ +-> + ? + (In No Fly Zone) + (At Flight Altitude) + [Takeoff] + ? + + (In No Fly Zone) + (On Ground) + [Land] + include $(find behavior_tree_example)/config/example3_nested_subtree.tree + ? + (At Home) + [Go Home] \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/config/ugv_tree.tree b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/config/ugv_tree.tree new file mode 100644 index 00000000..84e18504 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/config/ugv_tree.tree @@ -0,0 +1,37 @@ +? + -> + (User Pressed Estop) + ? + (Not Stopped) + [Stop] + -> + (User Pressed Return Home Button) + ? + (At Home) + [Go To Home] + -> + (User Pressed Start Button) + ? + (Not Stopped) + [Initiate] + ? + (Not Stuck) + [Recovery 1] + [Recovery 2] + [Recovery 3] + ? + (Low Battery) + -> + ? + (Good Comms) + [Explore] + ? + (In Comms) + [Drop Comms] + [Return to Comms] + ? + (At Home) + [Go To Home] + ? + (Not Stopped) + [Stop] \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/config/ugv_tree_basic.tree b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/config/ugv_tree_basic.tree new file mode 100644 index 00000000..5d93cae6 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/config/ugv_tree_basic.tree @@ -0,0 +1,25 @@ +? + -> + (User Pressed Estop) + ? + (Not Stopped) + [Stop] + -> + (User Pressed Return Home Button) + ? + (At Home) + [Go To Home] + -> + (User Pressed Resume) + ? + (Stopped) + [Initiate] + ? + (Low Battery) + [Explore] + ? + (At Home) + [Go To Home] + ? + (Not Stopped) + [Stop] \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/launch/behavior_tree_example.xml b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/launch/behavior_tree_example.xml new file mode 100644 index 00000000..fb5ba46a --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/launch/behavior_tree_example.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/package.xml b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/package.xml new file mode 100644 index 00000000..56aa8345 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/package.xml @@ -0,0 +1,27 @@ + + + + behavior_tree_example + 0.0.0 + TODO: Package description + john + TODO: License declaration + + ament_cmake + + rclcpp + behavior_tree_msgs + behavior_tree + std_msgs + visualization_msgs + geometry_msgs + nav_msgs + tf2 + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/rviz/behavior_tree.perspective b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/rviz/behavior_tree.perspective new file mode 100644 index 00000000..ea05a446 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/rviz/behavior_tree.perspective @@ -0,0 +1,62 @@ +{ + "keys": {}, + "groups": { + "mainwindow": { + "keys": { + "geometry": { + "repr(QByteArray.hex)": "QtCore.QByteArray(b'01d9d0cb000300000000006400000064000005600000038e0000006400000089000005600000038e00000000000000000a000000006400000089000005600000038e')", + "type": "repr(QByteArray.hex)", + "pretty-print": " d d ` d ` d ` " + }, + "state": { + "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff00000000fd0000000100000003000004fd000002dafc0100000001fb00000042007200710074005f006200650068006100760069006f0072005f0074007200650065005f005f005000790043006f006e0073006f006c0065005f005f0031005f005f0100000000000004fd0000017300ffffff000004fd0000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')", + "type": "repr(QByteArray.hex)", + "pretty-print": " Brqt_behavior_tree__PyConsole__1__ 6MinimizedDockWidgetsToolbar " + } + }, + "groups": { + "toolbar_areas": { + "keys": { + "MinimizedDockWidgetsToolbar": { + "repr": "8", + "type": "repr" + } + }, + "groups": {} + } + } + }, + "pluginmanager": { + "keys": { + "running-plugins": { + "repr": "{'rqt_behavior_tree/PyConsole': [1]}", + "type": "repr" + } + }, + "groups": { + "plugin__rqt_behavior_tree__PyConsole__1": { + "keys": {}, + "groups": { + "dock_widget__": { + "keys": { + "dock_widget_title": { + "repr": "''", + "type": "repr" + }, + "dockable": { + "repr": "True", + "type": "repr" + }, + "parent": { + "repr": "None", + "type": "repr" + } + }, + "groups": {} + } + } + } + } + } + } +} \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/rviz/behavior_tree_example.rviz b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/rviz/behavior_tree_example.rviz new file mode 100644 index 00000000..1159cebd --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/rviz/behavior_tree_example.rviz @@ -0,0 +1,210 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.5 + Tree Height: 365 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 100 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/PointStamped + Color: 0; 0; 255 + Enabled: true + History Length: 1 + Name: Destination + Radius: 0.4000000059604645 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /destination + Value: true + - Alpha: 1 + Class: rviz_default_plugins/PointStamped + Color: 0; 255; 0 + Enabled: true + History Length: 1 + Name: Home + Radius: 0.4000000059604645 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /home + Value: true + - Angle Tolerance: 0 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 1 + Name: Robot Odometry + Position Tolerance: 0 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Axes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: perception/state_estimation/odometry + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Markers + Namespaces: + destination_label: true + home_label: true + no_fly_zone: true + no_fly_zone_label: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /no_fly_zone_vis + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 13.15915298461914 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 5.043847560882568 + Y: -0.7309801578521729 + Z: 1.8234843015670776 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.06039784476161003 + Target Frame: + Value: Orbit (rviz) + Yaw: 1.57039475440979 + Saved: ~ +Window Geometry: + Displays: + collapsed: true + Height: 846 + Hide Left Dock: true + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd000000040000000000000156000002f4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d000002f4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000100000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002f4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004b0000002f800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1200 + X: 1182 + Y: 227 diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/src/behavior_tree_example.cpp b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/src/behavior_tree_example.cpp new file mode 100644 index 00000000..15171453 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/src/behavior_tree_example.cpp @@ -0,0 +1,298 @@ +#include +#include +#include +#include +#include +#include +#include +#include + + +#include "tf2/LinearMath/Quaternion.h" +#include "tf2/LinearMath/Matrix3x3.h" +#include "tf2/LinearMath/Vector3.h" + +#include // Include the behavior tree library containing the Action and Condition classes. + + +class BehaviorTreeExample : public rclcpp::Node +{ +private: + bool got_robot_odom, got_home, got_destination, got_no_fly_zone, got_no_fly_zone_radius; + nav_msgs::msg::Odometry robot_odom; + float flight_z, ground_z; + geometry_msgs::msg::PointStamped home, destination; + float acceptance_radius; + geometry_msgs::msg::PointStamped no_fly_zone; + float no_fly_zone_radius; + + // Condition variables + bt::Condition* at_flight_altitude_condition; + bt::Condition* on_ground_condition; + bt::Condition* visited_destination_condition; + bt::Condition* at_home_condition; + bt::Condition* in_fly_zone_condition; + bt::Condition* in_no_fly_zone_condition; + + // Action variables + bt::Action* land_action; + bt::Action* takeoff_action; + bt::Action* go_to_destination_action; + bt::Action* go_home_action; + + // subscribers + rclcpp::Subscription::SharedPtr robot_odom_sub; + rclcpp::Subscription::SharedPtr home_sub; + rclcpp::Subscription::SharedPtr destination_sub; + rclcpp::Subscription::SharedPtr no_fly_zone_sub; + rclcpp::Subscription::SharedPtr no_fly_zone_radius_sub; + + // publishers + rclcpp::Publisher::SharedPtr vel_cmd_pub; + + // timers + rclcpp::TimerBase::SharedPtr timer; + +public: + BehaviorTreeExample() + : Node("behavior_tree_example"){ + + + on_ground_condition = new bt::Condition("On Ground", this); + at_flight_altitude_condition = new bt::Condition("At Flight Altitude", this); + visited_destination_condition = new bt::Condition("Visited Destination", this); + at_home_condition = new bt::Condition("At Home", this); + in_fly_zone_condition = new bt::Condition("In Fly Zone", this); + in_no_fly_zone_condition = new bt::Condition("In No Fly Zone", this); + + land_action = new bt::Action("Land", this); + // Do the same for the "Takeoff" action. + takeoff_action = new bt::Action("Takeoff", this, + &BehaviorTreeExample::takeoff_active_callback, this, + &BehaviorTreeExample::takeoff_inactive_callback, this); + go_to_destination_action = new bt::Action("Go To Destination", this); + go_home_action = new bt::Action("Go Home", this); + + // init subscribers + robot_odom_sub = this->create_subscription("odometry", 10, + std::bind(&BehaviorTreeExample::robot_odom_callback, this, std::placeholders::_1)); + home_sub = this->create_subscription("home", 10, + std::bind(&BehaviorTreeExample::home_callback, this, std::placeholders::_1)); + destination_sub = this->create_subscription("destination", 10, + std::bind(&BehaviorTreeExample::destination_callback, this, std::placeholders::_1)); + + no_fly_zone_sub = this->create_subscription("no_fly_zone", 10, + std::bind(&BehaviorTreeExample::no_fly_zone_callback, this, std::placeholders::_1)); + no_fly_zone_radius_sub = this->create_subscription("no_fly_zone_radius", 10, + std::bind(&BehaviorTreeExample::no_fly_zone_radius_callback, this, std::placeholders::_1)); + + + // init publishers + vel_cmd_pub = this->create_publisher("/vel_cmd", 10); + + // initialization + got_robot_odom = false; + got_home = false; + got_destination = false; + got_no_fly_zone = false; + got_no_fly_zone_radius = false; + flight_z = 0.99; + ground_z = 0.01; + acceptance_radius = 0.01; + + // init timer + timer = this->create_wall_timer(std::chrono::milliseconds(50), std::bind(&BehaviorTreeExample::timer_callback, this)); + } + + void timer_callback(){ + if(!got_robot_odom || !got_home || !got_destination) + return; + + // The condition should be success if the robot is above the flying height's z level, + // and failure otherwise. + at_flight_altitude_condition->set(robot_odom.pose.pose.position.z > flight_z); + at_flight_altitude_condition->publish(); + + // Set the condition to success if the robot is below the ground's z level, + // or to failure if the robot is above the ground's z level. + on_ground_condition->set(robot_odom.pose.pose.position.z < ground_z); + on_ground_condition->publish(); + + // The at home condition should be set to success if the robot is within + // a threshold distance of the home location and failure otherwise. + at_home_condition->set(distance(robot_odom, home) < acceptance_radius); + at_home_condition->publish(); + + if(got_no_fly_zone && got_no_fly_zone_radius){ + float distance = sqrt(pow(robot_odom.pose.pose.position.x - no_fly_zone.point.x, 2) + + pow(robot_odom.pose.pose.position.y - no_fly_zone.point.y, 2)); + + // Check if the robot is with the radius of the no fly zone point. + bool in_no_fly_zone = distance <= no_fly_zone_radius; + + in_no_fly_zone_condition->set(in_no_fly_zone); // Set the condition. + in_no_fly_zone_condition->publish(); // Publish. + + in_fly_zone_condition->set(!in_no_fly_zone); // Set the condition. + in_fly_zone_condition->publish(); // Publish. + } + + // The visited destination condition should be set to success (true) when we first + // get close enough to the destination point and remain true forever. + float destination_distance = distance(robot_odom, destination); + if(!visited_destination_condition->get()) // Only changed the condition value if it hasn't been set to true yet. + visited_destination_condition->set(destination_distance < acceptance_radius); + visited_destination_condition->publish(); // Publish the condition. + + if(land_action->is_active()){ // Make sure the robot only lands if the land action is active. + geometry_msgs::msg::TwistStamped vel_cmd; + vel_cmd.header.stamp = this->get_clock()->now(); + vel_cmd.header.frame_id = "world"; + + if(on_ground_condition->get()){ + // Set the status to success if the robot has landed. + land_action->set_success(); + // Command zero velocity when the robot is done landing. + vel_cmd.twist.linear.x = vel_cmd.twist.linear.y = vel_cmd.twist.linear.z = 0; + } + else{ + // Set the status to running while the robot is landing. + land_action->set_running(); + // Command downward velocity to land. + vel_cmd.twist.linear.z = -0.3; + } + + vel_cmd_pub->publish(vel_cmd); + + // Publish the land action's status. + land_action->publish(); + } + + if(go_home_action->is_active()){ // Make sure the robot tries to go home when the go home action is active. + geometry_msgs::msg::TwistStamped vel_cmd; + vel_cmd.header.stamp = this->get_clock()->now(); + vel_cmd.header.frame_id = "world"; + + if(at_home_condition->get()){ + // Set the status to "SUCCESS" if the robot has reached the home location + go_home_action->set_success(); + // Command zero velocity when the robot is done traveling home. + vel_cmd.twist.linear.x = vel_cmd.twist.linear.y = vel_cmd.twist.linear.z = 0; + } + else{ + // Set the status to "RUNNING" while the robot is moving toward the home location. + go_home_action->set_running(); + // Command a velocity towards the home location. + tf2::Vector3 vel(home.point.x - robot_odom.pose.pose.position.x, + home.point.y - robot_odom.pose.pose.position.y, 0); + vel = 0.3*vel.normalized(); + vel_cmd.twist.linear.x = vel.x(); + vel_cmd.twist.linear.y = vel.y(); + vel_cmd.twist.linear.z = vel.z(); + } + + vel_cmd_pub->publish(vel_cmd); + + // Publish the go home action's status + go_home_action->publish(); + } + + // Make sure the robot only tries to go to the destination while the go to destination action is active + if(go_to_destination_action->is_active()){ + geometry_msgs::msg::TwistStamped vel_cmd; + vel_cmd.header.stamp = this->get_clock()->now(); + vel_cmd.header.frame_id = "world"; + + if(destination_distance < acceptance_radius){ + // Set the status to success if the robot has reached the destination location. + go_to_destination_action->set_success(); + // Command zero velocity when the robot is done traveling to the destination. + vel_cmd.twist.linear.x = vel_cmd.twist.linear.y = vel_cmd.twist.linear.z = 0; + } + else{ + // Set the status to running while the robot is moving towards the destination location. + go_to_destination_action->set_running(); + // Command a velocity towards the destination location. + tf2::Vector3 vel(destination.point.x - robot_odom.pose.pose.position.x, + destination.point.y - robot_odom.pose.pose.position.y, 0); + vel = 0.3*vel.normalized(); + vel_cmd.twist.linear.x = vel.x(); + vel_cmd.twist.linear.y = vel.y(); + vel_cmd.twist.linear.z = vel.z(); + } + + vel_cmd_pub->publish(vel_cmd); + + // Publish the go to destination action's status. + go_to_destination_action->publish(); + } + + } + + void takeoff_active_callback(){ + geometry_msgs::msg::TwistStamped vel_cmd; + vel_cmd.header.stamp = this->get_clock()->now(); + vel_cmd.header.frame_id = "world"; + // Set the status to "RUNNING" while the take off is in progress. + takeoff_action->set_running(); + // Command upward velocity to take off. + vel_cmd.twist.linear.z = 0.3; + + vel_cmd_pub->publish(vel_cmd); + } + + void takeoff_inactive_callback(){ + geometry_msgs::msg::TwistStamped vel_cmd; + vel_cmd.header.stamp = this->get_clock()->now(); + vel_cmd.header.frame_id = "world"; + // Set the status to "RUNNING" while the take off is in progress. + takeoff_action->set_running(); + // Command upward velocity to take off. + vel_cmd.twist.linear.z = 0.0; + + vel_cmd_pub->publish(vel_cmd); + + } + + + void robot_odom_callback(const nav_msgs::msg::Odometry::SharedPtr robot_odom){ + got_robot_odom = true; + this->robot_odom = *robot_odom; + } + + void home_callback(const geometry_msgs::msg::PointStamped::SharedPtr home){ + got_home = true; + this->home = *home; + } + + void destination_callback(const geometry_msgs::msg::PointStamped::SharedPtr destination){ + got_destination = true; + this->destination = *destination; + } + + void no_fly_zone_callback(const geometry_msgs::msg::PointStamped::SharedPtr no_fly_zone){ + got_no_fly_zone = true; + this->no_fly_zone = *no_fly_zone; + } + + void no_fly_zone_radius_callback(const std_msgs::msg::Float32::SharedPtr no_fly_zone_radius){ + got_no_fly_zone_radius = true; + this->no_fly_zone_radius = no_fly_zone_radius->data; + } + + float distance(nav_msgs::msg::Odometry odom, geometry_msgs::msg::PointStamped point){ + float distance = sqrt(pow(odom.pose.pose.position.x - point.point.x, 2) + + pow(odom.pose.pose.position.y - point.point.y, 2)); + return distance; + } + + +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/src/robot_node.cpp b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/src/robot_node.cpp new file mode 100644 index 00000000..5aae3be6 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_example/src/robot_node.cpp @@ -0,0 +1,209 @@ +#include + +#include +#include +#include +#include +#include +#include +#include "tf2/LinearMath/Quaternion.h" +#include "tf2/LinearMath/Matrix3x3.h" + +class RobotNode : public rclcpp::Node +{ +private: + nav_msgs::msg::Odometry odom; + geometry_msgs::msg::TwistStamped vel_cmd, ang_vel_cmd; + rclcpp::Time last_execute_time; + geometry_msgs::msg::PointStamped home, destination, no_fly_zone; + std_msgs::msg::Float32 no_fly_zone_radius; + visualization_msgs::msg::Marker no_fly_zone_marker, no_fly_zone_label, home_label, destination_label; + visualization_msgs::msg::MarkerArray marker_array; + + // subscribers + rclcpp::Subscription::SharedPtr vel_cmd_sub; + rclcpp::Subscription::SharedPtr ang_vel_cmd_sub; + + // publishers + rclcpp::Publisher::SharedPtr robot_odom_pub; + rclcpp::Publisher::SharedPtr home_pub; + rclcpp::Publisher::SharedPtr destination_pub; + rclcpp::Publisher::SharedPtr no_fly_zone_pub; + rclcpp::Publisher::SharedPtr no_fly_zone_radius_pub; + rclcpp::Publisher::SharedPtr no_fly_zone_vis_pub; + + rclcpp::TimerBase::SharedPtr timer; + +public: + RobotNode() + : Node("robot_node"){ + // initialization + home.header.frame_id = "world"; + home.point.x = 0; + home.point.y = 0; + home.point.z = 0; + + destination.header.frame_id = "world"; + destination.point.x = 10; + destination.point.y = 0; + destination.point.z = 0; + + no_fly_zone.header.frame_id = "world"; + no_fly_zone.point.x = 5; + no_fly_zone.point.y = 0; + no_fly_zone.point.z = 0; + + no_fly_zone_radius.data = 2.f; + + float marker_height = 10; + no_fly_zone_marker.header.frame_id = "world"; + no_fly_zone_marker.ns = "no_fly_zone"; + no_fly_zone_marker.id = 0; + no_fly_zone_marker.type = visualization_msgs::msg::Marker::CYLINDER; + no_fly_zone_marker.action = visualization_msgs::msg::Marker::ADD; + no_fly_zone_marker.pose.orientation.w = 1; + no_fly_zone_marker.pose.position.x = no_fly_zone.point.x; + no_fly_zone_marker.pose.position.y = no_fly_zone.point.y; + no_fly_zone_marker.pose.position.z = marker_height/2; + no_fly_zone_marker.scale.x = 2*no_fly_zone_radius.data; + no_fly_zone_marker.scale.y = 2*no_fly_zone_radius.data; + no_fly_zone_marker.scale.z = marker_height; + no_fly_zone_marker.color.r = 1; + no_fly_zone_marker.color.g = 0; + no_fly_zone_marker.color.b = 0; + no_fly_zone_marker.color.a = 0.3; + + no_fly_zone_label.header.frame_id = "world"; + no_fly_zone_label.ns = "no_fly_zone_label"; + no_fly_zone_label.id = 0; + no_fly_zone_label.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + no_fly_zone_label.action = visualization_msgs::msg::Marker::ADD; + no_fly_zone_label.pose.orientation.w = 1; + no_fly_zone_label.pose.position.x = no_fly_zone.point.x; + no_fly_zone_label.pose.position.y = no_fly_zone.point.y; + no_fly_zone_label.pose.position.z = marker_height/2; + no_fly_zone_label.scale.x = 1;//2*no_fly_zone_radius.data; + no_fly_zone_label.scale.y = 1;//2*no_fly_zone_radius.data; + no_fly_zone_label.scale.z = 1;//marker_height; + no_fly_zone_label.color.r = 1; + no_fly_zone_label.color.g = 1; + no_fly_zone_label.color.b = 1; + no_fly_zone_label.color.a = 1; + no_fly_zone_label.text = "No Fly Zone"; + + home_label.header.frame_id = "world"; + home_label.ns = "home_label"; + home_label.id = 0; + home_label.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + home_label.action = visualization_msgs::msg::Marker::ADD; + home_label.pose.orientation.w = 1; + home_label.pose.position.x = home.point.x; + home_label.pose.position.y = home.point.y; + home_label.pose.position.z = home.point.z; + home_label.scale.x = 0.5; + home_label.scale.y = 0.5; + home_label.scale.z = 0.5; + home_label.color.r = 1; + home_label.color.g = 1; + home_label.color.b = 1; + home_label.color.a = 1; + home_label.text = "Home"; + + destination_label.header.frame_id = "world"; + destination_label.ns = "destination_label"; + destination_label.id = 0; + destination_label.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + destination_label.action = visualization_msgs::msg::Marker::ADD; + destination_label.pose.orientation.w = 1; + destination_label.pose.position.x = destination.point.x; + destination_label.pose.position.y = destination.point.y; + destination_label.pose.position.z = destination.point.z; + destination_label.scale.x = 0.5; + destination_label.scale.y = 0.5; + destination_label.scale.z = 0.5; + destination_label.color.r = 1; + destination_label.color.g = 1; + destination_label.color.b = 1; + destination_label.color.a = 1; + destination_label.text = "Destination"; + + marker_array.markers.push_back(no_fly_zone_marker); + marker_array.markers.push_back(no_fly_zone_label); + marker_array.markers.push_back(home_label); + marker_array.markers.push_back(destination_label); + + odom.header.frame_id = "world"; + odom.child_frame_id = "world"; + odom.pose.pose.orientation.w = 1; + + last_execute_time = this->get_clock()->now(); + + + // init subscribers + vel_cmd_sub = this->create_subscription("vel_cmd", 10, std::bind(&RobotNode::vel_cmd_callback, this, std::placeholders::_1)); + ang_vel_cmd_sub = this->create_subscription("ang_vel_cmd", 10, std::bind(&RobotNode::ang_vel_cmd_callback, this, std::placeholders::_1)); + + // init publishers + robot_odom_pub = this->create_publisher("odometry", 10); + home_pub = this->create_publisher("home", 10); + destination_pub = this->create_publisher("destination", 10); + no_fly_zone_pub = this->create_publisher("no_fly_zone", 10); + no_fly_zone_radius_pub = this->create_publisher("no_fly_zone_radius", 10); + no_fly_zone_vis_pub = this->create_publisher("no_fly_zone_vis", 10); + + // init timer + timer = this->create_wall_timer(std::chrono::milliseconds(50), std::bind(&RobotNode::timer_callback, this)); + } + + void timer_callback(){ + // keep track of time + rclcpp::Time now = this->get_clock()->now(); + double dt = (now - last_execute_time).seconds(); + last_execute_time = now; + + // update the robot's position based on the velocity command + odom.pose.pose.position.x += vel_cmd.twist.linear.x*dt; + odom.pose.pose.position.y += vel_cmd.twist.linear.y*dt; + odom.pose.pose.position.z += vel_cmd.twist.linear.z*dt; + + + // update the robot's orientation based on the velocity command + double roll, pitch, yaw; + tf2::Quaternion q(odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, odom.pose.pose.orientation.z, odom.pose.pose.orientation.w); + tf2::Matrix3x3 m(q); + m.getRPY(roll, pitch, yaw); + roll += ang_vel_cmd.twist.angular.x*dt; + pitch += ang_vel_cmd.twist.angular.y*dt; + yaw += ang_vel_cmd.twist.angular.z*dt; + q.setRPY(roll, pitch, yaw); + odom.pose.pose.orientation.x = q.x(); + odom.pose.pose.orientation.y = q.y(); + odom.pose.pose.orientation.z = q.z(); + odom.pose.pose.orientation.w = q.w(); + + // publish + odom.header.stamp = home.header.stamp = destination.header.stamp = no_fly_zone.header.stamp = no_fly_zone_marker.header.stamp = this->get_clock()->now(); + robot_odom_pub->publish(odom); + home_pub->publish(home); + destination_pub->publish(destination); + no_fly_zone_pub->publish(no_fly_zone); + no_fly_zone_radius_pub->publish(no_fly_zone_radius); + no_fly_zone_vis_pub->publish(marker_array);//no_fly_zone_marker); + } + + void vel_cmd_callback(const geometry_msgs::msg::TwistStamped::SharedPtr twist){ + vel_cmd = *twist; + } + + void ang_vel_cmd_callback(const geometry_msgs::msg::TwistStamped::SharedPtr twist){ + ang_vel_cmd = *twist; + } +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_msgs/CMakeLists.txt b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_msgs/CMakeLists.txt new file mode 100644 index 00000000..8ac81126 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_msgs/CMakeLists.txt @@ -0,0 +1,46 @@ +cmake_minimum_required(VERSION 3.5) +project(behavior_tree_msgs) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(std_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/Active.msg" + "msg/Status.msg" + "msg/BehaviorTreeCommand.msg" + "msg/BehaviorTreeCommands.msg" + DEPENDENCIES std_msgs +) + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_msgs/msg/Active.msg b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_msgs/msg/Active.msg new file mode 100644 index 00000000..40f59c1c --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_msgs/msg/Active.msg @@ -0,0 +1,2 @@ +bool active +int64 id \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_msgs/msg/BehaviorTreeCommand.msg b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_msgs/msg/BehaviorTreeCommand.msg new file mode 100644 index 00000000..7662dcf8 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_msgs/msg/BehaviorTreeCommand.msg @@ -0,0 +1,2 @@ +string condition_name +int8 status \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_msgs/msg/BehaviorTreeCommands.msg b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_msgs/msg/BehaviorTreeCommands.msg new file mode 100644 index 00000000..e2671e67 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_msgs/msg/BehaviorTreeCommands.msg @@ -0,0 +1 @@ +BehaviorTreeCommand[] commands \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_msgs/msg/Status.msg b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_msgs/msg/Status.msg new file mode 100644 index 00000000..ae407837 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_msgs/msg/Status.msg @@ -0,0 +1,6 @@ +int8 FAILURE=0 +int8 RUNNING=1 +int8 SUCCESS=2 + +int8 status +uint64 id \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_msgs/package.xml b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_msgs/package.xml new file mode 100644 index 00000000..689a29e5 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/behavior_tree_msgs/package.xml @@ -0,0 +1,23 @@ + + + + behavior_tree_msgs + 0.0.0 + TODO: Package description + john + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + std_msgs + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + + diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/CHANGELOG.rst b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/CHANGELOG.rst new file mode 100644 index 00000000..0a789b78 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/CHANGELOG.rst @@ -0,0 +1,149 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package rqt_py_console +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.0.2 (2021-08-31) +------------------ +* Fix modern setuptools warning about dashes instead of underscores (`#11 `_) +* Contributors: Chris Lalancette + +1.0.1 (2021-04-27) +------------------ +* Changed the build type to ament_python and fixed package to run with ros2 run (`#8 `_) +* Contributors: Alejandro Hernández Cordero + +1.0.0 (2018-12-11) +------------------ +* spyderlib -> spyder (`#5 `_) +* ros2 port (`#3 `_) +* autopep8 (`#2 `_) +* Contributors: Mike Lautman + +0.4.8 (2017-04-28) +------------------ + +0.4.7 (2017-03-02) +------------------ + +0.4.6 (2017-02-27) +------------------ + +0.4.5 (2017-02-03) +------------------ + +0.4.4 (2017-01-24) +------------------ +* use Python 3 compatible syntax (`#421 `_) + +0.4.3 (2016-11-02) +------------------ + +0.4.2 (2016-09-19) +------------------ + +0.4.1 (2016-05-16) +------------------ + +0.4.0 (2016-04-27) +------------------ +* Support Qt 5 (in Kinetic and higher) as well as Qt 4 (in Jade and earlier) (`#359 `_) + +0.3.13 (2016-03-08) +------------------- + +0.3.12 (2015-07-24) +------------------- + +0.3.11 (2015-04-30) +------------------- + +0.3.10 (2014-10-01) +------------------- +* update plugin scripts to use full name to avoid future naming collisions + +0.3.9 (2014-08-18) +------------------ + +0.3.8 (2014-07-15) +------------------ + +0.3.7 (2014-07-11) +------------------ +* export architecture_independent flag in package.xml (`#254 `_) + +0.3.6 (2014-06-02) +------------------ + +0.3.5 (2014-05-07) +------------------ + +0.3.4 (2014-01-28) +------------------ + +0.3.3 (2014-01-08) +------------------ +* add groups for rqt plugins, renamed some plugins (`#167 `_) + +0.3.2 (2013-10-14) +------------------ + +0.3.1 (2013-10-09) +------------------ + +0.3.0 (2013-08-28) +------------------ + +0.2.17 (2013-07-04) +------------------- + +0.2.16 (2013-04-09 13:33) +------------------------- + +0.2.15 (2013-04-09 00:02) +------------------------- + +0.2.14 (2013-03-14) +------------------- + +0.2.13 (2013-03-11 22:14) +------------------------- + +0.2.12 (2013-03-11 13:56) +------------------------- + +0.2.11 (2013-03-08) +------------------- + +0.2.10 (2013-01-22) +------------------- + +0.2.9 (2013-01-17) +------------------ + +0.2.8 (2013-01-11) +------------------ + +0.2.7 (2012-12-24) +------------------ + +0.2.6 (2012-12-23) +------------------ + +0.2.5 (2012-12-21 19:11) +------------------------ + +0.2.4 (2012-12-21 01:13) +------------------------ + +0.2.3 (2012-12-21 00:24) +------------------------ + +0.2.2 (2012-12-20 18:29) +------------------------ + +0.2.1 (2012-12-20 17:47) +------------------------ + +0.2.0 (2012-12-20 17:39) +------------------------ +* first release of this package into groovy diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/package.xml b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/package.xml new file mode 100644 index 00000000..4f42763d --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/package.xml @@ -0,0 +1,29 @@ + + rqt_behavior_tree + 1.0.2 + rqt_behavior_tree is a Python GUI plugin providing an interactive Python console. + Dorian Scholz + Michael Lautman + + BSD + + http://wiki.ros.org/rqt_behavior_tree + https://github.com/ros-visualization/rqt_behavior_tree + https://github.com/ros-visualization/rqt_behavior_tree/issues + + Dorian Scholz + + ament_index_python + python_qt_binding + qt_gui + qt_gui_py_common + rclpy + rqt_gui + rqt_gui_py + + + + + ament_python + + diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/plugin.xml b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/plugin.xml new file mode 100644 index 00000000..0fb6b39a --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/plugin.xml @@ -0,0 +1,17 @@ + + + + A Python GUI plugin providing an interactive Python console. + + + + + folder + Plugins related to miscellaneous tools. + + + applications-python + A Python GUI plugin providing an interactive Python console. + + + diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/resource/py_console_widget.ui b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/resource/py_console_widget.ui new file mode 100644 index 00000000..cf321997 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/resource/py_console_widget.ui @@ -0,0 +1,53 @@ + + + PyConsole + + + + 0 + 0 + 276 + 212 + + + + PyConsoleeeeeeeeeeeeeeeeee + + + + 0 + + + 0 + + + 0 + + + 3 + + + 0 + + + + + 0 + + + + + + + + + + + PyConsoleTextEdit + QTextEdit +
rqt_behavior_tree.py_console_text_edit
+ + + + + diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/resource/rqt_behavior_tree b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/resource/rqt_behavior_tree new file mode 100644 index 00000000..e69de29b diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/setup.cfg b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/setup.cfg new file mode 100644 index 00000000..60bf582f --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/rqt_behavior_tree +[install] +install_scripts=$base/lib/rqt_behavior_tree diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/setup.py b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/setup.py new file mode 100644 index 00000000..3913f21c --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/setup.py @@ -0,0 +1,39 @@ +from setuptools import setup + +package_name = 'rqt_behavior_tree' + +setup( + name=package_name, + version='1.0.2', + packages=[package_name, package_name + '/xdot'], + package_dir={'': 'src'}, + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name + '/resource', + ['resource/py_console_widget.ui']), + ('share/' + package_name, ['package.xml']), + ('share/' + package_name, ['plugin.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + author='Dorian Scholz', + maintainer='Dirk Thomas, Dorian Scholz, Michael Lautman', + maintainer_email='dthomas@osrfoundation.org', + keywords=['ROS'], + classifiers=[ + 'Intended Audience :: Developers', + 'License :: OSI Approved :: BSD License', + 'Programming Language :: Python', + 'Topic :: Software Development', + ], + description=( + 'rqt_behavior_tree is a Python GUI plugin providing an interactive Python console.' + ), + license='BSD', + entry_points={ + 'console_scripts': [ + 'rqt_behavior_tree = ' + package_name + '.main:main', + ], + }, +) diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/__init__.py b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/main.py b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/main.py new file mode 100644 index 00000000..db924165 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/main.py @@ -0,0 +1,12 @@ +import sys + +from rqt_gui.main import Main + + +def main(): + main = Main() + sys.exit(main.main(sys.argv, standalone='rqt_behavior_tree.py_console.PyConsole')) + + +if __name__ == '__main__': + main() diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/py_console.py b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/py_console.py new file mode 100644 index 00000000..eacc3b13 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/py_console.py @@ -0,0 +1,246 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2012, Dorian Scholz +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +from python_qt_binding.QtWidgets import QVBoxLayout, QWidget +from rqt_gui_py.plugin import Plugin +from qt_gui_py_common.simple_settings_dialog import SimpleSettingsDialog +from rqt_behavior_tree.py_console_widget import PyConsoleWidget +import threading + +from python_qt_binding.QtCore import Slot, Qt, qVersion, qWarning, Signal +import python_qt_binding.QtWidgets as qt +import python_qt_binding.QtCore as core +import python_qt_binding.QtGui as gui + +import rqt_behavior_tree +print(dir(rqt_behavior_tree)) + +from rqt_behavior_tree.xdot.xdot_qt import DotWidget + +from std_msgs.msg import String + +try: + from rqt_behavior_tree.spyder_console_widget import SpyderConsoleWidget + _has_spyderlib = True +except ImportError: + _has_spyderlib = False + + +class PyConsole(Plugin): + """ + Plugin providing an interactive Python console + """ + + def __init__(self, context): + super(PyConsole, self).__init__(context) + self.setObjectName('PyConsole') + + self.tree = None + + self.initialized_buttons = False + self.prev_graphviz = '' + + #self.behavior_tree_graphviz_sub = rospy.Subscriber('behavior_tree_graphviz', String, self.behavior_tree_graphviz_callback) + #self.timer = rospy.Timer(rospy.Duration(0.1), self.timer_callback) + self.functions_mutex = threading.Lock() + self.functions = {} + self.last_graphviz_string = '' + + self.widget = QWidget() + self.vbox = qt.QVBoxLayout() + self.widget.setLayout(self.vbox) + context.add_widget(self.widget) + #self.widget.setStyleSheet('QWidget{margin-left:-1px;}') + + self.top_widget = qt.QWidget() + self.top_layout = qt.QVBoxLayout() + self.top_widget.setLayout(self.top_layout) + + self.graph_widget = qt.QWidget() + self.graph_layout = qt.QVBoxLayout() + self.graph_widget.setLayout(self.graph_layout) + self.image_label = qt.QLabel('asdfadsf') + #self.graph_layout.addWidget(self.image_label) + + self.xdot_widget = DotWidget() + self.graph_layout.addWidget(self.xdot_widget) + + self.top_layout.addWidget(self.graph_widget) + self.graph_widget.setStyleSheet("background-color: rgb(255, 255, 255);") + + self.config_widget = qt.QWidget() + self.config_widget.setStyleSheet('QWidget{margin-left:-1px;}') + self.config_layout = qt.QHBoxLayout() + self.config_widget.setLayout(self.config_layout) + self.config_widget.setFixedHeight(50) + + self.config_button = qt.QPushButton('Open Config...') + #self.config_button.clicked.connect(self.select_config_file) + self.config_layout.addWidget(self.config_button) + + self.tree_label = qt.QLabel('tree filename: ') + self.config_layout.addWidget(self.tree_label) + + self.debug_checkbox = qt.QCheckBox('Debug Mode') + self.config_layout.addWidget(self.debug_checkbox) + #self.debug_checkbox.stateChanged.connect(self.debug_mode_changed) + + #self.config_widget.setStyleSheet("background-color: rgb(255, 0, 0);") + #self.top_layout.addWidget(self.config_widget) + + #self.vbox.addWidget(self.top_widget) + + self.button_container_widget = qt.QWidget() + self.button_container_layout = qt.QVBoxLayout() + self.button_container_widget.setLayout(self.button_container_layout) + #self.vbox.addWidget(self.button_container_widget) + + self.button_widget = qt.QWidget() + self.button_layout = qt.QHBoxLayout() + self.button_widget.setLayout(self.button_layout) + #self.button_widget.setStyleSheet("background-color: rgb(0, 0, 255);") + + + self.condition_widget = qt.QWidget() + self.condition_layout = qt.QVBoxLayout() + self.condition_widget.setLayout(self.condition_layout) + self.button_layout.addWidget(self.condition_widget) + + self.condition_label = qt.QLabel() + self.condition_label.setText('Conditions') + self.condition_label.setAlignment(Qt.AlignCenter) + self.condition_label.setFont(gui.QFont("SansSerif", 18, gui.QFont.Bold)) + self.condition_layout.addWidget(self.condition_label) + + self.action_widget = qt.QWidget() + self.action_layout = qt.QVBoxLayout() + self.action_widget.setLayout(self.action_layout) + self.button_layout.addWidget(self.action_widget) + + self.action_label = qt.QLabel() + self.action_label.setText('Actions') + self.action_label.setAlignment(Qt.AlignCenter) + self.action_label.setFont(gui.QFont("SansSerif", 18, gui.QFont.Bold)) + self.action_layout.addWidget(self.action_label) + + self.button_scroll_area = qt.QScrollArea() + self.button_scroll_area.setWidget(self.button_widget) + #self.button_scroll_area.setFixedHeight(200) + #self.button_container_widget.setFixedHeight(200) + self.button_container_layout.addWidget(self.button_scroll_area) + self.button_widget.setMinimumWidth(self.button_scroll_area.sizeHint().width()) + self.button_scroll_area.setWidgetResizable(True) + + self.horizontal_splitter = qt.QSplitter(core.Qt.Vertical) + self.horizontal_splitter.addWidget(self.top_widget) + self.horizontal_splitter.addWidget(self.button_container_widget) + self.vbox.addWidget(self.horizontal_splitter) + + self.button_container_widget.hide() + + self.behavior_tree_graphviz_sub = context.node.create_subscription(String, + 'behavior_tree_graphviz', + self.behavior_tree_graphviz_callback, + 10) + ''' + self.setObjectName('PyConsole') + + self._context = context + self._use_spyderlib = _has_spyderlib + self._console_widget = None + self._widget = QWidget() + self._widget.setLayout(QVBoxLayout()) + self._widget.layout().setContentsMargins(0, 0, 0, 0) + if context.serial_number() > 1: + self._widget.setWindowTitle( + self._widget.windowTitle() + (' (%d)' % context.serial_number())) + self._context.add_widget(self._widget) + ''' + + def behavior_tree_graphviz_callback(self, msg): + if msg.data != self.prev_graphviz: + self.xdot_widget.set_dotcode(msg.data) + self.prev_graphviz = msg.data + + def _switch_console_widget(self): + return + self._widget.layout().removeWidget(self._console_widget) + self.shutdown_console_widget() + + ''' + if _has_spyderlib and self._use_spyderlib: + self._console_widget = SpyderConsoleWidget(self._context) + self._widget.setWindowTitle('SpyderConsole') + else: + self._console_widget = PyConsoleWidget(self._context) + self._widget.setWindowTitle('PyConsole') + if self._context.serial_number() > 1: + self._widget.setWindowTitle( + self._widget.windowTitle() + (' (%d)' % self._context.serial_number())) + ''' + self._widget.layout().addWidget(self._console_widget) + + def save_settings(self, plugin_settings, instance_settings): + return + instance_settings.set_value('use_spyderlib', self._use_spyderlib) + + def restore_settings(self, plugin_settings, instance_settings): + return + self._use_spyderlib = _has_spyderlib and ( + instance_settings.value('use_spyderlib', True) in [True, 'true']) + self._switch_console_widget() + + def trigger_configuration(self): + options = [ + {'title': 'SpyderConsole', + 'description': + 'Advanced Python console with tab-completion (needs spyderlib to be installed).', + 'enabled': _has_spyderlib}, + {'title': 'PyConsole', + 'description': 'Simple Python console.'}, + ] + dialog = SimpleSettingsDialog(title='PyConsole Options') + dialog.add_exclusive_option_group( + title='Console Type', options=options, selected_index=int(not self._use_spyderlib)) + console_type = dialog.get_settings()[0] + new_use_spyderlib = {0: True, 1: False}.get( + console_type['selected_index'], self._use_spyderlib) + if self._use_spyderlib != new_use_spyderlib: + self._use_spyderlib = new_use_spyderlib + self._switch_console_widget() + + def shutdown_console_widget(self): + if self._console_widget is not None and hasattr(self._console_widget, 'shutdown'): + self._console_widget.shutdown() + + def shutdown_plugin(self): + self.shutdown_console_widget() diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/py_console_text_edit.py b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/py_console_text_edit.py new file mode 100644 index 00000000..dc9ce1a0 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/py_console_text_edit.py @@ -0,0 +1,69 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2012, Dorian Scholz +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import sys +from code import InteractiveInterpreter + +from python_qt_binding import QT_BINDING, QT_BINDING_VERSION +from python_qt_binding.QtCore import Qt, Signal + +from qt_gui_py_common.console_text_edit import ConsoleTextEdit + + +class PyConsoleTextEdit(ConsoleTextEdit): + _color_stdin = Qt.darkGreen + _multi_line_char = ':' + _multi_line_indent = ' ' + _prompt = ('>>> ', '... ') # prompt for single and multi line + exit = Signal() + + def __init__(self, parent=None): + super(PyConsoleTextEdit, self).__init__(parent) + + self._interpreter_locals = {} + self._interpreter = InteractiveInterpreter(self._interpreter_locals) + + self._comment_writer.write('Python %s on %s\n' % + (sys.version.replace('\n', ''), sys.platform)) + self._comment_writer.write( + 'Qt bindings: %s version %s\n' % (QT_BINDING, QT_BINDING_VERSION)) + + self._add_prompt() + + def update_interpreter_locals(self, newLocals): + self._interpreter_locals.update(newLocals) + + def _exec_code(self, code): + try: + self._interpreter.runsource(code) + except SystemExit: # catch sys.exit() calls, so they don't close the whole gui + self.exit.emit() diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/py_console_widget.py b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/py_console_widget.py new file mode 100644 index 00000000..6d3f879c --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/py_console_widget.py @@ -0,0 +1,59 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2012, Dorian Scholz +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import os +from ament_index_python.resources import get_resource + +from python_qt_binding import loadUi +from python_qt_binding.QtWidgets import QWidget +from rqt_behavior_tree.py_console_text_edit import PyConsoleTextEdit + + +class PyConsoleWidget(QWidget): + + def __init__(self, context=None): + super(PyConsoleWidget, self).__init__() + + _, package_path = get_resource('packages', 'rqt_behavior_tree') + ui_file = os.path.join( + package_path, 'share', 'rqt_behavior_tree', 'resource', 'py_console_widget.ui') + + loadUi(ui_file, self, {'PyConsoleTextEdit': PyConsoleTextEdit}) + self.setObjectName('PyConsoleWidget') + + my_locals = { + 'context': context + } + self.py_console.update_interpreter_locals(my_locals) + self.py_console.print_message( + 'The variable "context" is set to the PluginContext of this plugin.') + self.py_console.exit.connect(context.close_plugin) diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/spyder_console_widget.py b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/spyder_console_widget.py new file mode 100644 index 00000000..374ef7a5 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/spyder_console_widget.py @@ -0,0 +1,60 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2012, Dorian Scholz +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +from python_qt_binding.QtGui import QFont + +from spyder.widgets.internalshell import InternalShell +from spyder.utils.module_completion import moduleCompletion + +class SpyderConsoleWidget(InternalShell): + + def __init__(self, context=None): + my_locals = { + 'context': context + } + super(SpyderConsoleWidget, self).__init__(namespace=my_locals) + self.setObjectName('SpyderConsoleWidget') + self.set_pythonshell_font(QFont('Mono')) + self.interpreter.restore_stds() + + def get_module_completion(self, objtxt): + """Return module completion list associated to object name""" + return moduleCompletion(objtxt) + + def run_command(self, *args): + self.interpreter.redirect_stds() + super(SpyderConsoleWidget, self).run_command(*args) + self.flush() + self.interpreter.restore_stds() + + def shutdown(self): + self.exit_interpreter() diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/test.py b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/test.py new file mode 100644 index 00000000..dc9ce1a0 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/test.py @@ -0,0 +1,69 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2012, Dorian Scholz +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import sys +from code import InteractiveInterpreter + +from python_qt_binding import QT_BINDING, QT_BINDING_VERSION +from python_qt_binding.QtCore import Qt, Signal + +from qt_gui_py_common.console_text_edit import ConsoleTextEdit + + +class PyConsoleTextEdit(ConsoleTextEdit): + _color_stdin = Qt.darkGreen + _multi_line_char = ':' + _multi_line_indent = ' ' + _prompt = ('>>> ', '... ') # prompt for single and multi line + exit = Signal() + + def __init__(self, parent=None): + super(PyConsoleTextEdit, self).__init__(parent) + + self._interpreter_locals = {} + self._interpreter = InteractiveInterpreter(self._interpreter_locals) + + self._comment_writer.write('Python %s on %s\n' % + (sys.version.replace('\n', ''), sys.platform)) + self._comment_writer.write( + 'Qt bindings: %s version %s\n' % (QT_BINDING, QT_BINDING_VERSION)) + + self._add_prompt() + + def update_interpreter_locals(self, newLocals): + self._interpreter_locals.update(newLocals) + + def _exec_code(self, code): + try: + self._interpreter.runsource(code) + except SystemExit: # catch sys.exit() calls, so they don't close the whole gui + self.exit.emit() diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/xdot/__init__.py b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/xdot/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/xdot/wxxdot.py b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/xdot/wxxdot.py new file mode 100644 index 00000000..1e5dd83f --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/xdot/wxxdot.py @@ -0,0 +1,585 @@ +#!/usr/bin/env python +# +# wxpython widgets for using Jose Fonseca's cairo graphviz visualizer +# Copyright (c) 2010, Willow Garage, Inc. +# +# Source modified from Jose Fonseca's XDot pgtk widgets. That code is +# Copyright 2008 Jose Fonseca +# +# This program is free software: you can redistribute it and/or modify it +# under the terms of the GNU Lesser General Public License as published +# by the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU Lesser General Public License for more details. +# +# You should have received a copy of the GNU Lesser General Public License +# along with this program. If not, see . + +from xdot import * + +__all__ = ['WxDotWindow', 'WxDotFrame'] + +# We need to get the wx version with built-in cairo support +import wxversion +wxversion.select("2.8") +import wx +import wx.lib.wxcairo as wxcairo + +# This is a crazy hack to get this to work on 64-bit systems +if 'wxMac' in wx.PlatformInfo: + pass # Implement if necessary +elif 'wxMSW' in wx.PlatformInfo: + pass # Implement if necessary +elif 'wxGTK' in wx.PlatformInfo: + import ctypes + gdkLib = wx.lib.wxcairo._findGDKLib() + gdkLib.gdk_cairo_create.restype = ctypes.c_void_p + +class WxDragAction(object): + def __init__(self, dot_widget): + self.dot_widget = dot_widget + + def on_button_press(self, event): + x,y = event.GetPositionTuple() + self.startmousex = self.prevmousex = x + self.startmousey = self.prevmousey = y + self.start() + + def on_motion_notify(self, event): + x,y = event.GetPositionTuple() + deltax = self.prevmousex - x + deltay = self.prevmousey - y + self.drag(deltax, deltay) + self.prevmousex = x + self.prevmousey = y + + def on_button_release(self, event): + x,y = event.GetPositionTuple() + self.stopmousex = x + self.stopmousey = y + self.stop() + + def draw(self, cr): + pass + + def start(self): + pass + + def drag(self, deltax, deltay): + pass + + def stop(self): + pass + + def abort(self): + pass + +class WxNullAction(WxDragAction): + def on_motion_notify(self, event): + pass + +class WxPanAction(WxDragAction): + def start(self): + self.dot_widget.set_cursor(wx.CURSOR_SIZING) + + def drag(self, deltax, deltay): + self.dot_widget.x += deltax / self.dot_widget.zoom_ratio + self.dot_widget.y += deltay / self.dot_widget.zoom_ratio + self.dot_widget.Refresh() + + def stop(self): + self.dot_widget.set_cursor(wx.CURSOR_ARROW) + + abort = stop + +class WxZoomAction(WxDragAction): + def drag(self, deltax, deltay): + self.dot_widget.zoom_ratio *= 1.005 ** (deltax + deltay) + self.dot_widget.zoom_to_fit_on_resize = False + self.dot_widget.Refresh() + + def stop(self): + self.dot_widget.Refresh() + +class WxZoomAreaAction(WxDragAction): + def drag(self, deltax, deltay): + self.dot_widget.Refresh() + + def draw(self, cr): + cr.save() + cr.set_source_rgba(.5, .5, 1.0, 0.25) + cr.rectangle(self.startmousex, self.startmousey, + self.prevmousex - self.startmousex, + self.prevmousey - self.startmousey) + cr.fill() + cr.set_source_rgba(.5, .5, 1.0, 1.0) + cr.set_line_width(1) + cr.rectangle(self.startmousex - .5, self.startmousey - .5, + self.prevmousex - self.startmousex + 1, + self.prevmousey - self.startmousey + 1) + cr.stroke() + cr.restore() + + def stop(self): + x1, y1 = self.dot_widget.window2graph(self.startmousex, + self.startmousey) + x2, y2 = self.dot_widget.window2graph(self.stopmousex, + self.stopmousey) + self.dot_widget.zoom_to_area(x1, y1, x2, y2) + + def abort(self): + self.dot_widget.Refresh() + +class WxDotWindow(wx.Panel): + """wxpython Frame that draws dot graphs.""" + filter = 'dot' + + def __init__(self, parent, id): + """constructor""" + wx.Panel.__init__(self, parent, id) + + self.graph = Graph() + self.openfilename = None + + self.x, self.y = 0.0, 0.0 + self.zoom_ratio = 1.0 + self.zoom_to_fit_on_resize = False + self.animation = NoAnimation(self) + self.drag_action = WxNullAction(self) + self.presstime = None + self.highlight = None + + # Bind events + self.Bind(wx.EVT_PAINT, self.OnPaint) + self.Bind(wx.EVT_SIZE, self.OnResize) + + self.Bind(wx.EVT_MOUSEWHEEL, self.OnScroll) + + self.Bind(wx.EVT_MOUSE_EVENTS, self.OnMouse) + + self.Bind(wx.EVT_KEY_DOWN, self.OnKeyDown) + + # Callback register + self.select_cbs = [] + self.dc = None + self.ctx = None + self.items_by_url = {} + + ### User callbacks + def register_select_callback(self, cb): + self.select_cbs.append(cb) + + ### Event handlers + def OnResize(self, event): + self.Refresh() + + def OnPaint(self, event): + """Redraw the graph.""" + dc = wx.PaintDC(self) + + #print dc + ctx = wxcairo.ContextFromDC(dc) + ctx = pangocairo.CairoContext(ctx) + #print "DRAW" + + # Get widget size + width, height = self.GetSize() + #width,height = self.dc.GetSizeTuple() + + ctx.rectangle(0,0,width,height) + ctx.clip() + + ctx.set_source_rgba(1.0, 1.0, 1.0, 1.0) + ctx.paint() + + ctx.save() + ctx.translate(0.5*width, 0.5*height) + + ctx.scale(self.zoom_ratio, self.zoom_ratio) + ctx.translate(-self.x, -self.y) + self.graph.draw(ctx, highlight_items=self.highlight) + ctx.restore() + + self.drag_action.draw(ctx) + + def OnScroll(self, event): + """Zoom the view.""" + if event.GetWheelRotation() > 0: + self.zoom_image(self.zoom_ratio * self.ZOOM_INCREMENT, + pos=(event.GetX(), event.GetY())) + else: + self.zoom_image(self.zoom_ratio / self.ZOOM_INCREMENT, + pos=(event.GetX(), event.GetY())) + + def OnKeyDown(self, event): + """Process key down event.""" + key = event.GetKeyCode() + if key == wx.WXK_LEFT: + self.x -= self.POS_INCREMENT/self.zoom_ratio + self.Refresh() + if key == wx.WXK_RIGHT: + self.x += self.POS_INCREMENT/self.zoom_ratio + self.Refresh() + if key == wx.WXK_UP: + self.y -= self.POS_INCREMENT/self.zoom_ratio + self.Refresh() + if key == wx.WXK_DOWN: + self.y += self.POS_INCREMENT/self.zoom_ratio + self.Refresh() + if key == wx.WXK_PAGEUP: + self.zoom_image(self.zoom_ratio * self.ZOOM_INCREMENT) + self.Refresh() + if key == wx.WXK_PAGEDOWN: + self.zoom_image(self.zoom_ratio / self.ZOOM_INCREMENT) + self.Refresh() + if key == wx.WXK_ESCAPE: + self.drag_action.abort() + self.drag_action = WxNullAction(self) + if key == ord('F'): + self.zoom_to_fit() + if key == ord('R'): + self.reload() + if key == ord('Q'): + self.reload() + exit(0) + + ### Helper functions + def get_current_pos(self): + """Get the current graph position.""" + return self.x, self.y + + def set_current_pos(self, x, y): + """Set the current graph position.""" + self.x = x + self.y = y + self.Refresh() + + def set_highlight(self, items): + """Set a number of items to be hilighted.""" + if self.highlight != items: + self.highlight = items + self.Refresh() + + ### Cursor manipulation + def set_cursor(self, cursor_type): + self.cursor = wx.StockCursor(cursor_type) + self.SetCursor(self.cursor) + + ### Zooming methods + def zoom_image(self, zoom_ratio, center=False, pos=None): + """Zoom the graph.""" + if center: + self.x = self.graph.width/2 + self.y = self.graph.height/2 + elif pos is not None: + width, height = self.GetSize() + x, y = pos + x -= 0.5*width + y -= 0.5*height + self.x += x / self.zoom_ratio - x / zoom_ratio + self.y += y / self.zoom_ratio - y / zoom_ratio + self.zoom_ratio = zoom_ratio + self.zoom_to_fit_on_resize = False + self.Refresh() + + def zoom_to_area(self, x1, y1, x2, y2): + """Zoom to an area of the graph.""" + width, height = self.GetSize() + area_width = abs(x1 - x2) + area_height = abs(y1 - y2) + self.zoom_ratio = min( + float(width)/float(area_width), + float(height)/float(area_height) + ) + self.zoom_to_fit_on_resize = False + self.x = (x1 + x2) / 2 + self.y = (y1 + y2) / 2 + self.Refresh() + + def zoom_to_fit(self): + """Zoom to fit the size of the graph.""" + width,height = self.GetSize() + x = self.ZOOM_TO_FIT_MARGIN + y = self.ZOOM_TO_FIT_MARGIN + width -= 2 * self.ZOOM_TO_FIT_MARGIN + height -= 2 * self.ZOOM_TO_FIT_MARGIN + + if float(self.graph.width) > 0 and float(self.graph.height) > 0 and width > 0 and height > 0: + zoom_ratio = min( + float(width)/float(self.graph.width), + float(height)/float(self.graph.height) + ) + self.zoom_image(zoom_ratio, center=True) + self.zoom_to_fit_on_resize = True + + ZOOM_INCREMENT = 1.25 + ZOOM_TO_FIT_MARGIN = 12 + + def on_zoom_in(self, action): + self.zoom_image(self.zoom_ratio * self.ZOOM_INCREMENT) + + def on_zoom_out(self, action): + self.zoom_image(self.zoom_ratio / self.ZOOM_INCREMENT) + + def on_zoom_fit(self, action): + self.zoom_to_fit() + + def on_zoom_100(self, action): + self.zoom_image(1.0) + + POS_INCREMENT = 100 + + def get_drag_action(self, event): + """Get a drag action for this click.""" + # Grab the button + button = event.GetButton() + # Grab modifier keys + control_down = event.ControlDown() + alt_down = event.AltDown() + shift_down = event.ShiftDown() + + drag = event.Dragging() + motion = event.Moving() + + # Get the correct drag action for this click + if button in (wx.MOUSE_BTN_LEFT, wx.MOUSE_BTN_MIDDLE): # left or middle button + if control_down: + if shift_down: + return WxZoomAreaAction(self) + else: + return WxZoomAction(self) + else: + return WxPanAction(self) + + return WxNullAction(self) + + def OnMouse(self, event): + x,y = event.GetPositionTuple() + + item = None + + # Get the item + if not event.Dragging(): + item = self.get_url(x, y) + if item is None: + item = self.get_jump(x, y) + + if item is not None: + self.set_cursor(wx.CURSOR_HAND) + self.set_highlight(item.highlight) + + for cb in self.select_cbs: + cb(item,event) + else: + self.set_cursor(wx.CURSOR_ARROW) + self.set_highlight(None) + + if item is None: + if event.ButtonDown(): + self.animation.stop() + self.drag_action.abort() + + # Get the drag action + self.drag_action = self.get_drag_action(event) + self.drag_action.on_button_press(event) + + self.pressx = x + self.pressy = y + + if event.Dragging() or event.Moving(): + self.drag_action.on_motion_notify(event) + + if event.ButtonUp(): + self.drag_action.on_button_release(event) + self.drag_action = WxNullAction(self) + + event.Skip() + + + def on_area_size_allocate(self, area, allocation): + if self.zoom_to_fit_on_resize: + self.zoom_to_fit() + + def animate_to(self, x, y): + self.animation = ZoomToAnimation(self, x, y) + self.animation.start() + + def window2graph(self, x, y): + "Get the x,y coordinates in the graph from the x,y coordinates in the window.""" + width, height = self.GetSize() + x -= 0.5*width + y -= 0.5*height + x /= self.zoom_ratio + y /= self.zoom_ratio + x += self.x + y += self.y + return x, y + + def get_url(self, x, y): + x, y = self.window2graph(x, y) + return self.graph.get_url(x, y) + + def get_jump(self, x, y): + x, y = self.window2graph(x, y) + return self.graph.get_jump(x, y) + + def set_filter(self, filter): + self.filter = filter + + def set_dotcode(self, dotcode, filename=''): + if isinstance(dotcode, unicode): + dotcode = dotcode.encode('utf8') + p = subprocess.Popen( + [self.filter, '-Txdot'], + stdin=subprocess.PIPE, + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + shell=False, + universal_newlines=True + ) + xdotcode, error = p.communicate(dotcode) + if p.returncode != 0: + print("ERROR PARSING DOT CODE", error) + dialog = gtk.MessageDialog(type=gtk.MESSAGE_ERROR, + message_format=error, + buttons=gtk.BUTTONS_OK) + dialog.set_title('Dot Viewer') + dialog.run() + dialog.destroy() + return False + try: + self.set_xdotcode(xdotcode) + + # Store references to all the items + self.items_by_url = {} + for item in self.graph.nodes + self.graph.edges: + if item.url is not None: + self.items_by_url[item.url] = item + + # Store references to subgraph states + self.subgraph_shapes = self.graph.subgraph_shapes + + except ParseError as ex: + print("ERROR PARSING XDOT CODE") + dialog = gtk.MessageDialog(type=gtk.MESSAGE_ERROR, + message_format=str(ex), + buttons=gtk.BUTTONS_OK) + dialog.set_title('Dot Viewer') + dialog.run() + dialog.destroy() + return False + else: + self.openfilename = filename + return True + + def set_xdotcode(self, xdotcode): + """Set xdot code.""" + #print xdotcode + parser = XDotParser(xdotcode) + self.graph = parser.parse() + self.highlight = None + #self.zoom_image(self.zoom_ratio, center=True) + + def reload(self): + if self.openfilename is not None: + try: + fp = file(self.openfilename, 'rt') + self.set_dotcode(fp.read(), self.openfilename) + fp.close() + except IOError: + pass + + +class WxDotFrame(wx.Frame): + def __init__(self): + wx.Frame.__init__(self, None, -1, "Dot Viewer", size=(512,512)) + + vbox = wx.BoxSizer(wx.VERTICAL) + + # Construct toolbar + toolbar = wx.ToolBar(self, -1) + toolbar.AddLabelTool(wx.ID_OPEN, 'Open File', + wx.ArtProvider.GetBitmap(wx.ART_FOLDER_OPEN,wx.ART_OTHER,(16,16))) + toolbar.AddLabelTool(wx.ID_HELP, 'Help', + wx.ArtProvider.GetBitmap(wx.ART_HELP,wx.ART_OTHER,(16,16)) ) + toolbar.Realize() + + self.Bind(wx.EVT_TOOL, self.DoOpenFile, id=wx.ID_OPEN) + self.Bind(wx.EVT_TOOL, self.ShowControlsDialog, id=wx.ID_HELP) + + # Create dot widge + self.widget = WxDotWindow(self, -1) + + # Add elements to sizer + vbox.Add(toolbar, 0, wx.EXPAND) + vbox.Add(self.widget, 100, wx.EXPAND | wx.ALL) + + self.SetSizer(vbox) + self.Center() + + def ShowControlsDialog(self,event): + dial = wx.MessageDialog(None, + "\ +Pan: Arrow Keys\n\ +Zoom: PageUp / PageDown\n\ +Zoom To Fit: F\n\ +Refresh: R", + 'Keyboard Controls', wx.OK) + dial.ShowModal() + + def DoOpenFile(self,event): + wcd = 'All files (*)|*|GraphViz Dot Files(*.dot)|*.dot|' + dir = os.getcwd() + open_dlg = wx.FileDialog(self, message='Choose a file', defaultDir=dir, defaultFile='', + wildcard=wcd, style=wx.OPEN|wx.CHANGE_DIR) + if open_dlg.ShowModal() == wx.ID_OK: + path = open_dlg.GetPath() + + try: + self.open_file(path) + + except IOError as error: + dlg = wx.MessageDialog(self, 'Error opening file\n' + str(error)) + dlg.ShowModal() + + except UnicodeDecodeError as error: + dlg = wx.MessageDialog(self, 'Error opening file\n' + str(error)) + dlg.ShowModal() + + open_dlg.Destroy() + + def OnExit(self, event): + pass + + def set_dotcode(self, dotcode, filename=''): + if self.widget.set_dotcode(dotcode, filename): + self.SetTitle(os.path.basename(filename) + ' - Dot Viewer') + self.widget.zoom_to_fit() + + def set_xdotcode(self, xdotcode, filename=''): + if self.widget.set_xdotcode(xdotcode): + self.SetTitle(os.path.basename(filename) + ' - Dot Viewer') + self.widget.zoom_to_fit() + + def open_file(self, filename): + try: + fp = file(filename, 'rt') + self.set_dotcode(fp.read(), filename) + fp.close() + except IOError as ex: + """ + dlg = gtk.MessageDialog(type=gtk.MESSAGE_ERROR, + message_format=str(ex), + buttons=gtk.BUTTONS_OK) + dlg.set_title('Dot Viewer') + dlg.run() + dlg.destroy() + """ + + def set_filter(self, filter): + self.widget.set_filter(filter) + diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/xdot/xdot.py b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/xdot/xdot.py new file mode 100644 index 00000000..6b28398f --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/xdot/xdot.py @@ -0,0 +1,2174 @@ +#!/usr/bin/env python +# +# Copyright 2008 Jose Fonseca +# +# This program is free software: you can redistribute it and/or modify it +# under the terms of the GNU Lesser General Public License as published +# by the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU Lesser General Public License for more details. +# +# You should have received a copy of the GNU Lesser General Public License +# along with this program. If not, see . +# + +'''Visualize dot graphs via the xdot format.''' + +__author__ = "Jose Fonseca" + +__version__ = "0.4" + + +import os +import sys +import subprocess +import math +import colorsys +import time +import re + +import gobject +import gtk +import gtk.gdk +import gtk.keysyms +import cairo +import pango +import pangocairo + + +# See http://www.graphviz.org/pub/scm/graphviz-cairo/plugin/cairo/gvrender_cairo.c + +# For pygtk inspiration and guidance see: +# - http://mirageiv.berlios.de/ +# - http://comix.sourceforge.net/ + + +class Pen: + """Store pen attributes.""" + + def __init__(self): + # set default attributes + self.color = (0.0, 0.0, 0.0, 1.0) + self.fillcolor = (0.0, 0.0, 0.0, 1.0) + self.linewidth = 1.0 + self.fontsize = 14.0 + self.fontname = "Times-Roman" + self.dash = () + + def copy(self): + """Create a copy of this pen.""" + pen = Pen() + pen.__dict__ = self.__dict__.copy() + return pen + + def highlighted(self): + pen = self.copy() + pen.color = (1, 0, 0, 1) + pen.fillcolor = (1, .8, .8, 1) + return pen + + +class Shape: + """Abstract base class for all the drawing shapes.""" + + def __init__(self): + pass + + def draw(self, cr, highlight=False): + """Draw this shape with the given cairo context""" + raise NotImplementedError + + def select_pen(self, highlight): + if highlight: + if not hasattr(self, 'highlight_pen'): + self.highlight_pen = self.pen.highlighted() + return self.highlight_pen + else: + return self.pen + + +class TextShape(Shape): + + #fontmap = pangocairo.CairoFontMap() + #fontmap.set_resolution(72) + #context = fontmap.create_context() + + LEFT, CENTER, RIGHT = -1, 0, 1 + + def __init__(self, pen, x, y, j, w, t): + Shape.__init__(self) + self.pen = pen.copy() + self.x = x + self.y = y + self.j = j + self.w = w + self.t = t + + def draw(self, cr, highlight=False): + + try: + layout = self.layout + except AttributeError: + layout = cr.create_layout() + + # set font options + # see http://lists.freedesktop.org/archives/cairo/2007-February/009688.html + context = layout.get_context() + fo = cairo.FontOptions() + fo.set_antialias(cairo.ANTIALIAS_DEFAULT) + fo.set_hint_style(cairo.HINT_STYLE_NONE) + fo.set_hint_metrics(cairo.HINT_METRICS_OFF) + try: + pangocairo.context_set_font_options(context, fo) + except TypeError: + # XXX: Some broken pangocairo bindings show the error + # 'TypeError: font_options must be a cairo.FontOptions or None' + pass + + # set font + font = pango.FontDescription() + font.set_family(self.pen.fontname) + font.set_absolute_size(self.pen.fontsize*pango.SCALE) + layout.set_font_description(font) + + # set text + layout.set_text(self.t) + + # cache it + self.layout = layout + else: + cr.update_layout(layout) + + descent = 2 # XXX get descender from font metrics + + width, height = layout.get_size() + width = float(width)/pango.SCALE + height = float(height)/pango.SCALE + # we know the width that dot thinks this text should have + # we do not necessarily have a font with the same metrics + # scale it so that the text fits inside its box + if width > self.w: + f = self.w / width + width = self.w # equivalent to width *= f + height *= f + descent *= f + else: + f = 1.0 + + if self.j == self.LEFT: + x = self.x + elif self.j == self.CENTER: + x = self.x - 0.5*width + elif self.j == self.RIGHT: + x = self.x - width + else: + assert 0 + + y = self.y - height + descent + + cr.move_to(x, y) + + cr.save() + cr.scale(f, f) + cr.set_source_rgba(*self.select_pen(highlight).color) + cr.show_layout(layout) + cr.restore() + + if 0: # DEBUG + # show where dot thinks the text should appear + cr.set_source_rgba(1, 0, 0, .9) + if self.j == self.LEFT: + x = self.x + elif self.j == self.CENTER: + x = self.x - 0.5*self.w + elif self.j == self.RIGHT: + x = self.x - self.w + cr.move_to(x, self.y) + cr.line_to(x+self.w, self.y) + cr.stroke() + + +class EllipseShape(Shape): + + def __init__(self, pen, x0, y0, w, h, filled=False): + Shape.__init__(self) + self.pen = pen.copy() + self.x0 = x0 + self.y0 = y0 + self.w = w + self.h = h + self.filled = filled + + def draw(self, cr, highlight=False): + cr.save() + cr.translate(self.x0, self.y0) + cr.scale(self.w, self.h) + cr.move_to(1.0, 0.0) + cr.arc(0.0, 0.0, 1.0, 0, 2.0*math.pi) + cr.restore() + pen = self.select_pen(highlight) + if self.filled: + cr.set_source_rgba(*pen.fillcolor) + cr.fill() + else: + cr.set_dash(pen.dash) + cr.set_line_width(pen.linewidth) + cr.set_source_rgba(*pen.color) + cr.stroke() + + +class PolygonShape(Shape): + + def __init__(self, pen, points, filled=False): + Shape.__init__(self) + self.pen = pen.copy() + self.points = points + self.filled = filled + + def draw(self, cr, highlight=False): + x0, y0 = self.points[-1] + cr.move_to(x0, y0) + for x, y in self.points: + cr.line_to(x, y) + cr.close_path() + pen = self.select_pen(highlight) + if self.filled: + cr.set_source_rgba(*pen.fillcolor) + cr.fill_preserve() + cr.fill() + else: + cr.set_dash(pen.dash) + cr.set_line_width(pen.linewidth) + cr.set_source_rgba(*pen.color) + cr.stroke() + + +class LineShape(Shape): + + def __init__(self, pen, points): + Shape.__init__(self) + self.pen = pen.copy() + self.points = points + + def draw(self, cr, highlight=False): + x0, y0 = self.points[0] + cr.move_to(x0, y0) + for x1, y1 in self.points[1:]: + cr.line_to(x1, y1) + pen = self.select_pen(highlight) + cr.set_dash(pen.dash) + cr.set_line_width(pen.linewidth) + cr.set_source_rgba(*pen.color) + cr.stroke() + + +class BezierShape(Shape): + + def __init__(self, pen, points, filled=False): + Shape.__init__(self) + self.pen = pen.copy() + self.points = points + self.filled = filled + + def draw(self, cr, highlight=False): + x0, y0 = self.points[0] + cr.move_to(x0, y0) + for i in xrange(1, len(self.points), 3): + x1, y1 = self.points[i] + x2, y2 = self.points[i + 1] + x3, y3 = self.points[i + 2] + cr.curve_to(x1, y1, x2, y2, x3, y3) + pen = self.select_pen(highlight) + if self.filled: + cr.set_source_rgba(*pen.fillcolor) + cr.fill_preserve() + cr.fill() + else: + cr.set_dash(pen.dash) + cr.set_line_width(pen.linewidth) + cr.set_source_rgba(*pen.color) + cr.stroke() + + +class CompoundShape(Shape): + + def __init__(self, shapes): + Shape.__init__(self) + self.shapes = shapes + + def draw(self, cr, highlight=False): + for shape in self.shapes: + shape.draw(cr, highlight=highlight) + + +class Url(object): + + def __init__(self, item, url, highlight=None): + self.item = item + self.url = url + if highlight is None: + highlight = set([item]) + self.highlight = highlight + + +class Jump(object): + + def __init__(self, item, x, y, highlight=None, url=None): + self.item = item + self.x = x + self.y = y + if highlight is None: + highlight = set([item]) + self.highlight = highlight + self.url = url + + +class Element(CompoundShape): + """Base class for graph nodes and edges.""" + + def __init__(self, shapes): + CompoundShape.__init__(self, shapes) + + def get_url(self, x, y): + return None + + def get_jump(self, x, y): + return None + + +class Node(Element): + + def __init__(self, x, y, w, h, shapes, url): + Element.__init__(self, shapes) + + self.x = x + self.y = y + + self.x1 = x - 0.5*w + self.y1 = y - 0.5*h + self.x2 = x + 0.5*w + self.y2 = y + 0.5*h + + self.url = url + + def is_inside(self, x, y): + return self.x1 <= x and x <= self.x2 and self.y1 <= y and y <= self.y2 + + def get_url(self, x, y): + if self.url is None: + return None + #print (x, y), (self.x1, self.y1), "-", (self.x2, self.y2) + if self.is_inside(x, y): + return Url(self, self.url) + return None + + def get_jump(self, x, y): + if self.is_inside(x, y): + return Jump(self, self.x, self.y) + return None + + +def square_distance(x1, y1, x2, y2): + deltax = x2 - x1 + deltay = y2 - y1 + return deltax*deltax + deltay*deltay + + +class Edge(Element): + + def __init__(self, src, dst, points, shapes, url): + Element.__init__(self, shapes) + self.src = src + self.dst = dst + self.points = points + self.url = url + + RADIUS = 10 + + def get_jump(self, x, y): + if square_distance(x, y, *self.points[0]) <= self.RADIUS*self.RADIUS: + return Jump(self, self.dst.x, self.dst.y, highlight=set([self, self.dst]),url=self.url) + if square_distance(x, y, *self.points[-1]) <= self.RADIUS*self.RADIUS: + return Jump(self, self.src.x, self.src.y, highlight=set([self, self.src]),url=self.url) + return None + + +class Graph(Shape): + + def __init__(self, width=1, height=1, shapes=(), nodes=(), edges=(), subgraph_shapes={}): + Shape.__init__(self) + + self.width = width + self.height = height + self.shapes = shapes + self.nodes = nodes + self.edges = edges + self.subgraph_shapes = subgraph_shapes + + def get_size(self): + return self.width, self.height + + def draw(self, cr, highlight_items=None): + if highlight_items is None: + highlight_items = () + cr.set_source_rgba(0.0, 0.0, 0.0, 1.0) + + cr.set_line_cap(cairo.LINE_CAP_BUTT) + cr.set_line_join(cairo.LINE_JOIN_MITER) + + for shape in self.shapes: + shape.draw(cr) + for edge in self.edges: + edge.draw(cr, highlight=(edge in highlight_items)) + for node in self.nodes: + node.draw(cr, highlight=(node in highlight_items)) + + def get_url(self, x, y): + for node in self.nodes: + url = node.get_url(x, y) + if url is not None: + return url + return None + + def get_jump(self, x, y): + for edge in self.edges: + jump = edge.get_jump(x, y) + if jump is not None: + return jump + for node in self.nodes: + jump = node.get_jump(x, y) + if jump is not None: + return jump + return None + + +class XDotAttrParser: + """Parser for xdot drawing attributes. + See also: + - http://www.graphviz.org/doc/info/output.html#d:xdot + """ + + def __init__(self, parser, buf): + self.parser = parser + self.buf = self.unescape(buf) + self.pos = 0 + + self.pen = Pen() + self.shapes = [] + + def __nonzero__(self): + return self.pos < len(self.buf) + + def unescape(self, buf): + buf = buf.replace('\\"', '"') + buf = buf.replace('\\n', '\n') + return buf + + def read_code(self): + pos = self.buf.find(" ", self.pos) + res = self.buf[self.pos:pos] + self.pos = pos + 1 + while self.pos < len(self.buf) and self.buf[self.pos].isspace(): + self.pos += 1 + return res + + def read_number(self): + return int(float(self.read_code())) + + def read_float(self): + return float(self.read_code()) + + def read_point(self): + x = self.read_number() + y = self.read_number() + return self.transform(x, y) + + def read_text(self): + num = self.read_number() + pos = self.buf.find("-", self.pos) + 1 + self.pos = pos + num + res = self.buf[pos:self.pos] + while self.pos < len(self.buf) and self.buf[self.pos].isspace(): + self.pos += 1 + return res + + def read_polygon(self): + n = self.read_number() + p = [] + for i in range(n): + x, y = self.read_point() + p.append((x, y)) + return p + + def read_color(self): + # See http://www.graphviz.org/doc/info/attrs.html#k:color + c = self.read_text() + c1 = c[:1] + if c1 == '#': + hex2float = lambda h: float(int(h, 16)/255.0) + r = hex2float(c[1:3]) + g = hex2float(c[3:5]) + b = hex2float(c[5:7]) + try: + a = hex2float(c[7:9]) + except (IndexError, ValueError): + a = 1.0 + return r, g, b, a + elif c1.isdigit() or c1 == ".": + # "H,S,V" or "H S V" or "H, S, V" or any other variation + h, s, v = map(float, c.replace(",", " ").split()) + r, g, b = colorsys.hsv_to_rgb(h, s, v) + a = 1.0 + return r, g, b, a + else: + return self.lookup_color(c) + + def lookup_color(self, c): + try: + color = gtk.gdk.color_parse(c) + except ValueError: + pass + else: + s = 1.0/65535.0 + r = color.red*s + g = color.green*s + b = color.blue*s + a = 1.0 + return r, g, b, a + + try: + dummy, scheme, index = c.split('/') + r, g, b = brewer_colors[scheme][int(index)] + except (ValueError, KeyError): + pass + else: + s = 1.0/255.0 + r = r*s + g = g*s + b = b*s + a = 1.0 + return r, g, b, a + + sys.stderr.write("unknown color '%s'\n" % c) + return None + + def parse(self): + s = self + + while s: + op = s.read_code() + if op == "c": + color = s.read_color() + if color is not None: + self.handle_color(color, filled=False) + elif op == "C": + color = s.read_color() + if color is not None: + self.handle_color(color, filled=True) + elif op == "S": + # http://www.graphviz.org/doc/info/attrs.html#k:style + style = s.read_text() + if style.startswith("setlinewidth("): + lw = style.split("(")[1].split(")")[0] + lw = float(lw) + self.handle_linewidth(lw) + elif style in ("solid", "dashed"): + self.handle_linestyle(style) + elif op == "F": + size = s.read_float() + name = s.read_text() + self.handle_font(size, name) + elif op == "T": + x, y = s.read_point() + j = s.read_number() + w = s.read_number() + t = s.read_text() + self.handle_text(x, y, j, w, t) + elif op == "E": + x0, y0 = s.read_point() + w = s.read_number() + h = s.read_number() + self.handle_ellipse(x0, y0, w, h, filled=True) + elif op == "e": + x0, y0 = s.read_point() + w = s.read_number() + h = s.read_number() + self.handle_ellipse(x0, y0, w, h, filled=False) + elif op == "L": + points = self.read_polygon() + self.handle_line(points) + elif op == "B": + points = self.read_polygon() + self.handle_bezier(points, filled=False) + elif op == "b": + points = self.read_polygon() + self.handle_bezier(points, filled=True) + elif op == "P": + points = self.read_polygon() + self.handle_polygon(points, filled=True) + elif op == "p": + points = self.read_polygon() + self.handle_polygon(points, filled=False) + else: + sys.stderr.write("unknown xdot opcode '%s'\n" % op) + break + + return self.shapes + + def transform(self, x, y): + return self.parser.transform(x, y) + + def handle_color(self, color, filled=False): + if filled: + self.pen.fillcolor = color + else: + self.pen.color = color + + def handle_linewidth(self, linewidth): + self.pen.linewidth = linewidth + + def handle_linestyle(self, style): + if style == "solid": + self.pen.dash = () + elif style == "dashed": + self.pen.dash = (6, ) # 6pt on, 6pt off + + def handle_font(self, size, name): + self.pen.fontsize = size + self.pen.fontname = name + + def handle_text(self, x, y, j, w, t): + self.shapes.append(TextShape(self.pen, x, y, j, w, t)) + + def handle_ellipse(self, x0, y0, w, h, filled=False): + if filled: + # xdot uses this to mean "draw a filled shape with an outline" + self.shapes.append(EllipseShape(self.pen, x0, y0, w, h, filled=True)) + self.shapes.append(EllipseShape(self.pen, x0, y0, w, h)) + + def handle_line(self, points): + self.shapes.append(LineShape(self.pen, points)) + + def handle_bezier(self, points, filled=False): + if filled: + # xdot uses this to mean "draw a filled shape with an outline" + self.shapes.append(BezierShape(self.pen, points, filled=True)) + self.shapes.append(BezierShape(self.pen, points)) + + def handle_polygon(self, points, filled=False): + if filled: + # xdot uses this to mean "draw a filled shape with an outline" + self.shapes.append(PolygonShape(self.pen, points, filled=True)) + self.shapes.append(PolygonShape(self.pen, points)) + + +EOF = -1 +SKIP = -2 + + +class ParseError(Exception): + + def __init__(self, msg=None, filename=None, line=None, col=None): + self.msg = msg + self.filename = filename + self.line = line + self.col = col + + def __str__(self): + return ':'.join([str(part) for part in (self.filename, self.line, self.col, self.msg) if part != None]) + + +class Scanner: + """Stateless scanner.""" + + # should be overriden by derived classes + tokens = [] + symbols = {} + literals = {} + ignorecase = False + + def __init__(self): + flags = re.DOTALL + if self.ignorecase: + flags |= re.IGNORECASE + self.tokens_re = re.compile( + '|'.join(['(' + regexp + ')' for type, regexp, test_lit in self.tokens]), + flags + ) + + def next(self, buf, pos): + if pos >= len(buf): + return EOF, '', pos + mo = self.tokens_re.match(buf, pos) + if mo: + text = mo.group() + type, regexp, test_lit = self.tokens[mo.lastindex - 1] + pos = mo.end() + if test_lit: + type = self.literals.get(text, type) + return type, text, pos + else: + c = buf[pos] + return self.symbols.get(c, None), c, pos + 1 + + +class Token: + + def __init__(self, type, text, line, col): + self.type = type + self.text = text + self.line = line + self.col = col + + +class Lexer: + + # should be overriden by derived classes + scanner = None + tabsize = 8 + + newline_re = re.compile(r'\r\n?|\n') + + def __init__(self, buf = None, pos = 0, filename = None, fp = None): + if fp is not None: + try: + fileno = fp.fileno() + length = os.path.getsize(fp.name) + import mmap + except: + # read whole file into memory + buf = fp.read() + pos = 0 + else: + # map the whole file into memory + if length: + # length must not be zero + buf = mmap.mmap(fileno, length, access = mmap.ACCESS_READ) + pos = os.lseek(fileno, 0, 1) + else: + buf = '' + pos = 0 + + if filename is None: + try: + filename = fp.name + except AttributeError: + filename = None + + self.buf = buf + self.pos = pos + self.line = 1 + self.col = 1 + self.filename = filename + + def next(self): + while True: + # save state + pos = self.pos + line = self.line + col = self.col + + type, text, endpos = self.scanner.next(self.buf, pos) + assert pos + len(text) == endpos + self.consume(text) + type, text = self.filter(type, text) + self.pos = endpos + + if type == SKIP: + continue + elif type is None: + msg = 'unexpected char ' + if text >= ' ' and text <= '~': + msg += "'%s'" % text + else: + msg += "0x%X" % ord(text) + raise ParseError(msg, self.filename, line, col) + else: + break + return Token(type = type, text = text, line = line, col = col) + + def consume(self, text): + # update line number + pos = 0 + for mo in self.newline_re.finditer(text, pos): + self.line += 1 + self.col = 1 + pos = mo.end() + + # update column number + while True: + tabpos = text.find('\t', pos) + if tabpos == -1: + break + self.col += tabpos - pos + self.col = ((self.col - 1)//self.tabsize + 1)*self.tabsize + 1 + pos = tabpos + 1 + self.col += len(text) - pos + + +class Parser: + + def __init__(self, lexer): + self.lexer = lexer + self.lookahead = self.lexer.next() + + def match(self, type): + if self.lookahead.type != type: + raise ParseError( + msg = 'unexpected token %r' % self.lookahead.text, + filename = self.lexer.filename, + line = self.lookahead.line, + col = self.lookahead.col) + + def skip(self, type): + while self.lookahead.type != type: + self.consume() + + def consume(self): + token = self.lookahead + self.lookahead = self.lexer.next() + return token + + +ID = 0 +STR_ID = 1 +HTML_ID = 2 +EDGE_OP = 3 + +LSQUARE = 4 +RSQUARE = 5 +LCURLY = 6 +RCURLY = 7 +COMMA = 8 +COLON = 9 +SEMI = 10 +EQUAL = 11 +PLUS = 12 + +STRICT = 13 +GRAPH = 14 +DIGRAPH = 15 +NODE = 16 +EDGE = 17 +SUBGRAPH = 18 + + +class DotScanner(Scanner): + + # token regular expression table + tokens = [ + # whitespace and comments + (SKIP, + r'[ \t\f\r\n\v]+|' + r'//[^\r\n]*|' + r'/\*.*?\*/|' + r'#[^\r\n]*', + False), + + # Alphanumeric IDs + (ID, r'[a-zA-Z_\x80-\xff][a-zA-Z0-9_\x80-\xff]*', True), + + # Numeric IDs + (ID, r'-?(?:\.[0-9]+|[0-9]+(?:\.[0-9]*)?)', False), + + # String IDs + (STR_ID, r'"[^"\\]*(?:\\.[^"\\]*)*"', False), + + # HTML IDs + (HTML_ID, r'<[^<>]*(?:<[^<>]*>[^<>]*)*>', False), + + # Edge operators + (EDGE_OP, r'-[>-]', False), + ] + + # symbol table + symbols = { + '[': LSQUARE, + ']': RSQUARE, + '{': LCURLY, + '}': RCURLY, + ',': COMMA, + ':': COLON, + ';': SEMI, + '=': EQUAL, + '+': PLUS, + } + + # literal table + literals = { + 'strict': STRICT, + 'graph': GRAPH, + 'digraph': DIGRAPH, + 'node': NODE, + 'edge': EDGE, + 'subgraph': SUBGRAPH, + } + + ignorecase = True + + +class DotLexer(Lexer): + + scanner = DotScanner() + + def filter(self, type, text): + # TODO: handle charset + if type == STR_ID: + text = text[1:-1] + + # line continuations + text = text.replace('\\\r\n', '') + text = text.replace('\\\r', '') + text = text.replace('\\\n', '') + + text = text.replace('\\r', '\r') + text = text.replace('\\n', '\n') + text = text.replace('\\t', '\t') + text = text.replace('\\', '') + + type = ID + + elif type == HTML_ID: + text = text[1:-1] + type = ID + + return type, text + + +class DotParser(Parser): + + def __init__(self, lexer): + Parser.__init__(self, lexer) + self.graph_attrs = {} + self.node_attrs = {} + self.edge_attrs = {} + + def parse(self): + self.parse_graph() + self.match(EOF) + + def parse_graph(self): + if self.lookahead.type == STRICT: + self.consume() + self.skip(LCURLY) + self.consume() + while self.lookahead.type != RCURLY: + self.parse_stmt() + self.consume() + + def parse_subgraph(self): + id = None + shapes_before = set(self.shapes) + if self.lookahead.type == SUBGRAPH: + self.consume() + if self.lookahead.type == ID: + id = self.lookahead.text + self.consume() + if self.lookahead.type == LCURLY: + self.consume() + while self.lookahead.type != RCURLY: + self.parse_stmt() + self.consume() + new_shapes = set(self.shapes) - shapes_before + self.subgraph_shapes[id] = [s for s in new_shapes if not any([s in ss for ss in self.subgraph_shapes.values()])] + return id + + def parse_stmt(self): + if self.lookahead.type == GRAPH: + self.consume() + attrs = self.parse_attrs() + self.graph_attrs.update(attrs) + self.handle_graph(attrs) + elif self.lookahead.type == NODE: + self.consume() + self.node_attrs.update(self.parse_attrs()) + elif self.lookahead.type == EDGE: + self.consume() + self.edge_attrs.update(self.parse_attrs()) + elif self.lookahead.type in (SUBGRAPH, LCURLY): + self.parse_subgraph() + else: + id = self.parse_node_id() + if self.lookahead.type == EDGE_OP: + self.consume() + node_ids = [id, self.parse_node_id()] + while self.lookahead.type == EDGE_OP: + node_ids.append(self.parse_node_id()) + attrs = self.parse_attrs() + for i in range(0, len(node_ids) - 1): + self.handle_edge(node_ids[i], node_ids[i + 1], attrs) + elif self.lookahead.type == EQUAL: + self.consume() + self.parse_id() + else: + attrs = self.parse_attrs() + self.handle_node(id, attrs) + if self.lookahead.type == SEMI: + self.consume() + + def parse_attrs(self): + attrs = {} + while self.lookahead.type == LSQUARE: + self.consume() + while self.lookahead.type != RSQUARE: + name, value = self.parse_attr() + attrs[name] = value + if self.lookahead.type == COMMA: + self.consume() + self.consume() + return attrs + + def parse_attr(self): + name = self.parse_id() + if self.lookahead.type == EQUAL: + self.consume() + value = self.parse_id() + else: + value = 'true' + return name, value + + def parse_node_id(self): + node_id = self.parse_id() + if self.lookahead.type == COLON: + self.consume() + port = self.parse_id() + if self.lookahead.type == COLON: + self.consume() + compass_pt = self.parse_id() + else: + compass_pt = None + else: + port = None + compass_pt = None + # XXX: we don't really care about port and compass point values when parsing xdot + return node_id + + def parse_id(self): + self.match(ID) + id = self.lookahead.text + self.consume() + return id + + def handle_graph(self, attrs): + pass + + def handle_node(self, id, attrs): + pass + + def handle_edge(self, src_id, dst_id, attrs): + pass + + +class XDotParser(DotParser): + + def __init__(self, xdotcode): + lexer = DotLexer(buf = xdotcode) + DotParser.__init__(self, lexer) + + self.nodes = [] + self.edges = [] + self.shapes = [] + self.node_by_name = {} + self.top_graph = True + self.subgraph_shapes = {} + + def handle_graph(self, attrs): + if self.top_graph: + try: + bb = attrs['bb'] + except KeyError: + return + + if not bb: + return + + xmin, ymin, xmax, ymax = map(float, bb.split(",")) + + self.xoffset = -xmin + self.yoffset = -ymax + self.xscale = 1.0 + self.yscale = -1.0 + # FIXME: scale from points to pixels + + self.width = xmax - xmin + self.height = ymax - ymin + + self.top_graph = False + + for attr in ("_draw_", "_ldraw_", "_hdraw_", "_tdraw_", "_hldraw_", "_tldraw_"): + if attr in attrs: + parser = XDotAttrParser(self, attrs[attr]) + self.shapes.extend(parser.parse()) + + def handle_node(self, id, attrs): + try: + pos = attrs['pos'] + except KeyError: + return + + x, y = self.parse_node_pos(pos) + w = float(attrs['width'])*72 + h = float(attrs['height'])*72 + shapes = [] + for attr in ("_draw_", "_ldraw_"): + if attr in attrs: + parser = XDotAttrParser(self, attrs[attr]) + shapes.extend(parser.parse()) + url = attrs.get('URL', None) + node = Node(x, y, w, h, shapes, url) + self.node_by_name[id] = node + if shapes: + self.nodes.append(node) + + def handle_edge(self, src_id, dst_id, attrs): + try: + pos = attrs['pos'] + except KeyError: + return + + points = self.parse_edge_pos(pos) + shapes = [] + for attr in ("_draw_", "_ldraw_", "_hdraw_", "_tdraw_", "_hldraw_", "_tldraw_"): + if attr in attrs: + parser = XDotAttrParser(self, attrs[attr]) + shapes.extend(parser.parse()) + url = attrs.get('URL', None) + if shapes: + src = self.node_by_name[src_id] + dst = self.node_by_name[dst_id] + self.edges.append(Edge(src, dst, points, shapes, url)) + + def parse(self): + DotParser.parse(self) + + """ + for k,shapes in self.subgraph_shapes.iteritems(): + self.shapes += shapes + """ + + return Graph(self.width, self.height, self.shapes, self.nodes, self.edges, self.subgraph_shapes) + + def parse_node_pos(self, pos): + x, y = pos.split(",") + return self.transform(float(x), float(y)) + + def parse_edge_pos(self, pos): + points = [] + for entry in pos.split(' '): + fields = entry.split(',') + try: + x, y = fields + except ValueError: + # TODO: handle start/end points + continue + else: + points.append(self.transform(float(x), float(y))) + return points + + def transform(self, x, y): + # XXX: this is not the right place for this code + x = (x + self.xoffset)*self.xscale + y = (y + self.yoffset)*self.yscale + return x, y + + +class Animation(object): + + step = 0.03 # seconds + + def __init__(self, dot_widget): + self.dot_widget = dot_widget + self.timeout_id = None + + def start(self): + self.timeout_id = gobject.timeout_add(int(self.step * 1000), self.tick) + + def stop(self): + self.dot_widget.animation = NoAnimation(self.dot_widget) + if self.timeout_id is not None: + gobject.source_remove(self.timeout_id) + self.timeout_id = None + + def tick(self): + self.stop() + + +class NoAnimation(Animation): + + def start(self): + pass + + def stop(self): + pass + + +class LinearAnimation(Animation): + + duration = 0.6 + + def start(self): + self.started = time.time() + Animation.start(self) + + def tick(self): + t = (time.time() - self.started) / self.duration + self.animate(max(0, min(t, 1))) + return (t < 1) + + def animate(self, t): + pass + + +class MoveToAnimation(LinearAnimation): + + def __init__(self, dot_widget, target_x, target_y): + Animation.__init__(self, dot_widget) + self.source_x = dot_widget.x + self.source_y = dot_widget.y + self.target_x = target_x + self.target_y = target_y + + def animate(self, t): + sx, sy = self.source_x, self.source_y + tx, ty = self.target_x, self.target_y + self.dot_widget.x = tx * t + sx * (1-t) + self.dot_widget.y = ty * t + sy * (1-t) + self.dot_widget.queue_draw() + + +class ZoomToAnimation(MoveToAnimation): + + def __init__(self, dot_widget, target_x, target_y): + MoveToAnimation.__init__(self, dot_widget, target_x, target_y) + self.source_zoom = dot_widget.zoom_ratio + self.target_zoom = self.source_zoom + self.extra_zoom = 0 + + middle_zoom = 0.5 * (self.source_zoom + self.target_zoom) + + distance = math.hypot(self.source_x - self.target_x, + self.source_y - self.target_y) + rect = self.dot_widget.get_allocation() + visible = min(rect.width, rect.height) / self.dot_widget.zoom_ratio + visible *= 0.9 + if distance > 0: + desired_middle_zoom = visible / distance + self.extra_zoom = min(0, 4 * (desired_middle_zoom - middle_zoom)) + + def animate(self, t): + a, b, c = self.source_zoom, self.extra_zoom, self.target_zoom + self.dot_widget.zoom_ratio = c*t + b*t*(1-t) + a*(1-t) + self.dot_widget.zoom_to_fit_on_resize = False + MoveToAnimation.animate(self, t) + + +class DragAction(object): + + def __init__(self, dot_widget): + self.dot_widget = dot_widget + + def on_button_press(self, event): + self.startmousex = self.prevmousex = event.x + self.startmousey = self.prevmousey = event.y + self.start() + + def on_motion_notify(self, event): + if event.is_hint: + x, y, state = event.window.get_pointer() + else: + x, y, state = event.x, event.y, event.state + deltax = self.prevmousex - x + deltay = self.prevmousey - y + self.drag(deltax, deltay) + self.prevmousex = x + self.prevmousey = y + + def on_button_release(self, event): + self.stopmousex = event.x + self.stopmousey = event.y + self.stop() + + def draw(self, cr): + pass + + def start(self): + pass + + def drag(self, deltax, deltay): + pass + + def stop(self): + pass + + def abort(self): + pass + + +class NullAction(DragAction): + + def on_motion_notify(self, event): + if event.is_hint: + x, y, state = event.window.get_pointer() + else: + x, y, state = event.x, event.y, event.state + dot_widget = self.dot_widget + item = dot_widget.get_url(x, y) + if item is None: + item = dot_widget.get_jump(x, y) + if item is not None: + dot_widget.window.set_cursor(gtk.gdk.Cursor(gtk.gdk.HAND2)) + dot_widget.set_highlight(item.highlight) + else: + dot_widget.window.set_cursor(gtk.gdk.Cursor(gtk.gdk.ARROW)) + dot_widget.set_highlight(None) + + +class PanAction(DragAction): + + def start(self): + self.dot_widget.window.set_cursor(gtk.gdk.Cursor(gtk.gdk.FLEUR)) + + def drag(self, deltax, deltay): + self.dot_widget.x += deltax / self.dot_widget.zoom_ratio + self.dot_widget.y += deltay / self.dot_widget.zoom_ratio + self.dot_widget.queue_draw() + + def stop(self): + self.dot_widget.window.set_cursor(gtk.gdk.Cursor(gtk.gdk.ARROW)) + + abort = stop + + +class ZoomAction(DragAction): + + def drag(self, deltax, deltay): + self.dot_widget.zoom_ratio *= 1.005 ** (deltax + deltay) + self.dot_widget.zoom_to_fit_on_resize = False + self.dot_widget.queue_draw() + + def stop(self): + self.dot_widget.queue_draw() + + +class ZoomAreaAction(DragAction): + + def drag(self, deltax, deltay): + self.dot_widget.queue_draw() + + def draw(self, cr): + cr.save() + cr.set_source_rgba(.5, .5, 1.0, 0.25) + cr.rectangle(self.startmousex, self.startmousey, + self.prevmousex - self.startmousex, + self.prevmousey - self.startmousey) + cr.fill() + cr.set_source_rgba(.5, .5, 1.0, 1.0) + cr.set_line_width(1) + cr.rectangle(self.startmousex - .5, self.startmousey - .5, + self.prevmousex - self.startmousex + 1, + self.prevmousey - self.startmousey + 1) + cr.stroke() + cr.restore() + + def stop(self): + x1, y1 = self.dot_widget.window2graph(self.startmousex, + self.startmousey) + x2, y2 = self.dot_widget.window2graph(self.stopmousex, + self.stopmousey) + self.dot_widget.zoom_to_area(x1, y1, x2, y2) + + def abort(self): + self.dot_widget.queue_draw() + + +class DotWidget(gtk.DrawingArea): + """PyGTK widget that draws dot graphs.""" + + __gsignals__ = { + 'expose-event': 'override', + 'clicked' : (gobject.SIGNAL_RUN_LAST, gobject.TYPE_NONE, (gobject.TYPE_STRING, gtk.gdk.Event)) + } + + filter = 'dot' + + def __init__(self): + gtk.DrawingArea.__init__(self) + + self.graph = Graph() + self.openfilename = None + + self.set_flags(gtk.CAN_FOCUS) + + self.add_events(gtk.gdk.BUTTON_PRESS_MASK | gtk.gdk.BUTTON_RELEASE_MASK) + self.connect("button-press-event", self.on_area_button_press) + self.connect("button-release-event", self.on_area_button_release) + self.add_events(gtk.gdk.POINTER_MOTION_MASK | gtk.gdk.POINTER_MOTION_HINT_MASK | gtk.gdk.BUTTON_RELEASE_MASK) + self.connect("motion-notify-event", self.on_area_motion_notify) + self.connect("scroll-event", self.on_area_scroll_event) + self.connect("size-allocate", self.on_area_size_allocate) + + self.connect('key-press-event', self.on_key_press_event) + + self.x, self.y = 0.0, 0.0 + self.zoom_ratio = 1.0 + self.zoom_to_fit_on_resize = False + self.animation = NoAnimation(self) + self.drag_action = NullAction(self) + self.presstime = None + self.highlight = None + + def set_filter(self, filter): + self.filter = filter + + def set_dotcode(self, dotcode, filename=''): + if isinstance(dotcode, unicode): + dotcode = dotcode.encode('utf8') + p = subprocess.Popen( + [self.filter, '-Txdot'], + stdin=subprocess.PIPE, + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + shell=False, + universal_newlines=True + ) + xdotcode, error = p.communicate(dotcode) + if p.returncode != 0: + print("UNABLE TO SHELL TO DOT", error) + dialog = gtk.MessageDialog(type=gtk.MESSAGE_ERROR, + message_format=error, + buttons=gtk.BUTTONS_OK) + dialog.set_title('Dot Viewer') + dialog.run() + dialog.destroy() + return False + try: + self.set_xdotcode(xdotcode) + except ParseError as ex: + dialog = gtk.MessageDialog(type=gtk.MESSAGE_ERROR, + message_format=str(ex), + buttons=gtk.BUTTONS_OK) + dialog.set_title('Dot Viewer') + dialog.run() + dialog.destroy() + return False + else: + self.openfilename = filename + return True + + def set_xdotcode(self, xdotcode): + #print xdotcode + parser = XDotParser(xdotcode) + self.graph = parser.parse() + self.zoom_image(self.zoom_ratio, center=False) + + def reload(self): + if self.openfilename is not None: + try: + fp = file(self.openfilename, 'rt') + self.set_dotcode(fp.read(), self.openfilename) + fp.close() + except IOError: + pass + + def do_expose_event(self, event): + cr = self.window.cairo_create() + + # set a clip region for the expose event + cr.rectangle( + event.area.x, event.area.y, + event.area.width, event.area.height + ) + cr.clip() + + cr.set_source_rgba(1.0, 1.0, 1.0, 1.0) + cr.paint() + + cr.save() + rect = self.get_allocation() + cr.translate(0.5*rect.width, 0.5*rect.height) + cr.scale(self.zoom_ratio, self.zoom_ratio) + cr.translate(-self.x, -self.y) + + self.graph.draw(cr, highlight_items=self.highlight) + cr.restore() + + self.drag_action.draw(cr) + + return False + + def get_current_pos(self): + return self.x, self.y + + def set_current_pos(self, x, y): + self.x = x + self.y = y + self.queue_draw() + + def set_highlight(self, items): + if self.highlight != items: + self.highlight = items + self.queue_draw() + + def zoom_image(self, zoom_ratio, center=False, pos=None): + if center: + self.x = self.graph.width/2 + self.y = self.graph.height/2 + elif pos is not None: + rect = self.get_allocation() + x, y = pos + x -= 0.5*rect.width + y -= 0.5*rect.height + self.x += x / self.zoom_ratio - x / zoom_ratio + self.y += y / self.zoom_ratio - y / zoom_ratio + self.zoom_ratio = zoom_ratio + self.zoom_to_fit_on_resize = False + self.queue_draw() + + def zoom_to_area(self, x1, y1, x2, y2): + rect = self.get_allocation() + width = abs(x1 - x2) + height = abs(y1 - y2) + self.zoom_ratio = min( + float(rect.width)/float(width), + float(rect.height)/float(height) + ) + self.zoom_to_fit_on_resize = False + self.x = (x1 + x2) / 2 + self.y = (y1 + y2) / 2 + self.queue_draw() + + def zoom_to_fit(self): + rect = self.get_allocation() + rect.x += self.ZOOM_TO_FIT_MARGIN + rect.y += self.ZOOM_TO_FIT_MARGIN + rect.width -= 2 * self.ZOOM_TO_FIT_MARGIN + rect.height -= 2 * self.ZOOM_TO_FIT_MARGIN + zoom_ratio = min( + float(rect.width)/float(self.graph.width), + float(rect.height)/float(self.graph.height) + ) + self.zoom_image(zoom_ratio, center=True) + self.zoom_to_fit_on_resize = True + + ZOOM_INCREMENT = 1.25 + ZOOM_TO_FIT_MARGIN = 12 + + def on_zoom_in(self, action): + self.zoom_image(self.zoom_ratio * self.ZOOM_INCREMENT) + + def on_zoom_out(self, action): + self.zoom_image(self.zoom_ratio / self.ZOOM_INCREMENT) + + def on_zoom_fit(self, action): + self.zoom_to_fit() + + def on_zoom_100(self, action): + self.zoom_image(1.0) + + POS_INCREMENT = 100 + + def on_key_press_event(self, widget, event): + if event.keyval == gtk.keysyms.Left: + self.x -= self.POS_INCREMENT/self.zoom_ratio + self.queue_draw() + return True + if event.keyval == gtk.keysyms.Right: + self.x += self.POS_INCREMENT/self.zoom_ratio + self.queue_draw() + return True + if event.keyval == gtk.keysyms.Up: + self.y -= self.POS_INCREMENT/self.zoom_ratio + self.queue_draw() + return True + if event.keyval == gtk.keysyms.Down: + self.y += self.POS_INCREMENT/self.zoom_ratio + self.queue_draw() + return True + if event.keyval == gtk.keysyms.Page_Up: + self.zoom_image(self.zoom_ratio * self.ZOOM_INCREMENT) + self.queue_draw() + return True + if event.keyval == gtk.keysyms.Page_Down: + self.zoom_image(self.zoom_ratio / self.ZOOM_INCREMENT) + self.queue_draw() + return True + if event.keyval == gtk.keysyms.Escape: + self.drag_action.abort() + self.drag_action = NullAction(self) + return True + if event.keyval == gtk.keysyms.r: + self.reload() + return True + if event.keyval == gtk.keysyms.q: + gtk.main_quit() + return True + return False + + def get_drag_action(self, event): + state = event.state + if event.button in (1, 2): # left or middle button + if state & gtk.gdk.CONTROL_MASK: + return ZoomAction + elif state & gtk.gdk.SHIFT_MASK: + return ZoomAreaAction + else: + return PanAction + return NullAction + + def on_area_button_press(self, area, event): + self.animation.stop() + self.drag_action.abort() + action_type = self.get_drag_action(event) + self.drag_action = action_type(self) + self.drag_action.on_button_press(event) + self.presstime = time.time() + self.pressx = event.x + self.pressy = event.y + return False + + def is_click(self, event, click_fuzz=4, click_timeout=1.0): + assert event.type == gtk.gdk.BUTTON_RELEASE + if self.presstime is None: + # got a button release without seeing the press? + return False + # XXX instead of doing this complicated logic, shouldn't we listen + # for gtk's clicked event instead? + deltax = self.pressx - event.x + deltay = self.pressy - event.y + return (time.time() < self.presstime + click_timeout + and math.hypot(deltax, deltay) < click_fuzz) + + def on_area_button_release(self, area, event): + self.drag_action.on_button_release(event) + self.drag_action = NullAction(self) + if event.button == 1 and self.is_click(event): + x, y = int(event.x), int(event.y) + url = self.get_url(x, y) + if url is not None: + self.emit('clicked', unicode(url.url), event) + else: + jump = self.get_jump(x, y) + if jump is not None: + self.animate_to(jump.x, jump.y) + + return True + if event.button == 1 or event.button == 2: + return True + return False + + def on_area_scroll_event(self, area, event): + if event.direction == gtk.gdk.SCROLL_UP: + self.zoom_image(self.zoom_ratio * self.ZOOM_INCREMENT, + pos=(event.x, event.y)) + return True + if event.direction == gtk.gdk.SCROLL_DOWN: + self.zoom_image(self.zoom_ratio / self.ZOOM_INCREMENT, + pos=(event.x, event.y)) + return True + return False + + def on_area_motion_notify(self, area, event): + self.drag_action.on_motion_notify(event) + return True + + def on_area_size_allocate(self, area, allocation): + if self.zoom_to_fit_on_resize: + self.zoom_to_fit() + + def animate_to(self, x, y): + self.animation = ZoomToAnimation(self, x, y) + self.animation.start() + + def window2graph(self, x, y): + rect = self.get_allocation() + x -= 0.5*rect.width + y -= 0.5*rect.height + x /= self.zoom_ratio + y /= self.zoom_ratio + x += self.x + y += self.y + return x, y + + def get_url(self, x, y): + x, y = self.window2graph(x, y) + return self.graph.get_url(x, y) + + def get_jump(self, x, y): + x, y = self.window2graph(x, y) + return self.graph.get_jump(x, y) + + +class DotWindow(gtk.Window): + + ui = ''' + + + + + + + + + + + + ''' + + def __init__(self): + gtk.Window.__init__(self) + + self.graph = Graph() + + window = self + + window.set_title('Dot Viewer') + window.set_default_size(512, 512) + vbox = gtk.VBox() + window.add(vbox) + + self.widget = DotWidget() + + # Create a UIManager instance + uimanager = self.uimanager = gtk.UIManager() + + # Add the accelerator group to the toplevel window + accelgroup = uimanager.get_accel_group() + window.add_accel_group(accelgroup) + + # Create an ActionGroup + actiongroup = gtk.ActionGroup('Actions') + self.actiongroup = actiongroup + + # Create actions + actiongroup.add_actions(( + ('Open', gtk.STOCK_OPEN, None, None, None, self.on_open), + ('Reload', gtk.STOCK_REFRESH, None, None, None, self.on_reload), + ('ZoomIn', gtk.STOCK_ZOOM_IN, None, None, None, self.widget.on_zoom_in), + ('ZoomOut', gtk.STOCK_ZOOM_OUT, None, None, None, self.widget.on_zoom_out), + ('ZoomFit', gtk.STOCK_ZOOM_FIT, None, None, None, self.widget.on_zoom_fit), + ('Zoom100', gtk.STOCK_ZOOM_100, None, None, None, self.widget.on_zoom_100), + )) + + # Add the actiongroup to the uimanager + uimanager.insert_action_group(actiongroup, 0) + + # Add a UI descrption + uimanager.add_ui_from_string(self.ui) + + # Create a Toolbar + toolbar = uimanager.get_widget('/ToolBar') + vbox.pack_start(toolbar, False) + + vbox.pack_start(self.widget) + + self.set_focus(self.widget) + + self.show_all() + + def update(self, filename): + import os + if not hasattr(self, "last_mtime"): + self.last_mtime = None + + current_mtime = os.stat(filename).st_mtime + if current_mtime != self.last_mtime: + self.last_mtime = current_mtime + self.open_file(filename,True) + + return True + + def set_filter(self, filter): + self.widget.set_filter(filter) + + def set_dotcode(self, dotcode, filename='',refresh=False): + if self.widget.set_dotcode(dotcode, filename): + self.set_title(os.path.basename(filename) + ' - Dot Viewer') + if not refresh: + self.widget.zoom_to_fit() + + def set_xdotcode(self, xdotcode, filename=''): + if self.widget.set_xdotcode(xdotcode): + self.set_title(os.path.basename(filename) + ' - Dot Viewer') + self.widget.zoom_to_fit() + + def open_file(self, filename, refresh=False): + try: + fp = file(filename, 'rt') + self.set_dotcode(fp.read(), filename ,refresh) + fp.close() + except IOError as ex: + dlg = gtk.MessageDialog(type=gtk.MESSAGE_ERROR, + message_format=str(ex), + buttons=gtk.BUTTONS_OK) + dlg.set_title('Dot Viewer') + dlg.run() + dlg.destroy() + + def on_open(self, action): + chooser = gtk.FileChooserDialog(title="Open dot File", + action=gtk.FILE_CHOOSER_ACTION_OPEN, + buttons=(gtk.STOCK_CANCEL, + gtk.RESPONSE_CANCEL, + gtk.STOCK_OPEN, + gtk.RESPONSE_OK)) + chooser.set_default_response(gtk.RESPONSE_OK) + filter = gtk.FileFilter() + filter.set_name("Graphviz dot files") + filter.add_pattern("*.dot") + chooser.add_filter(filter) + filter = gtk.FileFilter() + filter.set_name("All files") + filter.add_pattern("*") + chooser.add_filter(filter) + if chooser.run() == gtk.RESPONSE_OK: + filename = chooser.get_filename() + chooser.destroy() + self.open_file(filename) + else: + chooser.destroy() + + def on_reload(self, action): + self.widget.reload() + + +def main(): + import optparse + + parser = optparse.OptionParser( + usage='\n\t%prog [file]', + version='%%prog %s' % __version__) + parser.add_option( + '-f', '--filter', + type='choice', choices=('dot', 'neato', 'twopi', 'circo', 'fdp'), + dest='filter', default='dot', + help='graphviz filter: dot, neato, twopi, circo, or fdp [default: %default]') + + (options, args) = parser.parse_args(sys.argv[1:]) + if len(args) > 1: + parser.error('incorrect number of arguments') + + win = DotWindow() + win.connect('destroy', gtk.main_quit) + win.set_filter(options.filter) + if len(args) >= 1: + if args[0] == '-': + win.set_dotcode(sys.stdin.read()) + else: + win.open_file(args[0]) + gobject.timeout_add(1000, win.update, args[0]) + gtk.main() + + +# Apache-Style Software License for ColorBrewer software and ColorBrewer Color +# Schemes, Version 1.1 +# +# Copyright (c) 2002 Cynthia Brewer, Mark Harrower, and The Pennsylvania State +# University. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions as source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. The end-user documentation included with the redistribution, if any, +# must include the following acknowledgment: +# +# This product includes color specifications and designs developed by +# Cynthia Brewer (http://colorbrewer.org/). +# +# Alternately, this acknowledgment may appear in the software itself, if and +# wherever such third-party acknowledgments normally appear. +# +# 3. The name "ColorBrewer" must not be used to endorse or promote products +# derived from this software without prior written permission. For written +# permission, please contact Cynthia Brewer at cbrewer@psu.edu. +# +# 4. Products derived from this software may not be called "ColorBrewer", +# nor may "ColorBrewer" appear in their name, without prior written +# permission of Cynthia Brewer. +# +# THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESSED OR IMPLIED WARRANTIES, +# INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND +# FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL CYNTHIA +# BREWER, MARK HARROWER, OR THE PENNSYLVANIA STATE UNIVERSITY BE LIABLE FOR ANY +# DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +# (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +# ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +# SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +brewer_colors = { + 'accent3': [(127, 201, 127), (190, 174, 212), (253, 192, 134)], + 'accent4': [(127, 201, 127), (190, 174, 212), (253, 192, 134), (255, 255, 153)], + 'accent5': [(127, 201, 127), (190, 174, 212), (253, 192, 134), (255, 255, 153), (56, 108, 176)], + 'accent6': [(127, 201, 127), (190, 174, 212), (253, 192, 134), (255, 255, 153), (56, 108, 176), (240, 2, 127)], + 'accent7': [(127, 201, 127), (190, 174, 212), (253, 192, 134), (255, 255, 153), (56, 108, 176), (240, 2, 127), (191, 91, 23)], + 'accent8': [(127, 201, 127), (190, 174, 212), (253, 192, 134), (255, 255, 153), (56, 108, 176), (240, 2, 127), (191, 91, 23), (102, 102, 102)], + 'blues3': [(222, 235, 247), (158, 202, 225), (49, 130, 189)], + 'blues4': [(239, 243, 255), (189, 215, 231), (107, 174, 214), (33, 113, 181)], + 'blues5': [(239, 243, 255), (189, 215, 231), (107, 174, 214), (49, 130, 189), (8, 81, 156)], + 'blues6': [(239, 243, 255), (198, 219, 239), (158, 202, 225), (107, 174, 214), (49, 130, 189), (8, 81, 156)], + 'blues7': [(239, 243, 255), (198, 219, 239), (158, 202, 225), (107, 174, 214), (66, 146, 198), (33, 113, 181), (8, 69, 148)], + 'blues8': [(247, 251, 255), (222, 235, 247), (198, 219, 239), (158, 202, 225), (107, 174, 214), (66, 146, 198), (33, 113, 181), (8, 69, 148)], + 'blues9': [(247, 251, 255), (222, 235, 247), (198, 219, 239), (158, 202, 225), (107, 174, 214), (66, 146, 198), (33, 113, 181), (8, 81, 156), (8, 48, 107)], + 'brbg10': [(84, 48, 5), (0, 60, 48), (140, 81, 10), (191, 129, 45), (223, 194, 125), (246, 232, 195), (199, 234, 229), (128, 205, 193), (53, 151, 143), (1, 102, 94)], + 'brbg11': [(84, 48, 5), (1, 102, 94), (0, 60, 48), (140, 81, 10), (191, 129, 45), (223, 194, 125), (246, 232, 195), (245, 245, 245), (199, 234, 229), (128, 205, 193), (53, 151, 143)], + 'brbg3': [(216, 179, 101), (245, 245, 245), (90, 180, 172)], + 'brbg4': [(166, 97, 26), (223, 194, 125), (128, 205, 193), (1, 133, 113)], + 'brbg5': [(166, 97, 26), (223, 194, 125), (245, 245, 245), (128, 205, 193), (1, 133, 113)], + 'brbg6': [(140, 81, 10), (216, 179, 101), (246, 232, 195), (199, 234, 229), (90, 180, 172), (1, 102, 94)], + 'brbg7': [(140, 81, 10), (216, 179, 101), (246, 232, 195), (245, 245, 245), (199, 234, 229), (90, 180, 172), (1, 102, 94)], + 'brbg8': [(140, 81, 10), (191, 129, 45), (223, 194, 125), (246, 232, 195), (199, 234, 229), (128, 205, 193), (53, 151, 143), (1, 102, 94)], + 'brbg9': [(140, 81, 10), (191, 129, 45), (223, 194, 125), (246, 232, 195), (245, 245, 245), (199, 234, 229), (128, 205, 193), (53, 151, 143), (1, 102, 94)], + 'bugn3': [(229, 245, 249), (153, 216, 201), (44, 162, 95)], + 'bugn4': [(237, 248, 251), (178, 226, 226), (102, 194, 164), (35, 139, 69)], + 'bugn5': [(237, 248, 251), (178, 226, 226), (102, 194, 164), (44, 162, 95), (0, 109, 44)], + 'bugn6': [(237, 248, 251), (204, 236, 230), (153, 216, 201), (102, 194, 164), (44, 162, 95), (0, 109, 44)], + 'bugn7': [(237, 248, 251), (204, 236, 230), (153, 216, 201), (102, 194, 164), (65, 174, 118), (35, 139, 69), (0, 88, 36)], + 'bugn8': [(247, 252, 253), (229, 245, 249), (204, 236, 230), (153, 216, 201), (102, 194, 164), (65, 174, 118), (35, 139, 69), (0, 88, 36)], + 'bugn9': [(247, 252, 253), (229, 245, 249), (204, 236, 230), (153, 216, 201), (102, 194, 164), (65, 174, 118), (35, 139, 69), (0, 109, 44), (0, 68, 27)], + 'bupu3': [(224, 236, 244), (158, 188, 218), (136, 86, 167)], + 'bupu4': [(237, 248, 251), (179, 205, 227), (140, 150, 198), (136, 65, 157)], + 'bupu5': [(237, 248, 251), (179, 205, 227), (140, 150, 198), (136, 86, 167), (129, 15, 124)], + 'bupu6': [(237, 248, 251), (191, 211, 230), (158, 188, 218), (140, 150, 198), (136, 86, 167), (129, 15, 124)], + 'bupu7': [(237, 248, 251), (191, 211, 230), (158, 188, 218), (140, 150, 198), (140, 107, 177), (136, 65, 157), (110, 1, 107)], + 'bupu8': [(247, 252, 253), (224, 236, 244), (191, 211, 230), (158, 188, 218), (140, 150, 198), (140, 107, 177), (136, 65, 157), (110, 1, 107)], + 'bupu9': [(247, 252, 253), (224, 236, 244), (191, 211, 230), (158, 188, 218), (140, 150, 198), (140, 107, 177), (136, 65, 157), (129, 15, 124), (77, 0, 75)], + 'dark23': [(27, 158, 119), (217, 95, 2), (117, 112, 179)], + 'dark24': [(27, 158, 119), (217, 95, 2), (117, 112, 179), (231, 41, 138)], + 'dark25': [(27, 158, 119), (217, 95, 2), (117, 112, 179), (231, 41, 138), (102, 166, 30)], + 'dark26': [(27, 158, 119), (217, 95, 2), (117, 112, 179), (231, 41, 138), (102, 166, 30), (230, 171, 2)], + 'dark27': [(27, 158, 119), (217, 95, 2), (117, 112, 179), (231, 41, 138), (102, 166, 30), (230, 171, 2), (166, 118, 29)], + 'dark28': [(27, 158, 119), (217, 95, 2), (117, 112, 179), (231, 41, 138), (102, 166, 30), (230, 171, 2), (166, 118, 29), (102, 102, 102)], + 'gnbu3': [(224, 243, 219), (168, 221, 181), (67, 162, 202)], + 'gnbu4': [(240, 249, 232), (186, 228, 188), (123, 204, 196), (43, 140, 190)], + 'gnbu5': [(240, 249, 232), (186, 228, 188), (123, 204, 196), (67, 162, 202), (8, 104, 172)], + 'gnbu6': [(240, 249, 232), (204, 235, 197), (168, 221, 181), (123, 204, 196), (67, 162, 202), (8, 104, 172)], + 'gnbu7': [(240, 249, 232), (204, 235, 197), (168, 221, 181), (123, 204, 196), (78, 179, 211), (43, 140, 190), (8, 88, 158)], + 'gnbu8': [(247, 252, 240), (224, 243, 219), (204, 235, 197), (168, 221, 181), (123, 204, 196), (78, 179, 211), (43, 140, 190), (8, 88, 158)], + 'gnbu9': [(247, 252, 240), (224, 243, 219), (204, 235, 197), (168, 221, 181), (123, 204, 196), (78, 179, 211), (43, 140, 190), (8, 104, 172), (8, 64, 129)], + 'greens3': [(229, 245, 224), (161, 217, 155), (49, 163, 84)], + 'greens4': [(237, 248, 233), (186, 228, 179), (116, 196, 118), (35, 139, 69)], + 'greens5': [(237, 248, 233), (186, 228, 179), (116, 196, 118), (49, 163, 84), (0, 109, 44)], + 'greens6': [(237, 248, 233), (199, 233, 192), (161, 217, 155), (116, 196, 118), (49, 163, 84), (0, 109, 44)], + 'greens7': [(237, 248, 233), (199, 233, 192), (161, 217, 155), (116, 196, 118), (65, 171, 93), (35, 139, 69), (0, 90, 50)], + 'greens8': [(247, 252, 245), (229, 245, 224), (199, 233, 192), (161, 217, 155), (116, 196, 118), (65, 171, 93), (35, 139, 69), (0, 90, 50)], + 'greens9': [(247, 252, 245), (229, 245, 224), (199, 233, 192), (161, 217, 155), (116, 196, 118), (65, 171, 93), (35, 139, 69), (0, 109, 44), (0, 68, 27)], + 'greys3': [(240, 240, 240), (189, 189, 189), (99, 99, 99)], + 'greys4': [(247, 247, 247), (204, 204, 204), (150, 150, 150), (82, 82, 82)], + 'greys5': [(247, 247, 247), (204, 204, 204), (150, 150, 150), (99, 99, 99), (37, 37, 37)], + 'greys6': [(247, 247, 247), (217, 217, 217), (189, 189, 189), (150, 150, 150), (99, 99, 99), (37, 37, 37)], + 'greys7': [(247, 247, 247), (217, 217, 217), (189, 189, 189), (150, 150, 150), (115, 115, 115), (82, 82, 82), (37, 37, 37)], + 'greys8': [(255, 255, 255), (240, 240, 240), (217, 217, 217), (189, 189, 189), (150, 150, 150), (115, 115, 115), (82, 82, 82), (37, 37, 37)], + 'greys9': [(255, 255, 255), (240, 240, 240), (217, 217, 217), (189, 189, 189), (150, 150, 150), (115, 115, 115), (82, 82, 82), (37, 37, 37), (0, 0, 0)], + 'oranges3': [(254, 230, 206), (253, 174, 107), (230, 85, 13)], + 'oranges4': [(254, 237, 222), (253, 190, 133), (253, 141, 60), (217, 71, 1)], + 'oranges5': [(254, 237, 222), (253, 190, 133), (253, 141, 60), (230, 85, 13), (166, 54, 3)], + 'oranges6': [(254, 237, 222), (253, 208, 162), (253, 174, 107), (253, 141, 60), (230, 85, 13), (166, 54, 3)], + 'oranges7': [(254, 237, 222), (253, 208, 162), (253, 174, 107), (253, 141, 60), (241, 105, 19), (217, 72, 1), (140, 45, 4)], + 'oranges8': [(255, 245, 235), (254, 230, 206), (253, 208, 162), (253, 174, 107), (253, 141, 60), (241, 105, 19), (217, 72, 1), (140, 45, 4)], + 'oranges9': [(255, 245, 235), (254, 230, 206), (253, 208, 162), (253, 174, 107), (253, 141, 60), (241, 105, 19), (217, 72, 1), (166, 54, 3), (127, 39, 4)], + 'orrd3': [(254, 232, 200), (253, 187, 132), (227, 74, 51)], + 'orrd4': [(254, 240, 217), (253, 204, 138), (252, 141, 89), (215, 48, 31)], + 'orrd5': [(254, 240, 217), (253, 204, 138), (252, 141, 89), (227, 74, 51), (179, 0, 0)], + 'orrd6': [(254, 240, 217), (253, 212, 158), (253, 187, 132), (252, 141, 89), (227, 74, 51), (179, 0, 0)], + 'orrd7': [(254, 240, 217), (253, 212, 158), (253, 187, 132), (252, 141, 89), (239, 101, 72), (215, 48, 31), (153, 0, 0)], + 'orrd8': [(255, 247, 236), (254, 232, 200), (253, 212, 158), (253, 187, 132), (252, 141, 89), (239, 101, 72), (215, 48, 31), (153, 0, 0)], + 'orrd9': [(255, 247, 236), (254, 232, 200), (253, 212, 158), (253, 187, 132), (252, 141, 89), (239, 101, 72), (215, 48, 31), (179, 0, 0), (127, 0, 0)], + 'paired10': [(166, 206, 227), (106, 61, 154), (31, 120, 180), (178, 223, 138), (51, 160, 44), (251, 154, 153), (227, 26, 28), (253, 191, 111), (255, 127, 0), (202, 178, 214)], + 'paired11': [(166, 206, 227), (106, 61, 154), (255, 255, 153), (31, 120, 180), (178, 223, 138), (51, 160, 44), (251, 154, 153), (227, 26, 28), (253, 191, 111), (255, 127, 0), (202, 178, 214)], + 'paired12': [(166, 206, 227), (106, 61, 154), (255, 255, 153), (177, 89, 40), (31, 120, 180), (178, 223, 138), (51, 160, 44), (251, 154, 153), (227, 26, 28), (253, 191, 111), (255, 127, 0), (202, 178, 214)], + 'paired3': [(166, 206, 227), (31, 120, 180), (178, 223, 138)], + 'paired4': [(166, 206, 227), (31, 120, 180), (178, 223, 138), (51, 160, 44)], + 'paired5': [(166, 206, 227), (31, 120, 180), (178, 223, 138), (51, 160, 44), (251, 154, 153)], + 'paired6': [(166, 206, 227), (31, 120, 180), (178, 223, 138), (51, 160, 44), (251, 154, 153), (227, 26, 28)], + 'paired7': [(166, 206, 227), (31, 120, 180), (178, 223, 138), (51, 160, 44), (251, 154, 153), (227, 26, 28), (253, 191, 111)], + 'paired8': [(166, 206, 227), (31, 120, 180), (178, 223, 138), (51, 160, 44), (251, 154, 153), (227, 26, 28), (253, 191, 111), (255, 127, 0)], + 'paired9': [(166, 206, 227), (31, 120, 180), (178, 223, 138), (51, 160, 44), (251, 154, 153), (227, 26, 28), (253, 191, 111), (255, 127, 0), (202, 178, 214)], + 'pastel13': [(251, 180, 174), (179, 205, 227), (204, 235, 197)], + 'pastel14': [(251, 180, 174), (179, 205, 227), (204, 235, 197), (222, 203, 228)], + 'pastel15': [(251, 180, 174), (179, 205, 227), (204, 235, 197), (222, 203, 228), (254, 217, 166)], + 'pastel16': [(251, 180, 174), (179, 205, 227), (204, 235, 197), (222, 203, 228), (254, 217, 166), (255, 255, 204)], + 'pastel17': [(251, 180, 174), (179, 205, 227), (204, 235, 197), (222, 203, 228), (254, 217, 166), (255, 255, 204), (229, 216, 189)], + 'pastel18': [(251, 180, 174), (179, 205, 227), (204, 235, 197), (222, 203, 228), (254, 217, 166), (255, 255, 204), (229, 216, 189), (253, 218, 236)], + 'pastel19': [(251, 180, 174), (179, 205, 227), (204, 235, 197), (222, 203, 228), (254, 217, 166), (255, 255, 204), (229, 216, 189), (253, 218, 236), (242, 242, 242)], + 'pastel23': [(179, 226, 205), (253, 205, 172), (203, 213, 232)], + 'pastel24': [(179, 226, 205), (253, 205, 172), (203, 213, 232), (244, 202, 228)], + 'pastel25': [(179, 226, 205), (253, 205, 172), (203, 213, 232), (244, 202, 228), (230, 245, 201)], + 'pastel26': [(179, 226, 205), (253, 205, 172), (203, 213, 232), (244, 202, 228), (230, 245, 201), (255, 242, 174)], + 'pastel27': [(179, 226, 205), (253, 205, 172), (203, 213, 232), (244, 202, 228), (230, 245, 201), (255, 242, 174), (241, 226, 204)], + 'pastel28': [(179, 226, 205), (253, 205, 172), (203, 213, 232), (244, 202, 228), (230, 245, 201), (255, 242, 174), (241, 226, 204), (204, 204, 204)], + 'piyg10': [(142, 1, 82), (39, 100, 25), (197, 27, 125), (222, 119, 174), (241, 182, 218), (253, 224, 239), (230, 245, 208), (184, 225, 134), (127, 188, 65), (77, 146, 33)], + 'piyg11': [(142, 1, 82), (77, 146, 33), (39, 100, 25), (197, 27, 125), (222, 119, 174), (241, 182, 218), (253, 224, 239), (247, 247, 247), (230, 245, 208), (184, 225, 134), (127, 188, 65)], + 'piyg3': [(233, 163, 201), (247, 247, 247), (161, 215, 106)], + 'piyg4': [(208, 28, 139), (241, 182, 218), (184, 225, 134), (77, 172, 38)], + 'piyg5': [(208, 28, 139), (241, 182, 218), (247, 247, 247), (184, 225, 134), (77, 172, 38)], + 'piyg6': [(197, 27, 125), (233, 163, 201), (253, 224, 239), (230, 245, 208), (161, 215, 106), (77, 146, 33)], + 'piyg7': [(197, 27, 125), (233, 163, 201), (253, 224, 239), (247, 247, 247), (230, 245, 208), (161, 215, 106), (77, 146, 33)], + 'piyg8': [(197, 27, 125), (222, 119, 174), (241, 182, 218), (253, 224, 239), (230, 245, 208), (184, 225, 134), (127, 188, 65), (77, 146, 33)], + 'piyg9': [(197, 27, 125), (222, 119, 174), (241, 182, 218), (253, 224, 239), (247, 247, 247), (230, 245, 208), (184, 225, 134), (127, 188, 65), (77, 146, 33)], + 'prgn10': [(64, 0, 75), (0, 68, 27), (118, 42, 131), (153, 112, 171), (194, 165, 207), (231, 212, 232), (217, 240, 211), (166, 219, 160), (90, 174, 97), (27, 120, 55)], + 'prgn11': [(64, 0, 75), (27, 120, 55), (0, 68, 27), (118, 42, 131), (153, 112, 171), (194, 165, 207), (231, 212, 232), (247, 247, 247), (217, 240, 211), (166, 219, 160), (90, 174, 97)], + 'prgn3': [(175, 141, 195), (247, 247, 247), (127, 191, 123)], + 'prgn4': [(123, 50, 148), (194, 165, 207), (166, 219, 160), (0, 136, 55)], + 'prgn5': [(123, 50, 148), (194, 165, 207), (247, 247, 247), (166, 219, 160), (0, 136, 55)], + 'prgn6': [(118, 42, 131), (175, 141, 195), (231, 212, 232), (217, 240, 211), (127, 191, 123), (27, 120, 55)], + 'prgn7': [(118, 42, 131), (175, 141, 195), (231, 212, 232), (247, 247, 247), (217, 240, 211), (127, 191, 123), (27, 120, 55)], + 'prgn8': [(118, 42, 131), (153, 112, 171), (194, 165, 207), (231, 212, 232), (217, 240, 211), (166, 219, 160), (90, 174, 97), (27, 120, 55)], + 'prgn9': [(118, 42, 131), (153, 112, 171), (194, 165, 207), (231, 212, 232), (247, 247, 247), (217, 240, 211), (166, 219, 160), (90, 174, 97), (27, 120, 55)], + 'pubu3': [(236, 231, 242), (166, 189, 219), (43, 140, 190)], + 'pubu4': [(241, 238, 246), (189, 201, 225), (116, 169, 207), (5, 112, 176)], + 'pubu5': [(241, 238, 246), (189, 201, 225), (116, 169, 207), (43, 140, 190), (4, 90, 141)], + 'pubu6': [(241, 238, 246), (208, 209, 230), (166, 189, 219), (116, 169, 207), (43, 140, 190), (4, 90, 141)], + 'pubu7': [(241, 238, 246), (208, 209, 230), (166, 189, 219), (116, 169, 207), (54, 144, 192), (5, 112, 176), (3, 78, 123)], + 'pubu8': [(255, 247, 251), (236, 231, 242), (208, 209, 230), (166, 189, 219), (116, 169, 207), (54, 144, 192), (5, 112, 176), (3, 78, 123)], + 'pubu9': [(255, 247, 251), (236, 231, 242), (208, 209, 230), (166, 189, 219), (116, 169, 207), (54, 144, 192), (5, 112, 176), (4, 90, 141), (2, 56, 88)], + 'pubugn3': [(236, 226, 240), (166, 189, 219), (28, 144, 153)], + 'pubugn4': [(246, 239, 247), (189, 201, 225), (103, 169, 207), (2, 129, 138)], + 'pubugn5': [(246, 239, 247), (189, 201, 225), (103, 169, 207), (28, 144, 153), (1, 108, 89)], + 'pubugn6': [(246, 239, 247), (208, 209, 230), (166, 189, 219), (103, 169, 207), (28, 144, 153), (1, 108, 89)], + 'pubugn7': [(246, 239, 247), (208, 209, 230), (166, 189, 219), (103, 169, 207), (54, 144, 192), (2, 129, 138), (1, 100, 80)], + 'pubugn8': [(255, 247, 251), (236, 226, 240), (208, 209, 230), (166, 189, 219), (103, 169, 207), (54, 144, 192), (2, 129, 138), (1, 100, 80)], + 'pubugn9': [(255, 247, 251), (236, 226, 240), (208, 209, 230), (166, 189, 219), (103, 169, 207), (54, 144, 192), (2, 129, 138), (1, 108, 89), (1, 70, 54)], + 'puor10': [(127, 59, 8), (45, 0, 75), (179, 88, 6), (224, 130, 20), (253, 184, 99), (254, 224, 182), (216, 218, 235), (178, 171, 210), (128, 115, 172), (84, 39, 136)], + 'puor11': [(127, 59, 8), (84, 39, 136), (45, 0, 75), (179, 88, 6), (224, 130, 20), (253, 184, 99), (254, 224, 182), (247, 247, 247), (216, 218, 235), (178, 171, 210), (128, 115, 172)], + 'puor3': [(241, 163, 64), (247, 247, 247), (153, 142, 195)], + 'puor4': [(230, 97, 1), (253, 184, 99), (178, 171, 210), (94, 60, 153)], + 'puor5': [(230, 97, 1), (253, 184, 99), (247, 247, 247), (178, 171, 210), (94, 60, 153)], + 'puor6': [(179, 88, 6), (241, 163, 64), (254, 224, 182), (216, 218, 235), (153, 142, 195), (84, 39, 136)], + 'puor7': [(179, 88, 6), (241, 163, 64), (254, 224, 182), (247, 247, 247), (216, 218, 235), (153, 142, 195), (84, 39, 136)], + 'puor8': [(179, 88, 6), (224, 130, 20), (253, 184, 99), (254, 224, 182), (216, 218, 235), (178, 171, 210), (128, 115, 172), (84, 39, 136)], + 'puor9': [(179, 88, 6), (224, 130, 20), (253, 184, 99), (254, 224, 182), (247, 247, 247), (216, 218, 235), (178, 171, 210), (128, 115, 172), (84, 39, 136)], + 'purd3': [(231, 225, 239), (201, 148, 199), (221, 28, 119)], + 'purd4': [(241, 238, 246), (215, 181, 216), (223, 101, 176), (206, 18, 86)], + 'purd5': [(241, 238, 246), (215, 181, 216), (223, 101, 176), (221, 28, 119), (152, 0, 67)], + 'purd6': [(241, 238, 246), (212, 185, 218), (201, 148, 199), (223, 101, 176), (221, 28, 119), (152, 0, 67)], + 'purd7': [(241, 238, 246), (212, 185, 218), (201, 148, 199), (223, 101, 176), (231, 41, 138), (206, 18, 86), (145, 0, 63)], + 'purd8': [(247, 244, 249), (231, 225, 239), (212, 185, 218), (201, 148, 199), (223, 101, 176), (231, 41, 138), (206, 18, 86), (145, 0, 63)], + 'purd9': [(247, 244, 249), (231, 225, 239), (212, 185, 218), (201, 148, 199), (223, 101, 176), (231, 41, 138), (206, 18, 86), (152, 0, 67), (103, 0, 31)], + 'purples3': [(239, 237, 245), (188, 189, 220), (117, 107, 177)], + 'purples4': [(242, 240, 247), (203, 201, 226), (158, 154, 200), (106, 81, 163)], + 'purples5': [(242, 240, 247), (203, 201, 226), (158, 154, 200), (117, 107, 177), (84, 39, 143)], + 'purples6': [(242, 240, 247), (218, 218, 235), (188, 189, 220), (158, 154, 200), (117, 107, 177), (84, 39, 143)], + 'purples7': [(242, 240, 247), (218, 218, 235), (188, 189, 220), (158, 154, 200), (128, 125, 186), (106, 81, 163), (74, 20, 134)], + 'purples8': [(252, 251, 253), (239, 237, 245), (218, 218, 235), (188, 189, 220), (158, 154, 200), (128, 125, 186), (106, 81, 163), (74, 20, 134)], + 'purples9': [(252, 251, 253), (239, 237, 245), (218, 218, 235), (188, 189, 220), (158, 154, 200), (128, 125, 186), (106, 81, 163), (84, 39, 143), (63, 0, 125)], + 'rdbu10': [(103, 0, 31), (5, 48, 97), (178, 24, 43), (214, 96, 77), (244, 165, 130), (253, 219, 199), (209, 229, 240), (146, 197, 222), (67, 147, 195), (33, 102, 172)], + 'rdbu11': [(103, 0, 31), (33, 102, 172), (5, 48, 97), (178, 24, 43), (214, 96, 77), (244, 165, 130), (253, 219, 199), (247, 247, 247), (209, 229, 240), (146, 197, 222), (67, 147, 195)], + 'rdbu3': [(239, 138, 98), (247, 247, 247), (103, 169, 207)], + 'rdbu4': [(202, 0, 32), (244, 165, 130), (146, 197, 222), (5, 113, 176)], + 'rdbu5': [(202, 0, 32), (244, 165, 130), (247, 247, 247), (146, 197, 222), (5, 113, 176)], + 'rdbu6': [(178, 24, 43), (239, 138, 98), (253, 219, 199), (209, 229, 240), (103, 169, 207), (33, 102, 172)], + 'rdbu7': [(178, 24, 43), (239, 138, 98), (253, 219, 199), (247, 247, 247), (209, 229, 240), (103, 169, 207), (33, 102, 172)], + 'rdbu8': [(178, 24, 43), (214, 96, 77), (244, 165, 130), (253, 219, 199), (209, 229, 240), (146, 197, 222), (67, 147, 195), (33, 102, 172)], + 'rdbu9': [(178, 24, 43), (214, 96, 77), (244, 165, 130), (253, 219, 199), (247, 247, 247), (209, 229, 240), (146, 197, 222), (67, 147, 195), (33, 102, 172)], + 'rdgy10': [(103, 0, 31), (26, 26, 26), (178, 24, 43), (214, 96, 77), (244, 165, 130), (253, 219, 199), (224, 224, 224), (186, 186, 186), (135, 135, 135), (77, 77, 77)], + 'rdgy11': [(103, 0, 31), (77, 77, 77), (26, 26, 26), (178, 24, 43), (214, 96, 77), (244, 165, 130), (253, 219, 199), (255, 255, 255), (224, 224, 224), (186, 186, 186), (135, 135, 135)], + 'rdgy3': [(239, 138, 98), (255, 255, 255), (153, 153, 153)], + 'rdgy4': [(202, 0, 32), (244, 165, 130), (186, 186, 186), (64, 64, 64)], + 'rdgy5': [(202, 0, 32), (244, 165, 130), (255, 255, 255), (186, 186, 186), (64, 64, 64)], + 'rdgy6': [(178, 24, 43), (239, 138, 98), (253, 219, 199), (224, 224, 224), (153, 153, 153), (77, 77, 77)], + 'rdgy7': [(178, 24, 43), (239, 138, 98), (253, 219, 199), (255, 255, 255), (224, 224, 224), (153, 153, 153), (77, 77, 77)], + 'rdgy8': [(178, 24, 43), (214, 96, 77), (244, 165, 130), (253, 219, 199), (224, 224, 224), (186, 186, 186), (135, 135, 135), (77, 77, 77)], + 'rdgy9': [(178, 24, 43), (214, 96, 77), (244, 165, 130), (253, 219, 199), (255, 255, 255), (224, 224, 224), (186, 186, 186), (135, 135, 135), (77, 77, 77)], + 'rdpu3': [(253, 224, 221), (250, 159, 181), (197, 27, 138)], + 'rdpu4': [(254, 235, 226), (251, 180, 185), (247, 104, 161), (174, 1, 126)], + 'rdpu5': [(254, 235, 226), (251, 180, 185), (247, 104, 161), (197, 27, 138), (122, 1, 119)], + 'rdpu6': [(254, 235, 226), (252, 197, 192), (250, 159, 181), (247, 104, 161), (197, 27, 138), (122, 1, 119)], + 'rdpu7': [(254, 235, 226), (252, 197, 192), (250, 159, 181), (247, 104, 161), (221, 52, 151), (174, 1, 126), (122, 1, 119)], + 'rdpu8': [(255, 247, 243), (253, 224, 221), (252, 197, 192), (250, 159, 181), (247, 104, 161), (221, 52, 151), (174, 1, 126), (122, 1, 119)], + 'rdpu9': [(255, 247, 243), (253, 224, 221), (252, 197, 192), (250, 159, 181), (247, 104, 161), (221, 52, 151), (174, 1, 126), (122, 1, 119), (73, 0, 106)], + 'rdylbu10': [(165, 0, 38), (49, 54, 149), (215, 48, 39), (244, 109, 67), (253, 174, 97), (254, 224, 144), (224, 243, 248), (171, 217, 233), (116, 173, 209), (69, 117, 180)], + 'rdylbu11': [(165, 0, 38), (69, 117, 180), (49, 54, 149), (215, 48, 39), (244, 109, 67), (253, 174, 97), (254, 224, 144), (255, 255, 191), (224, 243, 248), (171, 217, 233), (116, 173, 209)], + 'rdylbu3': [(252, 141, 89), (255, 255, 191), (145, 191, 219)], + 'rdylbu4': [(215, 25, 28), (253, 174, 97), (171, 217, 233), (44, 123, 182)], + 'rdylbu5': [(215, 25, 28), (253, 174, 97), (255, 255, 191), (171, 217, 233), (44, 123, 182)], + 'rdylbu6': [(215, 48, 39), (252, 141, 89), (254, 224, 144), (224, 243, 248), (145, 191, 219), (69, 117, 180)], + 'rdylbu7': [(215, 48, 39), (252, 141, 89), (254, 224, 144), (255, 255, 191), (224, 243, 248), (145, 191, 219), (69, 117, 180)], + 'rdylbu8': [(215, 48, 39), (244, 109, 67), (253, 174, 97), (254, 224, 144), (224, 243, 248), (171, 217, 233), (116, 173, 209), (69, 117, 180)], + 'rdylbu9': [(215, 48, 39), (244, 109, 67), (253, 174, 97), (254, 224, 144), (255, 255, 191), (224, 243, 248), (171, 217, 233), (116, 173, 209), (69, 117, 180)], + 'rdylgn10': [(165, 0, 38), (0, 104, 55), (215, 48, 39), (244, 109, 67), (253, 174, 97), (254, 224, 139), (217, 239, 139), (166, 217, 106), (102, 189, 99), (26, 152, 80)], + 'rdylgn11': [(165, 0, 38), (26, 152, 80), (0, 104, 55), (215, 48, 39), (244, 109, 67), (253, 174, 97), (254, 224, 139), (255, 255, 191), (217, 239, 139), (166, 217, 106), (102, 189, 99)], + 'rdylgn3': [(252, 141, 89), (255, 255, 191), (145, 207, 96)], + 'rdylgn4': [(215, 25, 28), (253, 174, 97), (166, 217, 106), (26, 150, 65)], + 'rdylgn5': [(215, 25, 28), (253, 174, 97), (255, 255, 191), (166, 217, 106), (26, 150, 65)], + 'rdylgn6': [(215, 48, 39), (252, 141, 89), (254, 224, 139), (217, 239, 139), (145, 207, 96), (26, 152, 80)], + 'rdylgn7': [(215, 48, 39), (252, 141, 89), (254, 224, 139), (255, 255, 191), (217, 239, 139), (145, 207, 96), (26, 152, 80)], + 'rdylgn8': [(215, 48, 39), (244, 109, 67), (253, 174, 97), (254, 224, 139), (217, 239, 139), (166, 217, 106), (102, 189, 99), (26, 152, 80)], + 'rdylgn9': [(215, 48, 39), (244, 109, 67), (253, 174, 97), (254, 224, 139), (255, 255, 191), (217, 239, 139), (166, 217, 106), (102, 189, 99), (26, 152, 80)], + 'reds3': [(254, 224, 210), (252, 146, 114), (222, 45, 38)], + 'reds4': [(254, 229, 217), (252, 174, 145), (251, 106, 74), (203, 24, 29)], + 'reds5': [(254, 229, 217), (252, 174, 145), (251, 106, 74), (222, 45, 38), (165, 15, 21)], + 'reds6': [(254, 229, 217), (252, 187, 161), (252, 146, 114), (251, 106, 74), (222, 45, 38), (165, 15, 21)], + 'reds7': [(254, 229, 217), (252, 187, 161), (252, 146, 114), (251, 106, 74), (239, 59, 44), (203, 24, 29), (153, 0, 13)], + 'reds8': [(255, 245, 240), (254, 224, 210), (252, 187, 161), (252, 146, 114), (251, 106, 74), (239, 59, 44), (203, 24, 29), (153, 0, 13)], + 'reds9': [(255, 245, 240), (254, 224, 210), (252, 187, 161), (252, 146, 114), (251, 106, 74), (239, 59, 44), (203, 24, 29), (165, 15, 21), (103, 0, 13)], + 'set13': [(228, 26, 28), (55, 126, 184), (77, 175, 74)], + 'set14': [(228, 26, 28), (55, 126, 184), (77, 175, 74), (152, 78, 163)], + 'set15': [(228, 26, 28), (55, 126, 184), (77, 175, 74), (152, 78, 163), (255, 127, 0)], + 'set16': [(228, 26, 28), (55, 126, 184), (77, 175, 74), (152, 78, 163), (255, 127, 0), (255, 255, 51)], + 'set17': [(228, 26, 28), (55, 126, 184), (77, 175, 74), (152, 78, 163), (255, 127, 0), (255, 255, 51), (166, 86, 40)], + 'set18': [(228, 26, 28), (55, 126, 184), (77, 175, 74), (152, 78, 163), (255, 127, 0), (255, 255, 51), (166, 86, 40), (247, 129, 191)], + 'set19': [(228, 26, 28), (55, 126, 184), (77, 175, 74), (152, 78, 163), (255, 127, 0), (255, 255, 51), (166, 86, 40), (247, 129, 191), (153, 153, 153)], + 'set23': [(102, 194, 165), (252, 141, 98), (141, 160, 203)], + 'set24': [(102, 194, 165), (252, 141, 98), (141, 160, 203), (231, 138, 195)], + 'set25': [(102, 194, 165), (252, 141, 98), (141, 160, 203), (231, 138, 195), (166, 216, 84)], + 'set26': [(102, 194, 165), (252, 141, 98), (141, 160, 203), (231, 138, 195), (166, 216, 84), (255, 217, 47)], + 'set27': [(102, 194, 165), (252, 141, 98), (141, 160, 203), (231, 138, 195), (166, 216, 84), (255, 217, 47), (229, 196, 148)], + 'set28': [(102, 194, 165), (252, 141, 98), (141, 160, 203), (231, 138, 195), (166, 216, 84), (255, 217, 47), (229, 196, 148), (179, 179, 179)], + 'set310': [(141, 211, 199), (188, 128, 189), (255, 255, 179), (190, 186, 218), (251, 128, 114), (128, 177, 211), (253, 180, 98), (179, 222, 105), (252, 205, 229), (217, 217, 217)], + 'set311': [(141, 211, 199), (188, 128, 189), (204, 235, 197), (255, 255, 179), (190, 186, 218), (251, 128, 114), (128, 177, 211), (253, 180, 98), (179, 222, 105), (252, 205, 229), (217, 217, 217)], + 'set312': [(141, 211, 199), (188, 128, 189), (204, 235, 197), (255, 237, 111), (255, 255, 179), (190, 186, 218), (251, 128, 114), (128, 177, 211), (253, 180, 98), (179, 222, 105), (252, 205, 229), (217, 217, 217)], + 'set33': [(141, 211, 199), (255, 255, 179), (190, 186, 218)], + 'set34': [(141, 211, 199), (255, 255, 179), (190, 186, 218), (251, 128, 114)], + 'set35': [(141, 211, 199), (255, 255, 179), (190, 186, 218), (251, 128, 114), (128, 177, 211)], + 'set36': [(141, 211, 199), (255, 255, 179), (190, 186, 218), (251, 128, 114), (128, 177, 211), (253, 180, 98)], + 'set37': [(141, 211, 199), (255, 255, 179), (190, 186, 218), (251, 128, 114), (128, 177, 211), (253, 180, 98), (179, 222, 105)], + 'set38': [(141, 211, 199), (255, 255, 179), (190, 186, 218), (251, 128, 114), (128, 177, 211), (253, 180, 98), (179, 222, 105), (252, 205, 229)], + 'set39': [(141, 211, 199), (255, 255, 179), (190, 186, 218), (251, 128, 114), (128, 177, 211), (253, 180, 98), (179, 222, 105), (252, 205, 229), (217, 217, 217)], + 'spectral10': [(158, 1, 66), (94, 79, 162), (213, 62, 79), (244, 109, 67), (253, 174, 97), (254, 224, 139), (230, 245, 152), (171, 221, 164), (102, 194, 165), (50, 136, 189)], + 'spectral11': [(158, 1, 66), (50, 136, 189), (94, 79, 162), (213, 62, 79), (244, 109, 67), (253, 174, 97), (254, 224, 139), (255, 255, 191), (230, 245, 152), (171, 221, 164), (102, 194, 165)], + 'spectral3': [(252, 141, 89), (255, 255, 191), (153, 213, 148)], + 'spectral4': [(215, 25, 28), (253, 174, 97), (171, 221, 164), (43, 131, 186)], + 'spectral5': [(215, 25, 28), (253, 174, 97), (255, 255, 191), (171, 221, 164), (43, 131, 186)], + 'spectral6': [(213, 62, 79), (252, 141, 89), (254, 224, 139), (230, 245, 152), (153, 213, 148), (50, 136, 189)], + 'spectral7': [(213, 62, 79), (252, 141, 89), (254, 224, 139), (255, 255, 191), (230, 245, 152), (153, 213, 148), (50, 136, 189)], + 'spectral8': [(213, 62, 79), (244, 109, 67), (253, 174, 97), (254, 224, 139), (230, 245, 152), (171, 221, 164), (102, 194, 165), (50, 136, 189)], + 'spectral9': [(213, 62, 79), (244, 109, 67), (253, 174, 97), (254, 224, 139), (255, 255, 191), (230, 245, 152), (171, 221, 164), (102, 194, 165), (50, 136, 189)], + 'ylgn3': [(247, 252, 185), (173, 221, 142), (49, 163, 84)], + 'ylgn4': [(255, 255, 204), (194, 230, 153), (120, 198, 121), (35, 132, 67)], + 'ylgn5': [(255, 255, 204), (194, 230, 153), (120, 198, 121), (49, 163, 84), (0, 104, 55)], + 'ylgn6': [(255, 255, 204), (217, 240, 163), (173, 221, 142), (120, 198, 121), (49, 163, 84), (0, 104, 55)], + 'ylgn7': [(255, 255, 204), (217, 240, 163), (173, 221, 142), (120, 198, 121), (65, 171, 93), (35, 132, 67), (0, 90, 50)], + 'ylgn8': [(255, 255, 229), (247, 252, 185), (217, 240, 163), (173, 221, 142), (120, 198, 121), (65, 171, 93), (35, 132, 67), (0, 90, 50)], + 'ylgn9': [(255, 255, 229), (247, 252, 185), (217, 240, 163), (173, 221, 142), (120, 198, 121), (65, 171, 93), (35, 132, 67), (0, 104, 55), (0, 69, 41)], + 'ylgnbu3': [(237, 248, 177), (127, 205, 187), (44, 127, 184)], + 'ylgnbu4': [(255, 255, 204), (161, 218, 180), (65, 182, 196), (34, 94, 168)], + 'ylgnbu5': [(255, 255, 204), (161, 218, 180), (65, 182, 196), (44, 127, 184), (37, 52, 148)], + 'ylgnbu6': [(255, 255, 204), (199, 233, 180), (127, 205, 187), (65, 182, 196), (44, 127, 184), (37, 52, 148)], + 'ylgnbu7': [(255, 255, 204), (199, 233, 180), (127, 205, 187), (65, 182, 196), (29, 145, 192), (34, 94, 168), (12, 44, 132)], + 'ylgnbu8': [(255, 255, 217), (237, 248, 177), (199, 233, 180), (127, 205, 187), (65, 182, 196), (29, 145, 192), (34, 94, 168), (12, 44, 132)], + 'ylgnbu9': [(255, 255, 217), (237, 248, 177), (199, 233, 180), (127, 205, 187), (65, 182, 196), (29, 145, 192), (34, 94, 168), (37, 52, 148), (8, 29, 88)], + 'ylorbr3': [(255, 247, 188), (254, 196, 79), (217, 95, 14)], + 'ylorbr4': [(255, 255, 212), (254, 217, 142), (254, 153, 41), (204, 76, 2)], + 'ylorbr5': [(255, 255, 212), (254, 217, 142), (254, 153, 41), (217, 95, 14), (153, 52, 4)], + 'ylorbr6': [(255, 255, 212), (254, 227, 145), (254, 196, 79), (254, 153, 41), (217, 95, 14), (153, 52, 4)], + 'ylorbr7': [(255, 255, 212), (254, 227, 145), (254, 196, 79), (254, 153, 41), (236, 112, 20), (204, 76, 2), (140, 45, 4)], + 'ylorbr8': [(255, 255, 229), (255, 247, 188), (254, 227, 145), (254, 196, 79), (254, 153, 41), (236, 112, 20), (204, 76, 2), (140, 45, 4)], + 'ylorbr9': [(255, 255, 229), (255, 247, 188), (254, 227, 145), (254, 196, 79), (254, 153, 41), (236, 112, 20), (204, 76, 2), (153, 52, 4), (102, 37, 6)], + 'ylorrd3': [(255, 237, 160), (254, 178, 76), (240, 59, 32)], + 'ylorrd4': [(255, 255, 178), (254, 204, 92), (253, 141, 60), (227, 26, 28)], + 'ylorrd5': [(255, 255, 178), (254, 204, 92), (253, 141, 60), (240, 59, 32), (189, 0, 38)], + 'ylorrd6': [(255, 255, 178), (254, 217, 118), (254, 178, 76), (253, 141, 60), (240, 59, 32), (189, 0, 38)], + 'ylorrd7': [(255, 255, 178), (254, 217, 118), (254, 178, 76), (253, 141, 60), (252, 78, 42), (227, 26, 28), (177, 0, 38)], + 'ylorrd8': [(255, 255, 204), (255, 237, 160), (254, 217, 118), (254, 178, 76), (253, 141, 60), (252, 78, 42), (227, 26, 28), (177, 0, 38)], +} + + +if __name__ == '__main__': + main() diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/xdot/xdot_qt.py b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/xdot/xdot_qt.py new file mode 100644 index 00000000..3e96af69 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree/src/rqt_behavior_tree/xdot/xdot_qt.py @@ -0,0 +1,2236 @@ +#!/usr/bin/env python +# +# Copyright 2008 Jose Fonseca +# +# This program is free software: you can redistribute it and/or modify it +# under the terms of the GNU Lesser General Public License as published +# by the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU Lesser General Public License for more details. +# +# You should have received a copy of the GNU Lesser General Public License +# along with this program. If not, see . +# +# QT Version adapted by Alex Bravo + +'''Visualize dot graphs via the xdot format.''' + +__author__ = "Jose Fonseca" + +__version__ = "0.4" + +import os +import sys +import subprocess +import math +import colorsys +import time +import re +import traceback + +#from PySide.QtCore import * +#from PySide.QtGui import * +#from PyQt4 import * +#from PyQt4.QtCore import * +#from PyQt4.QtGui import * + +from python_qt_binding import * +from python_qt_binding.QtCore import * +from python_qt_binding.QtGui import * +from python_qt_binding.QtWidgets import * + +#-------------------------------------------------------------------------------------------------------------------------------------------------------------------- +#-- Drawing Classes --# + +class Pen: + """Store pen attributes.""" + + def __init__(self): + # set default attributes + self.color = (0.0, 0.0, 0.0, 1.0) + self.fillcolor = (0.0, 0.0, 0.0, 1.0) + self.linewidth = 1.0 + self.fontsize = 14.0 + self.fontname = "Times-Roman" + self.dash = Qt.SolidLine + + def copy(self): + """Create a copy of this pen.""" + pen = Pen() + pen.__dict__ = self.__dict__.copy() + return pen + + def highlighted(self): + pen = self.copy() + pen.color = (1, 0, 0, 1) + pen.fillcolor = (1, .8, .8, 1) + return pen + +class Shape: + """Abstract base class for all the drawing shapes.""" + + def __init__(self): + pass + + def draw(self, cr, highlight=False): + """Draw this shape with the given cairo context""" + raise NotImplementedError + + def select_pen(self, highlight): + if highlight: + if not hasattr(self, 'highlight_pen'): + self.highlight_pen = self.pen.highlighted() + return self.highlight_pen + else: + return self.pen + +class TextShape(Shape): + """Used to draw a text shape with a QPainter""" + + LEFT, CENTER, RIGHT = -1, 0, 1 + + def __init__(self, pen, x, y, j, w, t): + Shape.__init__(self) + self.pen = pen.copy() + self.x = x + self.y = y + self.j = j #AB allignment? JRB: height. + self.w = w + self.t = t + + def draw(self, painter, highlight=False): + pen = self.select_pen(highlight) + painter.setPen(QColor.fromRgbF(*pen.color)) + font = QFont(self.pen.fontname) + + fontMetrics = QFontMetrics(QFont(self.pen.fontname)) + scale = float(fontMetrics.width(self.t)) / float(self.w) + + if scale < 1.0 or scale > 1.0: + font.setPointSizeF(font.pointSizeF()/scale); + + painter.setFont(font)#, self.pen.fontsize)) #AB simply setting font size doesn't work + painter.drawText( + int(self.x - self.w / 2.0), + int(self.y), + self.t) + pass + +class EllipseShape(Shape): + """Used to draw an ellipse shape with a QPainter""" + def __init__(self, pen, x0, y0, w, h, filled=False): + Shape.__init__(self) + self.pen = pen.copy() + self.x0 = x0 + self.y0 = y0 + self.w = w + self.h = h + self.filled = filled + + def draw(self, painter, highlight=False): + painter.save() + pen = self.select_pen(highlight) + if self.filled: + painter.setPen(QColor.fromRgbF(*pen.fillcolor)) + painter.setBrush(QColor.fromRgbF(*pen.fillcolor)) + else: + painter.setPen(QPen(QBrush(QColor.fromRgbF(*pen.color)), pen.linewidth, pen.dash)) + painter.setBrush(Qt.NoBrush) + painter.drawEllipse(int(self.x0 - self.w), int(self.y0 - self.h), int(self.w * 2), int(self.h * 2)) + painter.restore() + +class PolygonShape(Shape): + """Used to draw a polygon with QPainter.""" + + def __init__(self, pen, points, filled=False): + Shape.__init__(self) + self.pen = pen.copy() + self.points = points + self.filled = filled + + def draw(self, painter, highlight=False): + + polygon_points = QPolygonF() + for x, y in self.points: + polygon_points.append (QPointF(x, y)) + + pen = self.select_pen(highlight) + painter.save() + if self.filled: + painter.setPen(QColor.fromRgbF(*pen.fillcolor)) + painter.setBrush(QColor.fromRgbF(*pen.fillcolor)) + else: + painter.setPen(QPen(QBrush(QColor.fromRgbF(*pen.color)), pen.linewidth, + pen.dash, Qt.SquareCap, Qt.MiterJoin)) + painter.setBrush(Qt.NoBrush) + + painter.drawPolygon(polygon_points) + painter.restore() + +class LineShape(Shape): + """Used to draw a line with QPainter.""" + + def __init__(self, pen, points): + Shape.__init__(self) + self.pen = pen.copy() + self.points = points + + def draw(self, painter, highlight=False): + pen = self.select_pen(highlight) + painter.setPen(QPen(QBrush(QColor.fromRgbF(*pen.color)), pen.linewidth, + pen.dash, Qt.SquareCap, Qt.MiterJoin)) + + x0, y0 = self.points[0] + for x1, y1 in self.points[1:]: + painter.drawLine(QPointF(x0, y0), QPointF(x1, y1)) + x0 = x1 + y0 = y1 + +class BezierShape(Shape): + """Used to draw a bezier curve with QPainter.""" + + def __init__(self, pen, points, filled=False): + Shape.__init__(self) + self.pen = pen.copy() + self.points = points + self.filled = filled + + def draw(self, painter, highlight=False): + painter_path = QPainterPath() + painter_path.moveTo(QPointF(*self.points[0])) + for i in range(1, len(self.points), 3): + painter_path.cubicTo( + QPointF(*self.points[i]), + QPointF(*self.points[i + 1]), + QPointF(*self.points[i + 2])) + pen = self.select_pen(highlight) + qpen = QPen() + if self.filled: + brush = QBrush() + brush.setColor(QColor.fromRgbF(*pen.fillcolor)) + brush.setStyle(Qt.SolidPattern) + painter.setBrush(brush) + #qpen.setCapStyle(Qt.RoundCap) + #qpen.setJoinStyle(Qt.RoundJoin) + #painter_path.setFillRule(Qt.OddEvenFill) + else: + painter.setBrush(Qt.NoBrush) + + qpen.setStyle(pen.dash) + qpen.setWidth(int(pen.linewidth)) + qpen.setColor(QColor.fromRgbF(*pen.color)) + painter.setPen(qpen) + painter.drawPath(painter_path) + +class CompoundShape(Shape): + """Used to draw a set of shapes with QPainter.""" + + def __init__(self, shapes): + Shape.__init__(self) + self.shapes = shapes + + def draw(self, cr, highlight=False): + for shape in self.shapes: + shape.draw(cr, highlight=highlight) + +#-------------------------------------------------------------------------------------------------------------------------------------------------------------------- +#-- Metadata Classes --# + +class Url(object): + """Represents a graphviz URL.""" + + def __init__(self, item, url, highlight=None): + self.item = item + self.url = url + if highlight is None: + highlight = set([item]) + self.highlight = highlight + +class Jump(object): + """Represents a jump to another node's position on the canvas.""" + + def __init__(self, item, x, y, highlight=None, url=None): + self.item = item + self.x = x + self.y = y + if highlight is None: + highlight = set([item]) + self.highlight = highlight + self.url = url + +#-------------------------------------------------------------------------------------------------------------------------------------------------------------------- +#-- Graph Representation Classes --# + +class Element(CompoundShape): + """Base class for graph nodes and edges.""" + + def __init__(self, shapes): + CompoundShape.__init__(self, shapes) + + def get_url(self, x, y): + return None + + def get_jump(self, x, y): + return None + + +class Node(Element): + """An abstract node in the graph, it's spatial location, and it's visual representation.""" + + def __init__(self, x, y, w, h, shapes, url): + Element.__init__(self, shapes) + + self.x = x + self.y = y + + self.x1 = x - 0.5*w + self.y1 = y - 0.5*h + self.x2 = x + 0.5*w + self.y2 = y + 0.5*h + + self.url = url + + def is_inside(self, x, y): + """Used to check for 2D-picking via the mouse. + param x: The x position on the canvas + param y: The y position on the canvas + """ + return self.x1 <= x and x <= self.x2 and self.y1 <= y and y <= self.y2 + + def get_url(self, x, y): + """Get the elemnt's metadata.""" + if self.url is None: + return None + if self.is_inside(x, y): + return Url(self, self.url) + return None + + def get_jump(self, x, y): + if self.is_inside(x, y): + return Jump(self, self.x, self.y) + return None + + +def square_distance(x1, y1, x2, y2): + deltax = x2 - x1 + deltay = y2 - y1 + return deltax*deltax + deltay*deltay + + +class Edge(Element): + + def __init__(self, src, dst, points, shapes, url): + Element.__init__(self, shapes) + self.src = src + self.dst = dst + self.points = points + self.url = url + + RADIUS = 10 + + def get_jump(self, x, y): + if square_distance(x, y, *self.points[0]) <= self.RADIUS*self.RADIUS: + return Jump(self, self.dst.x, self.dst.y, highlight=set([self, self.dst]),url=self.url) + if square_distance(x, y, *self.points[-1]) <= self.RADIUS*self.RADIUS: + return Jump(self, self.src.x, self.src.y, highlight=set([self, self.src]),url=self.url) + return None + + +#-------------------------------------------------------------------------------------------------------------------------------------------------------------------- +class Graph(Shape): + + def __init__(self, width=1, height=1, shapes=(), nodes=(), edges=(), subgraph_shapes={}): + Shape.__init__(self) + + self.width = width + self.height = height + self.shapes = shapes + self.nodes = nodes + self.edges = edges + self.subgraph_shapes = subgraph_shapes + + def get_size(self): + return self.width, self.height + + def draw(self, cr, highlight_items=None): + if highlight_items is None: + highlight_items = () + for shape in self.shapes: + shape.draw(cr) + for edge in self.edges: + edge.draw(cr, highlight=(edge in highlight_items)) + for node in self.nodes: + node.draw(cr, highlight=(node in highlight_items)) + + def get_url(self, x, y): + for node in self.nodes: + url = node.get_url(x, y) + if url is not None: + return url + return None + + def get_jump(self, x, y): + for edge in self.edges: + jump = edge.get_jump(x, y) + if jump is not None: + return jump + for node in self.nodes: + jump = node.get_jump(x, y) + if jump is not None: + return jump + return None + + +#-------------------------------------------------------------------------------------------------------------------------------------------------------------------- +class XDotAttrParser: + """Parser for xdot drawing attributes. + See also: + - http://www.graphviz.org/doc/info/output.html#d:xdot + """ + + def __init__(self, parser, buf): + self.parser = parser + self.buf = self.unescape(buf) + self.pos = 0 + + self.pen = Pen() + self.shapes = [] + + def __nonzero__(self): + return self.pos < len(self.buf) + + def unescape(self, buf): + buf = buf.replace('\\"', '"') + buf = buf.replace('\\n', '\n') + return buf + + def read_code(self): + pos = self.buf.find(" ", self.pos) + res = self.buf[self.pos:pos] + self.pos = pos + 1 + while self.pos < len(self.buf) and self.buf[self.pos].isspace(): + self.pos += 1 + return res + + def read_number(self): + return int(float(self.read_code())) + + def read_float(self): + return float(self.read_code()) + + def read_point(self): + x = self.read_number() + y = self.read_number() + return self.transform(x, y) + + def read_text(self): + num = self.read_number() + pos = self.buf.find("-", self.pos) + 1 + self.pos = pos + num + res = self.buf[pos:self.pos] + while self.pos < len(self.buf) and self.buf[self.pos].isspace(): + self.pos += 1 + return res + + def read_polygon(self): + n = self.read_number() + p = [] +# p = QPointF[] + for i in range(n): + x, y = self.read_point() + p.append((x, y)) +# p.append (QPointF(x, y)) + return p + + def read_color(self): + # See http://www.graphviz.org/doc/info/attrs.html#k:color + c = self.read_text() + c1 = c[:1] + if c1 == '#': + hex2float = lambda h: float(int(h, 16)/255.0) + r = hex2float(c[1:3]) + g = hex2float(c[3:5]) + b = hex2float(c[5:7]) + try: + a = hex2float(c[7:9]) + except (IndexError, ValueError): + a = 1.0 + return r, g, b, a + elif c1.isdigit() or c1 == ".": + # "H,S,V" or "H S V" or "H, S, V" or any other variation + h, s, v = map(float, c.replace(",", " ").split()) + r, g, b = colorsys.hsv_to_rgb(h, s, v) + a = 1.0 + return r, g, b, a + else: + return self.lookup_color(c) + + def lookup_color(self, c): + try: + color = gtk.gdk.color_parse(c) + except ValueError: + pass + else: + s = 1.0/65535.0 + r = color.red*s + g = color.green*s + b = color.blue*s + a = 1.0 + return r, g, b, a + + try: + dummy, scheme, index = c.split('/') + r, g, b = brewer_colors[scheme][int(index)] + except (ValueError, KeyError): + pass + else: + s = 1.0/255.0 + r = r*s + g = g*s + b = b*s + a = 1.0 + return r, g, b, a + + sys.stderr.write("unknown color '%s'\n" % c) + return None + + def parse(self): + s = self + + while s: + op = s.read_code() + if op == "c": + color = s.read_color() + if color is not None: + self.handle_color(color, filled=False) + elif op == "C": + color = s.read_color() + if color is not None: + self.handle_color(color, filled=True) + elif op == "S": + # http://www.graphviz.org/doc/info/attrs.html#k:style + style = s.read_text() + if style.startswith("setlinewidth("): + lw = style.split("(")[1].split(")")[0] + lw = float(lw) + self.handle_linewidth(lw) + elif style in ("solid", "dashed"): + self.handle_linestyle(style) + elif op == "F": + size = s.read_float() + name = s.read_text() + self.handle_font(size, name) + elif op == "T": + x, y = s.read_point() + j = s.read_number() + w = s.read_number() + t = s.read_text() + self.handle_text(x, y, j, w, t) + elif op == "E": + x0, y0 = s.read_point() + w = s.read_number() + h = s.read_number() + self.handle_ellipse(x0, y0, w, h, filled=True) + elif op == "e": + x0, y0 = s.read_point() + w = s.read_number() + h = s.read_number() + self.handle_ellipse(x0, y0, w, h, filled=False) + elif op == "L": + points = self.read_polygon() + self.handle_line(points) + elif op == "B": + points = self.read_polygon() + self.handle_bezier(points, filled=False) + elif op == "b": + points = self.read_polygon() + self.handle_bezier(points, filled=True) + elif op == "P": + points = self.read_polygon() + self.handle_polygon(points, filled=True) + elif op == "p": + points = self.read_polygon() + self.handle_polygon(points, filled=False) + else: + #sys.stderr.write("unknown xdot opcode '%s'\n" % op) + break + + return self.shapes + + def transform(self, x, y): + return self.parser.transform(x, y) + + def handle_color(self, color, filled=False): + if filled: + self.pen.fillcolor = color + else: + self.pen.color = color + + def handle_linewidth(self, linewidth): + self.pen.linewidth = linewidth + + def handle_linestyle(self, style): + if style == "solid": +# self.pen.dash = () + self.pen.dash = Qt.SolidLine + elif style == "dashed": +# self.pen.dash = (6, ) # 6pt on, 6pt off + self.pen.dash = Qt.DashLine + + def handle_font(self, size, name): + self.pen.fontsize = size + self.pen.fontname = name + + def handle_text(self, x, y, j, w, t): + self.shapes.append(TextShape(self.pen, x, y, j, w, t)) + + def handle_ellipse(self, x0, y0, w, h, filled=False): + if filled: + # xdot uses this to mean "draw a filled shape with an outline" + self.shapes.append(EllipseShape(self.pen, x0, y0, w, h, filled=True)) + self.shapes.append(EllipseShape(self.pen, x0, y0, w, h)) + + def handle_line(self, points): + self.shapes.append(LineShape(self.pen, points)) + + def handle_bezier(self, points, filled=False): + if filled: + # xdot uses this to mean "draw a filled shape with an outline" + self.shapes.append(BezierShape(self.pen, points, filled=True)) + self.shapes.append(BezierShape(self.pen, points)) + + def handle_polygon(self, points, filled=False): + #AB Should filled be handled inside of PolygonShape? + if filled: + # xdot uses this to mean "draw a filled shape with an outline" + self.shapes.append(PolygonShape(self.pen, points, filled=True)) + self.shapes.append(PolygonShape(self.pen, points)) + + +EOF = -1 +SKIP = -2 + + +class ParseError(Exception): + + def __init__(self, msg=None, filename=None, line=None, col=None): + self.msg = msg + self.filename = filename + self.line = line + self.col = col + + def __str__(self): + return ':'.join([str(part) for part in (self.filename, self.line, self.col, self.msg) if part != None]) + + +#-------------------------------------------------------------------------------------------------------------------------------------------------------------------- +class Scanner: + """Stateless scanner.""" + + # should be overriden by derived classes + tokens = [] + symbols = {} + literals = {} + ignorecase = False + + def __init__(self): + flags = re.DOTALL + if self.ignorecase: + flags |= re.IGNORECASE + self.tokens_re = re.compile( + '|'.join(['(' + regexp + ')' for type, regexp, test_lit in self.tokens]), + flags + ) + + def next(self, buf, pos): + if pos >= len(buf): + return EOF, '', pos + mo = self.tokens_re.match(buf, pos) + if mo: + text = mo.group() + type, regexp, test_lit = self.tokens[mo.lastindex - 1] + pos = mo.end() + if test_lit: + type = self.literals.get(text, type) + return type, text, pos + else: + c = buf[pos] + return self.symbols.get(c, None), c, pos + 1 + + +class Token: + + def __init__(self, type, text, line, col): + self.type = type + self.text = text + self.line = line + self.col = col + + +class Lexer: + + # should be overriden by derived classes + scanner = None + tabsize = 8 + + newline_re = re.compile(r'\r\n?|\n') + + def __init__(self, buf = None, pos = 0, filename = None, fp = None): + if fp is not None: + try: + fileno = fp.fileno() + length = os.path.getsize(fp.name) + import mmap + except: + # read whole file into memory + buf = fp.read() + pos = 0 + else: + # map the whole file into memory + if length: + # length must not be zero + buf = mmap.mmap(fileno, length, access = mmap.ACCESS_READ) + pos = os.lseek(fileno, 0, 1) + else: + buf = '' + pos = 0 + + if filename is None: + try: + filename = fp.name + except AttributeError: + filename = None + + self.buf = buf + self.pos = pos + self.line = 1 + self.col = 1 + self.filename = filename + + def next(self): + while True: + # save state + pos = self.pos + line = self.line + col = self.col + + type, text, endpos = self.scanner.next(self.buf, pos) + assert pos + len(text) == endpos + self.consume(text) + type, text = self.filter(type, text) + self.pos = endpos + + if type == SKIP: + continue + elif type is None: + msg = 'unexpected char ' + if text >= ' ' and text <= '~': + msg += "'%s'" % text + else: + msg += "0x%X" % ord(text) + raise ParseError(msg, self.filename, line, col) + else: + break + return Token(type = type, text = text, line = line, col = col) + + def consume(self, text): + # update line number + pos = 0 + for mo in self.newline_re.finditer(text, pos): + self.line += 1 + self.col = 1 + pos = mo.end() + + # update column number + while True: + tabpos = text.find('\t', pos) + if tabpos == -1: + break + self.col += tabpos - pos + self.col = ((self.col - 1)//self.tabsize + 1)*self.tabsize + 1 + pos = tabpos + 1 + self.col += len(text) - pos + + +class Parser: + + def __init__(self, lexer): + self.lexer = lexer + self.lookahead = self.lexer.next() + + def match(self, type): + if self.lookahead.type != type: + raise ParseError( + msg = 'unexpected token %r' % self.lookahead.text, + filename = self.lexer.filename, + line = self.lookahead.line, + col = self.lookahead.col) + + def skip(self, type): + while self.lookahead.type != type: + self.consume() + + def consume(self): + token = self.lookahead + self.lookahead = self.lexer.next() + return token + + +ID = 0 +STR_ID = 1 +HTML_ID = 2 +EDGE_OP = 3 + +LSQUARE = 4 +RSQUARE = 5 +LCURLY = 6 +RCURLY = 7 +COMMA = 8 +COLON = 9 +SEMI = 10 +EQUAL = 11 +PLUS = 12 + +STRICT = 13 +GRAPH = 14 +DIGRAPH = 15 +NODE = 16 +EDGE = 17 +SUBGRAPH = 18 + + +class DotScanner(Scanner): + + # token regular expression table + tokens = [ + # whitespace and comments + (SKIP, + r'[ \t\f\r\n\v]+|' + r'//[^\r\n]*|' + r'/\*.*?\*/|' + r'#[^\r\n]*', + False), + + # Alphanumeric IDs + (ID, r'[a-zA-Z_\x80-\xff][a-zA-Z0-9_\x80-\xff]*', True), + + # Numeric IDs + (ID, r'-?(?:\.[0-9]+|[0-9]+(?:\.[0-9]*)?)', False), + + # String IDs + (STR_ID, r'"[^"\\]*(?:\\.[^"\\]*)*"', False), + + # HTML IDs + (HTML_ID, r'<[^<>]*(?:<[^<>]*>[^<>]*)*>', False), + + # Edge operators + (EDGE_OP, r'-[>-]', False), + ] + + # symbol table + symbols = { + '[': LSQUARE, + ']': RSQUARE, + '{': LCURLY, + '}': RCURLY, + ',': COMMA, + ':': COLON, + ';': SEMI, + '=': EQUAL, + '+': PLUS, + } + + # literal table + literals = { + 'strict': STRICT, + 'graph': GRAPH, + 'digraph': DIGRAPH, + 'node': NODE, + 'edge': EDGE, + 'subgraph': SUBGRAPH, + } + + ignorecase = True + + +class DotLexer(Lexer): + + scanner = DotScanner() + + def filter(self, type, text): + # TODO: handle charset + if type == STR_ID: + text = text[1:-1] + + # line continuations + text = text.replace('\\\r\n', '') + text = text.replace('\\\r', '') + text = text.replace('\\\n', '') + + text = text.replace('\\r', '\r') + text = text.replace('\\n', '\n') + text = text.replace('\\t', '\t') + text = text.replace('\\', '') + + type = ID + + elif type == HTML_ID: + text = text[1:-1] + type = ID + + return type, text + + +class DotParser(Parser): + + def __init__(self, lexer): + Parser.__init__(self, lexer) + self.graph_attrs = {} + self.node_attrs = {} + self.edge_attrs = {} + + def parse(self): + self.parse_graph() + self.match(EOF) + + def parse_graph(self): + if self.lookahead.type == STRICT: + self.consume() + self.skip(LCURLY) + self.consume() + while self.lookahead.type != RCURLY: + self.parse_stmt() + self.consume() + + def parse_subgraph(self): + id = None + shapes_before = set(self.shapes) + if self.lookahead.type == SUBGRAPH: + self.consume() + if self.lookahead.type == ID: + id = self.lookahead.text + self.consume() + if self.lookahead.type == LCURLY: + self.consume() + while self.lookahead.type != RCURLY: + self.parse_stmt() + self.consume() + new_shapes = set(self.shapes) - shapes_before + self.subgraph_shapes[id] = [s for s in new_shapes if not any([s in ss for ss in self.subgraph_shapes.values()])] + return id + + def parse_stmt(self): + if self.lookahead.type == GRAPH: + self.consume() + attrs = self.parse_attrs() + self.graph_attrs.update(attrs) + self.handle_graph(attrs) + elif self.lookahead.type == NODE: + self.consume() + self.node_attrs.update(self.parse_attrs()) + elif self.lookahead.type == EDGE: + self.consume() + self.edge_attrs.update(self.parse_attrs()) + elif self.lookahead.type in (SUBGRAPH, LCURLY): + self.parse_subgraph() + else: + id = self.parse_node_id() + if self.lookahead.type == EDGE_OP: + self.consume() + node_ids = [id, self.parse_node_id()] + while self.lookahead.type == EDGE_OP: + node_ids.append(self.parse_node_id()) + attrs = self.parse_attrs() + for i in range(0, len(node_ids) - 1): + self.handle_edge(node_ids[i], node_ids[i + 1], attrs) + elif self.lookahead.type == EQUAL: + self.consume() + self.parse_id() + else: + attrs = self.parse_attrs() + self.handle_node(id, attrs) + if self.lookahead.type == SEMI: + self.consume() + + def parse_attrs(self): + attrs = {} + while self.lookahead.type == LSQUARE: + self.consume() + while self.lookahead.type != RSQUARE: + name, value = self.parse_attr() + attrs[name] = value + if self.lookahead.type == COMMA: + self.consume() + self.consume() + return attrs + + def parse_attr(self): + name = self.parse_id() + if self.lookahead.type == EQUAL: + self.consume() + value = self.parse_id() + else: + value = 'true' + return name, value + + def parse_node_id(self): + node_id = self.parse_id() + if self.lookahead.type == COLON: + self.consume() + port = self.parse_id() + if self.lookahead.type == COLON: + self.consume() + compass_pt = self.parse_id() + else: + compass_pt = None + else: + port = None + compass_pt = None + # XXX: we don't really care about port and compass point values when parsing xdot + return node_id + + def parse_id(self): + self.match(ID) + id = self.lookahead.text + self.consume() + return id + + def handle_graph(self, attrs): + pass + + def handle_node(self, id, attrs): + pass + + def handle_edge(self, src_id, dst_id, attrs): + pass + + +class XDotParser(DotParser): + + def __init__(self, xdotcode): + lexer = DotLexer(buf = xdotcode) + DotParser.__init__(self, lexer) + + self.nodes = [] + self.edges = [] + self.shapes = [] + self.node_by_name = {} + self.top_graph = True + self.subgraph_shapes = {} + + def handle_graph(self, attrs): + if self.top_graph: + try: + bb = attrs['bb'] + except KeyError: + return + + if not bb: + return + + xmin, ymin, xmax, ymax = map(float, bb.split(",")) + + self.xoffset = -xmin + self.yoffset = -ymax + self.xscale = 1.0 + self.yscale = -1.0 + # FIXME: scale from points to pixels + + self.width = xmax - xmin + self.height = ymax - ymin + + self.top_graph = False + + for attr in ("_draw_", "_ldraw_", "_hdraw_", "_tdraw_", "_hldraw_", "_tldraw_"): + if attr in attrs: + parser = XDotAttrParser(self, attrs[attr]) + self.shapes.extend(parser.parse()) + + def handle_node(self, id, attrs): + try: + pos = attrs['pos'] + except KeyError: + return + #print(attrs) + + x, y = self.parse_node_pos(pos) + w = float(attrs['width'])*72 + h = float(attrs['height'])*72 + shapes = [] + for attr in ("_draw_", "_ldraw_"): + if attr in attrs: + parser = XDotAttrParser(self, attrs[attr]) + shapes.extend(parser.parse()) + url = attrs.get('URL', None) + node = Node(x, y, w, h, shapes, url) + self.node_by_name[id] = node + if shapes: + self.nodes.append(node) + + def handle_edge(self, src_id, dst_id, attrs): + try: + pos = attrs['pos'] + except KeyError: + return + + points = self.parse_edge_pos(pos) + shapes = [] + for attr in ("_draw_", "_ldraw_", "_hdraw_", "_tdraw_", "_hldraw_", "_tldraw_"): + if attr in attrs: + parser = XDotAttrParser(self, attrs[attr]) + shapes.extend(parser.parse()) + url = attrs.get('URL', None) + if shapes: + src = self.node_by_name[src_id] + dst = self.node_by_name[dst_id] + self.edges.append(Edge(src, dst, points, shapes, url)) + + def parse(self): + DotParser.parse(self) + + """ + for k,shapes in self.subgraph_shapes.iteritems(): + self.shapes += shapes + """ + + return Graph(self.width, self.height, self.shapes, self.nodes, self.edges, self.subgraph_shapes) + + def parse_node_pos(self, pos): + x, y = pos.split(",") + return self.transform(float(x), float(y)) + + def parse_edge_pos(self, pos): + points = [] + for entry in pos.split(' '): + fields = entry.split(',') + try: + x, y = fields + except ValueError: + # TODO: handle start/end points + #AB Handle data like like e,40,50 here + continue + else: + points.append(self.transform(float(x), float(y))) + return points + + def transform(self, x, y): + # XXX: this is not the right place for this code + x = (x + self.xoffset)*self.xscale + y = (y + self.yoffset)*self.yscale + return x, y + + +#-------------------------------------------------------------------------------------------------------------------------------------------------------------------- +class Animation(object): + + step = 0.03 # seconds + + def __init__(self, dot_widget): + self.dot_widget = dot_widget + self.timeout_id = None + + def start(self): + self.timeout_id = QTimer(); + #self.dot_widget.connect(self.timeout_id, SIGNAL('timeout()'), self.tick) + self.timeout_id.start(int(self.step * 1000)) + + def stop(self): + self.dot_widget.animation = NoAnimation(self.dot_widget) + if self.timeout_id is not None: + self.timeout_id.stop() + self.timeout_id = None + + def tick(self): + self.stop() + + +class NoAnimation(Animation): + + def start(self): + pass + + def stop(self): + pass + + +class LinearAnimation(Animation): + + duration = 0.6 + + def start(self): + self.started = time.time() + Animation.start(self) + + def tick(self): + t = (time.time() - self.started) / self.duration + self.animate(max(0, min(t, 1))) +# return (t < 1) #AB returning False stops the timer + if t >= 1: #AB + self.timeout_id.stop() #AB + + def animate(self, t): + pass + + +class MoveToAnimation(LinearAnimation): + + def __init__(self, dot_widget, target_x, target_y): + Animation.__init__(self, dot_widget) + self.source_x = dot_widget.x + self.source_y = dot_widget.y + self.target_x = target_x + self.target_y = target_y + + def animate(self, t): + sx, sy = self.source_x, self.source_y + tx, ty = self.target_x, self.target_y + self.dot_widget.x = tx * t + sx * (1-t) + self.dot_widget.y = ty * t + sy * (1-t) + self.dot_widget.update() + +class ZoomToAnimation(MoveToAnimation): + + def __init__(self, dot_widget, target_x, target_y): + MoveToAnimation.__init__(self, dot_widget, target_x, target_y) + self.source_zoom = dot_widget.zoom_ratio + self.target_zoom = self.source_zoom + self.extra_zoom = 0 + + middle_zoom = 0.5 * (self.source_zoom + self.target_zoom) + + distance = math.hypot(self.source_x - self.target_x, + self.source_y - self.target_y) +# rect = self.dot_widget.get_allocation() + rect = self.dot_widget.rect() +# visible = min(rect.width, rect.height) / self.dot_widget.zoom_ratio + visible = min(rect.width(), rect.height()) / self.dot_widget.zoom_ratio + visible *= 0.9 + if distance > 0: + desired_middle_zoom = visible / distance + self.extra_zoom = min(0, 4 * (desired_middle_zoom - middle_zoom)) + + def animate(self, t): + a, b, c = self.source_zoom, self.extra_zoom, self.target_zoom + self.dot_widget.zoom_ratio = c*t + b*t*(1-t) + a*(1-t) + self.dot_widget.zoom_to_fit_on_resize = False + MoveToAnimation.animate(self, t) + +#-------------------------------------------------------------------------------------------------------------------------------------------------------------------- +class DragAction(object): + + def __init__(self, dot_widget): + self.dot_widget = dot_widget + + def on_button_press(self, event): +# self.startmousex = self.prevmousex = event.x + self.startmousex = self.prevmousex = event.x() + self.startmousey = self.prevmousey = event.y() + self.start() + + def on_motion_notify(self, event): + deltax = self.prevmousex - event.x() + deltay = self.prevmousey - event.y() + self.drag(deltax, deltay) + self.prevmousex = event.x() + self.prevmousey = event.y() + + def on_button_release(self, event): + self.stopmousex = event.x() + self.stopmousey = event.y() + self.stop() + + def draw(self, cr): + pass + + def start(self): + pass + + def drag(self, deltax, deltay): + pass + + def stop(self): + pass + + def abort(self): + pass + + +class NullAction(DragAction): + + def on_motion_notify(self, event): + x, y = event.x(), event.y() + + dot_widget = self.dot_widget + item = dot_widget.get_url(x, y) + if item is None: + item = dot_widget.get_jump(x, y) + if item is not None: + dot_widget.setCursor(Qt.PointingHandCursor) + dot_widget.set_highlight(item.highlight) + else: + dot_widget.setCursor(Qt.ArrowCursor) + dot_widget.set_highlight(None) + + +class PanAction(DragAction): + + def start(self): + self.dot_widget.setCursor(Qt.ClosedHandCursor) + + def drag(self, deltax, deltay): + self.dot_widget.x += deltax / self.dot_widget.zoom_ratio + self.dot_widget.y += deltay / self.dot_widget.zoom_ratio + self.dot_widget.update() + + def stop(self): + self.dot_widget.cursor().setShape(Qt.ArrowCursor) + + abort = stop + + +class ZoomAction(DragAction): + + def drag(self, deltax, deltay): + self.dot_widget.zoom_ratio *= 1.005 ** (deltax + deltay) + self.dot_widget.zoom_to_fit_on_resize = False + self.dot_widget.update() + +def stop(self): + self.dot_widget.update() + + +class ZoomAreaAction(DragAction): + + def drag(self, deltax, deltay): + self.dot_widget.update() + + def draw(self, painter): + #TODO: implement this for qt + print("ERROR: UNIMPLEMENTED ZoomAreaAction.draw") + return + painter.save() + painter.set_source_rgba(.5, .5, 1.0, 0.25) + painter.rectangle(self.startmousex, self.startmousey, + self.prevmousex - self.startmousex, + self.prevmousey - self.startmousey) + painter.fill() + painter.set_source_rgba(.5, .5, 1.0, 1.0) + painter.set_line_width(1) + painter.rectangle(self.startmousex - .5, self.startmousey - .5, + self.prevmousex - self.startmousex + 1, + self.prevmousey - self.startmousey + 1) + painter.stroke() + painter.restore() + + def stop(self): + x1, y1 = self.dot_widget.window_to_graph(self.startmousex, + self.startmousey) + x2, y2 = self.dot_widget.window_to_graph(self.stopmousex, + self.stopmousey) + self.dot_widget.zoom_to_area(x1, y1, x2, y2) + + def abort(self): + self.dot_widget.update() +#-------------------------------------------------------------------------------------------------------------------------------------------------------------------- +class DotWidget(QWidget): +#class DotWidget(gtk.DrawingArea): + """Qt widget that draws dot graphs.""" + + filter = 'dot' + + def __init__(self, parent=None): + super(DotWidget, self).__init__(parent) + self.graph = Graph() + self.openfilename = None + +# self.set_flags(gtk.CAN_FOCUS) +# self.add_events(gtk.gdk.BUTTON_PRESS_MASK | gtk.gdk.BUTTON_RELEASE_MASK) +# self.connect("button-press-event", self.on_area_button_press) +# self.connect("button-release-event", self.on_area_button_release) +# self.add_events(gtk.gdk.POINTER_MOTION_MASK | gtk.gdk.POINTER_MOTION_HINT_MASK | gtk.gdk.BUTTON_RELEASE_MASK) +# self.connect("motion-notify-event", self.on_area_motion_notify) +# self.connect("scroll-event", self.on_area_scroll_event) +# self.connect("size-allocate", self.on_area_size_allocate) +# self.connect('key-press-event', self.on_key_press_event) + + self.x, self.y = 0.0, 0.0 + self.zoom_ratio = 1.0 + self.zoom_to_fit_on_resize = False + self.animation = NoAnimation(self) + self.drag_action = NullAction(self) + self.presstime = None + self.highlight = None + + # Callback register + self.select_cbs = [] + self.dc = None + self.ctx = None + self.items_by_url = {} + + self.setMouseTracking (True) # track all mouse events + + ZOOM_INCREMENT = 1.25 + ZOOM_TO_FIT_MARGIN = 12 + POS_INCREMENT = 100 + + ### User callbacks + def register_select_callback(self, cb): + self.select_cbs.append(cb) + + def set_filter(self, filter): + self.filter = filter + + def set_dotcode(self, dotcode, filename='',center=True): + #if isinstance(dotcode, str):# unicode): # CHANGED + # dotcode = dotcode.encode('utf8') + p = subprocess.Popen( + [self.filter, '-Txdot'], + stdin=subprocess.PIPE, + stdout=subprocess.PIPE, + stderr=subprocess.PIPE, + shell=False, + universal_newlines=True + ) + xdotcode, error = p.communicate(dotcode) + if p.returncode != 0: + print("UNABLE TO SHELL TO DOT", error) +# dialog = gtk.MessageDialog(type=gtk.MESSAGE_ERROR, +# message_format=error, +# buttons=gtk.BUTTONS_OK) +# dialog.set_title('Dot Viewer') +# dialog.run() +# dialog.destroy() + return False + try: + self.set_xdotcode(xdotcode,center) + + # Store references to all the items + self.items_by_url = {} + for item in self.graph.nodes + self.graph.edges: + if item.url is not None: + self.items_by_url[item.url] = item + + # Store references to subgraph states + self.subgraph_shapes = self.graph.subgraph_shapes + + except ParseError as ex:#, ex: + print('ParseError', ex) +# dialog = gtk.MessageDialog(type=gtk.MESSAGE_ERROR, +# message_format=str(ex), +# buttons=gtk.BUTTONS_OK) +# dialog.set_title('Dot Viewer') +# dialog.run() +# dialog.destroy() + return False + else: + self.openfilename = filename + return True + + def set_xdotcode(self, xdotcode, center=True): + #print xdotcode + parser = XDotParser(xdotcode) + self.graph = parser.parse() + #self.zoom_image(self.zoom_ratio)#, center=center) + self.zoom_to_fit() + + def reload(self): + if self.openfilename is not None: + try: + fp = open(str(self.openfilename), "rb") + self.set_dotcode(fp.read(), self.openfilename) + fp.close() + except IOError: + pass + + def paintEvent (self, event=None): +# def do_expose_event(self, event): +# cr = self.window.cairo_create() + painter = QPainter (self) + painter.setRenderHint(QPainter.Antialiasing) + painter.setRenderHint(QPainter.TextAntialiasing) + painter.setRenderHint(QPainter.HighQualityAntialiasing) + + # set a clip region for the expose event +# cr.rectangle( +# event.area.x, event.area.y, +# event.area.width, event.area.height +# ) +# cr.clip() +# +# cr.set_source_rgba(1.0, 1.0, 1.0, 1.0) +# cr.paint() + + painter.setClipping(True) + painter.setClipRect(self.rect()) + painter.setBackground(QBrush(Qt.blue,Qt.SolidPattern)) + painter.save() + +# rect = self.get_allocation() + rect = self.rect() # JRB was self.rect() +# cr.translate(0.5*rect.width, 0.5*rect.height) + painter.translate(0.5*rect.width(), 0.5*rect.height()) + painter.scale(self.zoom_ratio, self.zoom_ratio) + painter.translate(-self.x, -self.y) + + self.graph.draw(painter, highlight_items=self.highlight) + painter.restore() + + self.drag_action.draw(painter) + + + def get_current_pos(self): + return self.x, self.y + +#AB This function is not used + def set_current_pos(self, x, y): + self.x = x + self.y = y + self.update() + + def set_highlight(self, items): + if self.highlight != items: + self.highlight = items + self.update() + + def zoom_image(self, zoom_ratio, center=False, pos=None): + if center: + self.x = self.graph.width/2 + self.y = self.graph.height/2 + elif pos is not None: +# rect = self.get_allocation() + rect = self.rect() + x, y = pos +# x -= 0.5*rect.width + x -= 0.5*rect.width() +# y -= 0.5*rect.height + y -= 0.5*rect.height() + self.x += x / self.zoom_ratio - x / zoom_ratio + self.y += y / self.zoom_ratio - y / zoom_ratio + self.zoom_ratio = zoom_ratio + self.zoom_to_fit_on_resize = False + self.update() + + def zoom_to_area(self, x1, y1, x2, y2): +# rect = self.get_allocation() + rect = self.rect() + width = abs(x1 - x2) + height = abs(y1 - y2) + self.zoom_ratio = min( + float(rect.width())/float(width), + float(rect.height())/float(height) + ) + self.zoom_to_fit_on_resize = False + self.x = (x1 + x2) / 2 + self.y = (y1 + y2) / 2 + self.update() + + def zoom_to_fit(self): +# rect = self.get_allocation() + rect = self.rect() +# rect.x += self.ZOOM_TO_FIT_MARGIN + rect.setX (rect.x() + self.ZOOM_TO_FIT_MARGIN) +# rect.y += self.ZOOM_TO_FIT_MARGIN + rect.setY (rect.y() + self.ZOOM_TO_FIT_MARGIN) +# rect.width -= 2 * self.ZOOM_TO_FIT_MARGIN + rect.setWidth(rect.width() - 2 * self.ZOOM_TO_FIT_MARGIN) +# rect.height -= 2 * self.ZOOM_TO_FIT_MARGIN + rect.setHeight(rect.height() - 2 * self.ZOOM_TO_FIT_MARGIN) + zoom_ratio = min( +# float(rect.width)/float(self.graph.width), + float(rect.width())/float(self.graph.width), +# float(rect.height)/float(self.graph.height) + float(rect.height())/float(self.graph.height) + ) + self.zoom_image(zoom_ratio, center=True) + self.zoom_to_fit_on_resize = True + + def on_zoom_in(self): +# def on_zoom_in(self, action): + self.zoom_image(self.zoom_ratio * self.ZOOM_INCREMENT) + + def on_zoom_out(self): +# def on_zoom_out(self, action): + self.zoom_image(self.zoom_ratio / self.ZOOM_INCREMENT) + + def on_zoom_fit(self): +# def on_zoom_fit(self, action): + self.zoom_to_fit() + + def on_zoom_100(self): +# def on_zoom_100(self, action): + self.zoom_image(1.0) + + def keyPressEvent(self, event): + self.animation.stop() + self.drag_action.abort() + if event.key() == Qt.Key_Left: + self.x -= self.POS_INCREMENT/self.zoom_ratio + self.update() + elif event.key() == Qt.Key_Right: + self.x += self.POS_INCREMENT/self.zoom_ratio + self.update() + elif event.key() == Qt.Key_Up: + self.y -= self.POS_INCREMENT/self.zoom_ratio + self.update() + elif event.key() == Qt.Key_Down: + self.y += self.POS_INCREMENT/self.zoom_ratio + self.update() + elif event.key() == Qt.Key_PageUp: + self.zoom_image(self.zoom_ratio * self.ZOOM_INCREMENT) + self.update() + elif event.key() == Qt.Key_PageDown: + self.zoom_image(self.zoom_ratio / self.ZOOM_INCREMENT) + self.update() + elif event.key() == Qt.Key_PageUp: + self.drag_action.abort() + self.drag_action = NullAction(self) + elif event.key() == Qt.Key_R: + self.reload() + elif event.key() == Qt.Key_F: + self.zoom_to_fit() + event.accept() + + def get_drag_action(self, event): + modifiers = event.modifiers() + if event.button() in (Qt.LeftButton, Qt.MidButton): + if modifiers & Qt.ControlModifier: + return ZoomAction + elif modifiers & Qt.ShiftModifier: + return ZoomAreaAction + else: + return PanAction + return NullAction + + def mousePressEvent(self, event): + self.animation.stop() + self.drag_action.abort() + + for cb in self.select_cbs: + cb(event) + + action_type = self.get_drag_action(event) + self.drag_action = action_type(self) + self.drag_action.on_button_press(event) + + self.presstime = time.time() + self.pressx = event.x() + self.pressy = event.y() + event.accept() + + def is_click(self, event, click_fuzz=4, click_timeout=1.0): + if self.presstime is None: + # got a button release without seeing the press? + return False + # XXX instead of doing this complicated logic, shouldn't we listen + # for gtk's clicked event instead? + deltax = self.pressx - event.x() + deltay = self.pressy - event.y() + return (time.time() < self.presstime + click_timeout + and math.hypot(deltax, deltay) < click_fuzz) + + def mouseReleaseEvent(self, event): + self.drag_action.on_button_release(event) + self.drag_action = NullAction(self) + if event.button() == Qt.LeftButton and self.is_click(event): + x, y = event.x(), event.y() + url = self.get_url(x, y) + #print(url) + if url is not None: + #self.emit(SIGNAL("clicked"), unicode(url.url), event) + pass + else: + #self.emit(SIGNAL("clicked"), 'none', event) + jump = self.get_jump(x, y) + if jump is not None: + self.animate_to(jump.x, jump.y) + + event.accept() + return + if event.button() == Qt.RightButton and self.is_click(event): + x, y = event.x(), event.y() + url = self.get_url(x, y) + if url is not None: + #self.emit(SIGNAL("right_clicked"), unicode(url.url), event) + pass + else: + #self.emit(SIGNAL("right_clicked"), 'none', event) + pass + jump = self.get_jump(x, y) + if jump is not None: + self.animate_to(jump.x, jump.y) + + if event.button() in (Qt.LeftButton, Qt.MidButton): + event.accept() + return + + def on_area_scroll_event(self, area, event): + return False + + def wheelEvent(self, event): + if event.angleDelta().y() > 0: + self.zoom_image(self.zoom_ratio * self.ZOOM_INCREMENT, + pos=(event.x(), event.y())) + if event.angleDelta().y() < 0: + self.zoom_image(self.zoom_ratio / self.ZOOM_INCREMENT, + pos=(event.x(), event.y())) + + def mouseMoveEvent(self, event): + self.drag_action.on_motion_notify(event) + self.setFocus() + for cb in self.select_cbs: + cb(event) + + def on_area_size_allocate(self, area, allocation): + if self.zoom_to_fit_on_resize: + self.zoom_to_fit() + + def animate_to(self, x, y): + self.animation = ZoomToAnimation(self, x, y) + self.animation.start() + + def window_to_graph(self, x, y): +# rect = self.get_allocation() + rect = self.rect() + x -= 0.5*rect.width() + y -= 0.5*rect.height() + x /= self.zoom_ratio + y /= self.zoom_ratio + x += self.x + y += self.y + return x, y + + def get_url(self, x, y): + x, y = self.window_to_graph(x, y) + return self.graph.get_url(x, y) + + def get_jump(self, x, y): + x, y = self.window_to_graph(x, y) + return self.graph.get_jump(x, y) + + +#-------------------------------------------------------------------------------------------------------------------------------------------------------------------- +#class DotWindow(gtk.Window): +class DotWindow(QMainWindow): + + def __init__(self): + super(DotWindow, self).__init__(None) + self.graph = Graph() + self.setWindowTitle(QApplication.applicationName()) + self.widget = DotWidget() + self.widget.setContextMenuPolicy(Qt.ActionsContextMenu) + self.setCentralWidget(self.widget) + + palette = QPalette () + palette.setColor(QPalette.Background, Qt.white) + self.setPalette(palette) + + self.filename = None + + file_open_action = self.create_action("&Open...", self.on_open, + QKeySequence.Open, "fileopen", "Open an existing dot file") + file_reload_action = self.create_action("&Refresh", self.on_reload, + QKeySequence.Refresh, "view-refresh", "Reload opened dot file") + zoom_in_action = self.create_action("Zoom In", self.widget.on_zoom_in, + QKeySequence.ZoomIn, "zoom-in", "Zoom in") + zoom_out_action = self.create_action("Zoom Out", self.widget.on_zoom_out, + QKeySequence.ZoomIn, "zoom-out", "Zoom Out") + zoom_fit_action = self.create_action("Zoom Fit", self.widget.on_zoom_fit, + None, "zoom-fit-best", "Zoom Fit") + zoom_100_action = self.create_action("Zoom 100%", self.widget.on_zoom_100, + None, "zoom-original", "Zoom 100%") + + self.file_menu = self.menuBar().addMenu("&File") + self.file_menu_actions = (file_open_action, file_reload_action) + self.connect(self.file_menu, SIGNAL("aboutToShow()"), self.update_file_menu) + + file_toolbar = self.addToolBar("File") + file_toolbar.setObjectName("FileToolBar") + self.add_actions(file_toolbar, (file_open_action, file_reload_action)) + + fileToolbar = self.addToolBar("Zoom") + fileToolbar.setObjectName("ZoomToolBar") + self.add_actions(fileToolbar, (zoom_in_action, zoom_out_action, zoom_fit_action, zoom_100_action)) + + settings = QSettings() + self.recent_files = settings.value("RecentFiles").toStringList() + size = settings.value("MainWindow/Size", QVariant(QSize(512, 512))).toSize() + self.resize(size) + position = settings.value("MainWindow/Position", QVariant(QPoint(0, 0))).toPoint() + self.move(position) + + self.restoreState(settings.value("MainWindow/State").toByteArray()) + self.update_file_menu() + + self.show() + + def create_action(self, text, slot=None, shortcut=None, icon=None, + tip=None, checkable=False, signal="triggered()"): + action = QAction(text, self) + if icon is not None: + action.setIcon(QIcon.fromTheme(icon)) + if shortcut is not None: + action.setShortcut(shortcut) + if tip is not None: + action.setToolTip(tip) + action.setStatusTip(tip) + if slot is not None: + self.connect(action, SIGNAL(signal), slot) + if checkable: + action.setCheckable(True) + return action + + def add_actions(self, target, actions): + for action in actions: + if action is None: + target.addSeparator() + else: + target.addAction(action) + + def update_file(self, filename): + import os + if not hasattr(self, "last_mtime"): + self.last_mtime = None + + current_mtime = os.stat(filename).st_mtime + if current_mtime != self.last_mtime: + self.last_mtime = current_mtime + self.open_file(filename) + + return True + + def set_filter(self, filter): + self.widget.set_filter(filter) + + def set_dotcode(self, dotcode, filename=''): + if self.widget.set_dotcode(dotcode, filename): + self.setWindowTitle(os.path.basename(filename) + ' - ' + QApplication.applicationName()) + self.widget.zoom_to_fit() + + def set_xdotcode(self, xdotcode, filename=''): + if self.widget.set_xdotcode(xdotcode): + self.setWindowTitle(os.path.basename(filename) + ' - ' + QApplication.applicationName()) + self.widget.zoom_to_fit() + + def open_file(self, filename=None): + if filename is None: + action = self.sender() + if isinstance(action, QAction): + filename = str(action.data().toString()) # CHANGED + else: + return + try: + fp = file(filename, 'rt') + self.set_dotcode(fp.read(), filename) + fp.close() + self.add_recent_file(filename) + except IOError:#, ex: + pass + + def on_open(self): + dir = os.path.dirname(self.filename) \ + if self.filename is not None else "." + formats = ["*.dot"] + filename = str(QFileDialog.getOpenFileName(self, # CHANGED + "Open dot File", dir, + "Dot files (%s)" % " ".join(formats))) + if filename: + self.open_file(filename) + + def on_reload(self): + self.widget.reload() + + def update_file_menu(self): + self.file_menu.clear() + self.add_actions(self.file_menu, self.file_menu_actions[:-1]) + current = QString(self.filename) \ + if self.filename is not None else None + recent_files = [] + for fname in self.recent_files: + if fname != current and QFile.exists(fname): + recent_files.append(fname) + if recent_files: + self.file_menu.addSeparator() + for i, fname in enumerate(recent_files): + action = QAction(QIcon(":/icon.png"), "&%d %s" % (i + 1, QFileInfo(fname).fileName()), self) + action.setData(QVariant(fname)) + self.connect(action, SIGNAL("triggered()"), self.open_file) + self.file_menu.addAction(action) + self.file_menu.addSeparator() + self.file_menu.addAction(self.file_menu_actions[-1]) + + def add_recent_file(self, filename): + if filename is None: + return + if not self.recent_files.contains(filename): + self.recent_files.prepend(QString(filename)) + while self.recent_files.count() > 14: + self.recent_files.takeLast() + +def closeEvent(self, event): + settings = QSettings() + filename = QVariant(QString(self.filename)) if self.filename is not None else QVariant() + settings.setValue("LastFile", filename) + recent_files = QVariant(self.recent_files) if self.recent_files else QVariant() + settings.setValue("RecentFiles", recent_files) + settings.setValue("MainWindow/Size", QVariant(self.size())) + + #AB It looks like PyQt 4.7.4 has a bug that results in both self.pos() and self.geometry() returning the same values + # see http://doc.qt.nokia.com/latest/application-windows.html#window-geometry for explanation of why they should be different + # The results is a small (5,24) shift of window on consequent start from previous position + #print (self.pos()) + #print (self.geometry()) + settings.setValue("MainWindow/Position", QVariant(self.pos())) + #settings.setValue("MainWindow/Geometry", QVariant(self.saveGeometry())) + settings.setValue("MainWindow/State", QVariant(self.saveState())) + +#-------------------------------------------------------------------------------------------------------------------------------------------------------------------- +def main(): + import optparse + + parser = optparse.OptionParser(usage='\n\t%prog [file]', version='%%prog %s' % __version__) + # This program can shell to different Graphviz filter processes specified by -f option + parser.add_option( + '-f', '--filter', + type='choice', choices=('dot', 'neato', 'twopi', 'circo', 'fdp'), + dest='filter', default='dot', + help='graphviz filter: dot, neato, twopi, circo, or fdp [default: %default]') + + (options, args) = parser.parse_args(sys.argv[1:]) + if len(args) > 1: + parser.error('incorrect number of arguments') + + app = QApplication(sys.argv) + app.setOrganizationName("RobotNV") + app.setOrganizationDomain("robotNV.com") + app.setApplicationName("Dot Viewer") + app.setWindowIcon(QIcon(":/icon.png")) + + win = DotWindow() + win.show() +# win.connect('destroy', gtk.main_quit) +# win.set_filter(options.filter) + if len(args) >= 1: + if args[0] == '-': + win.set_dotcode(sys.stdin.read()) + else: + win.open_file(args[0]) +# gobject.timeout_add(1000, win.update_file, args[0]) +# gtk.main() + sys.exit(app.exec_()) + + + + +# Apache-Style Software License for ColorBrewer software and ColorBrewer Color +# Schemes, Version 1.1 +# +# Copyright (c) 2002 Cynthia Brewer, Mark Harrower, and The Pennsylvania State +# University. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions as source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# +# 2. The end-user documentation included with the redistribution, if any, +# must include the following acknowledgment: +# +# This product includes color specifications and designs developed by +# Cynthia Brewer (http://colorbrewer.org/). +# +# Alternately, this acknowledgment may appear in the software itself, if and +# wherever such third-party acknowledgments normally appear. +# +# 3. The name "ColorBrewer" must not be used to endorse or promote products +# derived from this software without prior written permission. For written +# permission, please contact Cynthia Brewer at cbrewer@psu.edu. +# +# 4. Products derived from this software may not be called "ColorBrewer", +# nor may "ColorBrewer" appear in their name, without prior written +# permission of Cynthia Brewer. +# +# THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESSED OR IMPLIED WARRANTIES, +# INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND +# FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL CYNTHIA +# BREWER, MARK HARROWER, OR THE PENNSYLVANIA STATE UNIVERSITY BE LIABLE FOR ANY +# DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +# (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +# ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +# SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +brewer_colors = { + 'accent3': [(127, 201, 127), (190, 174, 212), (253, 192, 134)], + 'accent4': [(127, 201, 127), (190, 174, 212), (253, 192, 134), (255, 255, 153)], + 'accent5': [(127, 201, 127), (190, 174, 212), (253, 192, 134), (255, 255, 153), (56, 108, 176)], + 'accent6': [(127, 201, 127), (190, 174, 212), (253, 192, 134), (255, 255, 153), (56, 108, 176), (240, 2, 127)], + 'accent7': [(127, 201, 127), (190, 174, 212), (253, 192, 134), (255, 255, 153), (56, 108, 176), (240, 2, 127), (191, 91, 23)], + 'accent8': [(127, 201, 127), (190, 174, 212), (253, 192, 134), (255, 255, 153), (56, 108, 176), (240, 2, 127), (191, 91, 23), (102, 102, 102)], + 'blues3': [(222, 235, 247), (158, 202, 225), (49, 130, 189)], + 'blues4': [(239, 243, 255), (189, 215, 231), (107, 174, 214), (33, 113, 181)], + 'blues5': [(239, 243, 255), (189, 215, 231), (107, 174, 214), (49, 130, 189), (8, 81, 156)], + 'blues6': [(239, 243, 255), (198, 219, 239), (158, 202, 225), (107, 174, 214), (49, 130, 189), (8, 81, 156)], + 'blues7': [(239, 243, 255), (198, 219, 239), (158, 202, 225), (107, 174, 214), (66, 146, 198), (33, 113, 181), (8, 69, 148)], + 'blues8': [(247, 251, 255), (222, 235, 247), (198, 219, 239), (158, 202, 225), (107, 174, 214), (66, 146, 198), (33, 113, 181), (8, 69, 148)], + 'blues9': [(247, 251, 255), (222, 235, 247), (198, 219, 239), (158, 202, 225), (107, 174, 214), (66, 146, 198), (33, 113, 181), (8, 81, 156), (8, 48, 107)], + 'brbg10': [(84, 48, 5), (0, 60, 48), (140, 81, 10), (191, 129, 45), (223, 194, 125), (246, 232, 195), (199, 234, 229), (128, 205, 193), (53, 151, 143), (1, 102, 94)], + 'brbg11': [(84, 48, 5), (1, 102, 94), (0, 60, 48), (140, 81, 10), (191, 129, 45), (223, 194, 125), (246, 232, 195), (245, 245, 245), (199, 234, 229), (128, 205, 193), (53, 151, 143)], + 'brbg3': [(216, 179, 101), (245, 245, 245), (90, 180, 172)], + 'brbg4': [(166, 97, 26), (223, 194, 125), (128, 205, 193), (1, 133, 113)], + 'brbg5': [(166, 97, 26), (223, 194, 125), (245, 245, 245), (128, 205, 193), (1, 133, 113)], + 'brbg6': [(140, 81, 10), (216, 179, 101), (246, 232, 195), (199, 234, 229), (90, 180, 172), (1, 102, 94)], + 'brbg7': [(140, 81, 10), (216, 179, 101), (246, 232, 195), (245, 245, 245), (199, 234, 229), (90, 180, 172), (1, 102, 94)], + 'brbg8': [(140, 81, 10), (191, 129, 45), (223, 194, 125), (246, 232, 195), (199, 234, 229), (128, 205, 193), (53, 151, 143), (1, 102, 94)], + 'brbg9': [(140, 81, 10), (191, 129, 45), (223, 194, 125), (246, 232, 195), (245, 245, 245), (199, 234, 229), (128, 205, 193), (53, 151, 143), (1, 102, 94)], + 'bugn3': [(229, 245, 249), (153, 216, 201), (44, 162, 95)], + 'bugn4': [(237, 248, 251), (178, 226, 226), (102, 194, 164), (35, 139, 69)], + 'bugn5': [(237, 248, 251), (178, 226, 226), (102, 194, 164), (44, 162, 95), (0, 109, 44)], + 'bugn6': [(237, 248, 251), (204, 236, 230), (153, 216, 201), (102, 194, 164), (44, 162, 95), (0, 109, 44)], + 'bugn7': [(237, 248, 251), (204, 236, 230), (153, 216, 201), (102, 194, 164), (65, 174, 118), (35, 139, 69), (0, 88, 36)], + 'bugn8': [(247, 252, 253), (229, 245, 249), (204, 236, 230), (153, 216, 201), (102, 194, 164), (65, 174, 118), (35, 139, 69), (0, 88, 36)], + 'bugn9': [(247, 252, 253), (229, 245, 249), (204, 236, 230), (153, 216, 201), (102, 194, 164), (65, 174, 118), (35, 139, 69), (0, 109, 44), (0, 68, 27)], + 'bupu3': [(224, 236, 244), (158, 188, 218), (136, 86, 167)], + 'bupu4': [(237, 248, 251), (179, 205, 227), (140, 150, 198), (136, 65, 157)], + 'bupu5': [(237, 248, 251), (179, 205, 227), (140, 150, 198), (136, 86, 167), (129, 15, 124)], + 'bupu6': [(237, 248, 251), (191, 211, 230), (158, 188, 218), (140, 150, 198), (136, 86, 167), (129, 15, 124)], + 'bupu7': [(237, 248, 251), (191, 211, 230), (158, 188, 218), (140, 150, 198), (140, 107, 177), (136, 65, 157), (110, 1, 107)], + 'bupu8': [(247, 252, 253), (224, 236, 244), (191, 211, 230), (158, 188, 218), (140, 150, 198), (140, 107, 177), (136, 65, 157), (110, 1, 107)], + 'bupu9': [(247, 252, 253), (224, 236, 244), (191, 211, 230), (158, 188, 218), (140, 150, 198), (140, 107, 177), (136, 65, 157), (129, 15, 124), (77, 0, 75)], + 'dark23': [(27, 158, 119), (217, 95, 2), (117, 112, 179)], + 'dark24': [(27, 158, 119), (217, 95, 2), (117, 112, 179), (231, 41, 138)], + 'dark25': [(27, 158, 119), (217, 95, 2), (117, 112, 179), (231, 41, 138), (102, 166, 30)], + 'dark26': [(27, 158, 119), (217, 95, 2), (117, 112, 179), (231, 41, 138), (102, 166, 30), (230, 171, 2)], + 'dark27': [(27, 158, 119), (217, 95, 2), (117, 112, 179), (231, 41, 138), (102, 166, 30), (230, 171, 2), (166, 118, 29)], + 'dark28': [(27, 158, 119), (217, 95, 2), (117, 112, 179), (231, 41, 138), (102, 166, 30), (230, 171, 2), (166, 118, 29), (102, 102, 102)], + 'gnbu3': [(224, 243, 219), (168, 221, 181), (67, 162, 202)], + 'gnbu4': [(240, 249, 232), (186, 228, 188), (123, 204, 196), (43, 140, 190)], + 'gnbu5': [(240, 249, 232), (186, 228, 188), (123, 204, 196), (67, 162, 202), (8, 104, 172)], + 'gnbu6': [(240, 249, 232), (204, 235, 197), (168, 221, 181), (123, 204, 196), (67, 162, 202), (8, 104, 172)], + 'gnbu7': [(240, 249, 232), (204, 235, 197), (168, 221, 181), (123, 204, 196), (78, 179, 211), (43, 140, 190), (8, 88, 158)], + 'gnbu8': [(247, 252, 240), (224, 243, 219), (204, 235, 197), (168, 221, 181), (123, 204, 196), (78, 179, 211), (43, 140, 190), (8, 88, 158)], + 'gnbu9': [(247, 252, 240), (224, 243, 219), (204, 235, 197), (168, 221, 181), (123, 204, 196), (78, 179, 211), (43, 140, 190), (8, 104, 172), (8, 64, 129)], + 'greens3': [(229, 245, 224), (161, 217, 155), (49, 163, 84)], + 'greens4': [(237, 248, 233), (186, 228, 179), (116, 196, 118), (35, 139, 69)], + 'greens5': [(237, 248, 233), (186, 228, 179), (116, 196, 118), (49, 163, 84), (0, 109, 44)], + 'greens6': [(237, 248, 233), (199, 233, 192), (161, 217, 155), (116, 196, 118), (49, 163, 84), (0, 109, 44)], + 'greens7': [(237, 248, 233), (199, 233, 192), (161, 217, 155), (116, 196, 118), (65, 171, 93), (35, 139, 69), (0, 90, 50)], + 'greens8': [(247, 252, 245), (229, 245, 224), (199, 233, 192), (161, 217, 155), (116, 196, 118), (65, 171, 93), (35, 139, 69), (0, 90, 50)], + 'greens9': [(247, 252, 245), (229, 245, 224), (199, 233, 192), (161, 217, 155), (116, 196, 118), (65, 171, 93), (35, 139, 69), (0, 109, 44), (0, 68, 27)], + 'greys3': [(240, 240, 240), (189, 189, 189), (99, 99, 99)], + 'greys4': [(247, 247, 247), (204, 204, 204), (150, 150, 150), (82, 82, 82)], + 'greys5': [(247, 247, 247), (204, 204, 204), (150, 150, 150), (99, 99, 99), (37, 37, 37)], + 'greys6': [(247, 247, 247), (217, 217, 217), (189, 189, 189), (150, 150, 150), (99, 99, 99), (37, 37, 37)], + 'greys7': [(247, 247, 247), (217, 217, 217), (189, 189, 189), (150, 150, 150), (115, 115, 115), (82, 82, 82), (37, 37, 37)], + 'greys8': [(255, 255, 255), (240, 240, 240), (217, 217, 217), (189, 189, 189), (150, 150, 150), (115, 115, 115), (82, 82, 82), (37, 37, 37)], + 'greys9': [(255, 255, 255), (240, 240, 240), (217, 217, 217), (189, 189, 189), (150, 150, 150), (115, 115, 115), (82, 82, 82), (37, 37, 37), (0, 0, 0)], + 'oranges3': [(254, 230, 206), (253, 174, 107), (230, 85, 13)], + 'oranges4': [(254, 237, 222), (253, 190, 133), (253, 141, 60), (217, 71, 1)], + 'oranges5': [(254, 237, 222), (253, 190, 133), (253, 141, 60), (230, 85, 13), (166, 54, 3)], + 'oranges6': [(254, 237, 222), (253, 208, 162), (253, 174, 107), (253, 141, 60), (230, 85, 13), (166, 54, 3)], + 'oranges7': [(254, 237, 222), (253, 208, 162), (253, 174, 107), (253, 141, 60), (241, 105, 19), (217, 72, 1), (140, 45, 4)], + 'oranges8': [(255, 245, 235), (254, 230, 206), (253, 208, 162), (253, 174, 107), (253, 141, 60), (241, 105, 19), (217, 72, 1), (140, 45, 4)], + 'oranges9': [(255, 245, 235), (254, 230, 206), (253, 208, 162), (253, 174, 107), (253, 141, 60), (241, 105, 19), (217, 72, 1), (166, 54, 3), (127, 39, 4)], + 'orrd3': [(254, 232, 200), (253, 187, 132), (227, 74, 51)], + 'orrd4': [(254, 240, 217), (253, 204, 138), (252, 141, 89), (215, 48, 31)], + 'orrd5': [(254, 240, 217), (253, 204, 138), (252, 141, 89), (227, 74, 51), (179, 0, 0)], + 'orrd6': [(254, 240, 217), (253, 212, 158), (253, 187, 132), (252, 141, 89), (227, 74, 51), (179, 0, 0)], + 'orrd7': [(254, 240, 217), (253, 212, 158), (253, 187, 132), (252, 141, 89), (239, 101, 72), (215, 48, 31), (153, 0, 0)], + 'orrd8': [(255, 247, 236), (254, 232, 200), (253, 212, 158), (253, 187, 132), (252, 141, 89), (239, 101, 72), (215, 48, 31), (153, 0, 0)], + 'orrd9': [(255, 247, 236), (254, 232, 200), (253, 212, 158), (253, 187, 132), (252, 141, 89), (239, 101, 72), (215, 48, 31), (179, 0, 0), (127, 0, 0)], + 'paired10': [(166, 206, 227), (106, 61, 154), (31, 120, 180), (178, 223, 138), (51, 160, 44), (251, 154, 153), (227, 26, 28), (253, 191, 111), (255, 127, 0), (202, 178, 214)], + 'paired11': [(166, 206, 227), (106, 61, 154), (255, 255, 153), (31, 120, 180), (178, 223, 138), (51, 160, 44), (251, 154, 153), (227, 26, 28), (253, 191, 111), (255, 127, 0), (202, 178, 214)], + 'paired12': [(166, 206, 227), (106, 61, 154), (255, 255, 153), (177, 89, 40), (31, 120, 180), (178, 223, 138), (51, 160, 44), (251, 154, 153), (227, 26, 28), (253, 191, 111), (255, 127, 0), (202, 178, 214)], + 'paired3': [(166, 206, 227), (31, 120, 180), (178, 223, 138)], + 'paired4': [(166, 206, 227), (31, 120, 180), (178, 223, 138), (51, 160, 44)], + 'paired5': [(166, 206, 227), (31, 120, 180), (178, 223, 138), (51, 160, 44), (251, 154, 153)], + 'paired6': [(166, 206, 227), (31, 120, 180), (178, 223, 138), (51, 160, 44), (251, 154, 153), (227, 26, 28)], + 'paired7': [(166, 206, 227), (31, 120, 180), (178, 223, 138), (51, 160, 44), (251, 154, 153), (227, 26, 28), (253, 191, 111)], + 'paired8': [(166, 206, 227), (31, 120, 180), (178, 223, 138), (51, 160, 44), (251, 154, 153), (227, 26, 28), (253, 191, 111), (255, 127, 0)], + 'paired9': [(166, 206, 227), (31, 120, 180), (178, 223, 138), (51, 160, 44), (251, 154, 153), (227, 26, 28), (253, 191, 111), (255, 127, 0), (202, 178, 214)], + 'pastel13': [(251, 180, 174), (179, 205, 227), (204, 235, 197)], + 'pastel14': [(251, 180, 174), (179, 205, 227), (204, 235, 197), (222, 203, 228)], + 'pastel15': [(251, 180, 174), (179, 205, 227), (204, 235, 197), (222, 203, 228), (254, 217, 166)], + 'pastel16': [(251, 180, 174), (179, 205, 227), (204, 235, 197), (222, 203, 228), (254, 217, 166), (255, 255, 204)], + 'pastel17': [(251, 180, 174), (179, 205, 227), (204, 235, 197), (222, 203, 228), (254, 217, 166), (255, 255, 204), (229, 216, 189)], + 'pastel18': [(251, 180, 174), (179, 205, 227), (204, 235, 197), (222, 203, 228), (254, 217, 166), (255, 255, 204), (229, 216, 189), (253, 218, 236)], + 'pastel19': [(251, 180, 174), (179, 205, 227), (204, 235, 197), (222, 203, 228), (254, 217, 166), (255, 255, 204), (229, 216, 189), (253, 218, 236), (242, 242, 242)], + 'pastel23': [(179, 226, 205), (253, 205, 172), (203, 213, 232)], + 'pastel24': [(179, 226, 205), (253, 205, 172), (203, 213, 232), (244, 202, 228)], + 'pastel25': [(179, 226, 205), (253, 205, 172), (203, 213, 232), (244, 202, 228), (230, 245, 201)], + 'pastel26': [(179, 226, 205), (253, 205, 172), (203, 213, 232), (244, 202, 228), (230, 245, 201), (255, 242, 174)], + 'pastel27': [(179, 226, 205), (253, 205, 172), (203, 213, 232), (244, 202, 228), (230, 245, 201), (255, 242, 174), (241, 226, 204)], + 'pastel28': [(179, 226, 205), (253, 205, 172), (203, 213, 232), (244, 202, 228), (230, 245, 201), (255, 242, 174), (241, 226, 204), (204, 204, 204)], + 'piyg10': [(142, 1, 82), (39, 100, 25), (197, 27, 125), (222, 119, 174), (241, 182, 218), (253, 224, 239), (230, 245, 208), (184, 225, 134), (127, 188, 65), (77, 146, 33)], + 'piyg11': [(142, 1, 82), (77, 146, 33), (39, 100, 25), (197, 27, 125), (222, 119, 174), (241, 182, 218), (253, 224, 239), (247, 247, 247), (230, 245, 208), (184, 225, 134), (127, 188, 65)], + 'piyg3': [(233, 163, 201), (247, 247, 247), (161, 215, 106)], + 'piyg4': [(208, 28, 139), (241, 182, 218), (184, 225, 134), (77, 172, 38)], + 'piyg5': [(208, 28, 139), (241, 182, 218), (247, 247, 247), (184, 225, 134), (77, 172, 38)], + 'piyg6': [(197, 27, 125), (233, 163, 201), (253, 224, 239), (230, 245, 208), (161, 215, 106), (77, 146, 33)], + 'piyg7': [(197, 27, 125), (233, 163, 201), (253, 224, 239), (247, 247, 247), (230, 245, 208), (161, 215, 106), (77, 146, 33)], + 'piyg8': [(197, 27, 125), (222, 119, 174), (241, 182, 218), (253, 224, 239), (230, 245, 208), (184, 225, 134), (127, 188, 65), (77, 146, 33)], + 'piyg9': [(197, 27, 125), (222, 119, 174), (241, 182, 218), (253, 224, 239), (247, 247, 247), (230, 245, 208), (184, 225, 134), (127, 188, 65), (77, 146, 33)], + 'prgn10': [(64, 0, 75), (0, 68, 27), (118, 42, 131), (153, 112, 171), (194, 165, 207), (231, 212, 232), (217, 240, 211), (166, 219, 160), (90, 174, 97), (27, 120, 55)], + 'prgn11': [(64, 0, 75), (27, 120, 55), (0, 68, 27), (118, 42, 131), (153, 112, 171), (194, 165, 207), (231, 212, 232), (247, 247, 247), (217, 240, 211), (166, 219, 160), (90, 174, 97)], + 'prgn3': [(175, 141, 195), (247, 247, 247), (127, 191, 123)], + 'prgn4': [(123, 50, 148), (194, 165, 207), (166, 219, 160), (0, 136, 55)], + 'prgn5': [(123, 50, 148), (194, 165, 207), (247, 247, 247), (166, 219, 160), (0, 136, 55)], + 'prgn6': [(118, 42, 131), (175, 141, 195), (231, 212, 232), (217, 240, 211), (127, 191, 123), (27, 120, 55)], + 'prgn7': [(118, 42, 131), (175, 141, 195), (231, 212, 232), (247, 247, 247), (217, 240, 211), (127, 191, 123), (27, 120, 55)], + 'prgn8': [(118, 42, 131), (153, 112, 171), (194, 165, 207), (231, 212, 232), (217, 240, 211), (166, 219, 160), (90, 174, 97), (27, 120, 55)], + 'prgn9': [(118, 42, 131), (153, 112, 171), (194, 165, 207), (231, 212, 232), (247, 247, 247), (217, 240, 211), (166, 219, 160), (90, 174, 97), (27, 120, 55)], + 'pubu3': [(236, 231, 242), (166, 189, 219), (43, 140, 190)], + 'pubu4': [(241, 238, 246), (189, 201, 225), (116, 169, 207), (5, 112, 176)], + 'pubu5': [(241, 238, 246), (189, 201, 225), (116, 169, 207), (43, 140, 190), (4, 90, 141)], + 'pubu6': [(241, 238, 246), (208, 209, 230), (166, 189, 219), (116, 169, 207), (43, 140, 190), (4, 90, 141)], + 'pubu7': [(241, 238, 246), (208, 209, 230), (166, 189, 219), (116, 169, 207), (54, 144, 192), (5, 112, 176), (3, 78, 123)], + 'pubu8': [(255, 247, 251), (236, 231, 242), (208, 209, 230), (166, 189, 219), (116, 169, 207), (54, 144, 192), (5, 112, 176), (3, 78, 123)], + 'pubu9': [(255, 247, 251), (236, 231, 242), (208, 209, 230), (166, 189, 219), (116, 169, 207), (54, 144, 192), (5, 112, 176), (4, 90, 141), (2, 56, 88)], + 'pubugn3': [(236, 226, 240), (166, 189, 219), (28, 144, 153)], + 'pubugn4': [(246, 239, 247), (189, 201, 225), (103, 169, 207), (2, 129, 138)], + 'pubugn5': [(246, 239, 247), (189, 201, 225), (103, 169, 207), (28, 144, 153), (1, 108, 89)], + 'pubugn6': [(246, 239, 247), (208, 209, 230), (166, 189, 219), (103, 169, 207), (28, 144, 153), (1, 108, 89)], + 'pubugn7': [(246, 239, 247), (208, 209, 230), (166, 189, 219), (103, 169, 207), (54, 144, 192), (2, 129, 138), (1, 100, 80)], + 'pubugn8': [(255, 247, 251), (236, 226, 240), (208, 209, 230), (166, 189, 219), (103, 169, 207), (54, 144, 192), (2, 129, 138), (1, 100, 80)], + 'pubugn9': [(255, 247, 251), (236, 226, 240), (208, 209, 230), (166, 189, 219), (103, 169, 207), (54, 144, 192), (2, 129, 138), (1, 108, 89), (1, 70, 54)], + 'puor10': [(127, 59, 8), (45, 0, 75), (179, 88, 6), (224, 130, 20), (253, 184, 99), (254, 224, 182), (216, 218, 235), (178, 171, 210), (128, 115, 172), (84, 39, 136)], + 'puor11': [(127, 59, 8), (84, 39, 136), (45, 0, 75), (179, 88, 6), (224, 130, 20), (253, 184, 99), (254, 224, 182), (247, 247, 247), (216, 218, 235), (178, 171, 210), (128, 115, 172)], + 'puor3': [(241, 163, 64), (247, 247, 247), (153, 142, 195)], + 'puor4': [(230, 97, 1), (253, 184, 99), (178, 171, 210), (94, 60, 153)], + 'puor5': [(230, 97, 1), (253, 184, 99), (247, 247, 247), (178, 171, 210), (94, 60, 153)], + 'puor6': [(179, 88, 6), (241, 163, 64), (254, 224, 182), (216, 218, 235), (153, 142, 195), (84, 39, 136)], + 'puor7': [(179, 88, 6), (241, 163, 64), (254, 224, 182), (247, 247, 247), (216, 218, 235), (153, 142, 195), (84, 39, 136)], + 'puor8': [(179, 88, 6), (224, 130, 20), (253, 184, 99), (254, 224, 182), (216, 218, 235), (178, 171, 210), (128, 115, 172), (84, 39, 136)], + 'puor9': [(179, 88, 6), (224, 130, 20), (253, 184, 99), (254, 224, 182), (247, 247, 247), (216, 218, 235), (178, 171, 210), (128, 115, 172), (84, 39, 136)], + 'purd3': [(231, 225, 239), (201, 148, 199), (221, 28, 119)], + 'purd4': [(241, 238, 246), (215, 181, 216), (223, 101, 176), (206, 18, 86)], + 'purd5': [(241, 238, 246), (215, 181, 216), (223, 101, 176), (221, 28, 119), (152, 0, 67)], + 'purd6': [(241, 238, 246), (212, 185, 218), (201, 148, 199), (223, 101, 176), (221, 28, 119), (152, 0, 67)], + 'purd7': [(241, 238, 246), (212, 185, 218), (201, 148, 199), (223, 101, 176), (231, 41, 138), (206, 18, 86), (145, 0, 63)], + 'purd8': [(247, 244, 249), (231, 225, 239), (212, 185, 218), (201, 148, 199), (223, 101, 176), (231, 41, 138), (206, 18, 86), (145, 0, 63)], + 'purd9': [(247, 244, 249), (231, 225, 239), (212, 185, 218), (201, 148, 199), (223, 101, 176), (231, 41, 138), (206, 18, 86), (152, 0, 67), (103, 0, 31)], + 'purples3': [(239, 237, 245), (188, 189, 220), (117, 107, 177)], + 'purples4': [(242, 240, 247), (203, 201, 226), (158, 154, 200), (106, 81, 163)], + 'purples5': [(242, 240, 247), (203, 201, 226), (158, 154, 200), (117, 107, 177), (84, 39, 143)], + 'purples6': [(242, 240, 247), (218, 218, 235), (188, 189, 220), (158, 154, 200), (117, 107, 177), (84, 39, 143)], + 'purples7': [(242, 240, 247), (218, 218, 235), (188, 189, 220), (158, 154, 200), (128, 125, 186), (106, 81, 163), (74, 20, 134)], + 'purples8': [(252, 251, 253), (239, 237, 245), (218, 218, 235), (188, 189, 220), (158, 154, 200), (128, 125, 186), (106, 81, 163), (74, 20, 134)], + 'purples9': [(252, 251, 253), (239, 237, 245), (218, 218, 235), (188, 189, 220), (158, 154, 200), (128, 125, 186), (106, 81, 163), (84, 39, 143), (63, 0, 125)], + 'rdbu10': [(103, 0, 31), (5, 48, 97), (178, 24, 43), (214, 96, 77), (244, 165, 130), (253, 219, 199), (209, 229, 240), (146, 197, 222), (67, 147, 195), (33, 102, 172)], + 'rdbu11': [(103, 0, 31), (33, 102, 172), (5, 48, 97), (178, 24, 43), (214, 96, 77), (244, 165, 130), (253, 219, 199), (247, 247, 247), (209, 229, 240), (146, 197, 222), (67, 147, 195)], + 'rdbu3': [(239, 138, 98), (247, 247, 247), (103, 169, 207)], + 'rdbu4': [(202, 0, 32), (244, 165, 130), (146, 197, 222), (5, 113, 176)], + 'rdbu5': [(202, 0, 32), (244, 165, 130), (247, 247, 247), (146, 197, 222), (5, 113, 176)], + 'rdbu6': [(178, 24, 43), (239, 138, 98), (253, 219, 199), (209, 229, 240), (103, 169, 207), (33, 102, 172)], + 'rdbu7': [(178, 24, 43), (239, 138, 98), (253, 219, 199), (247, 247, 247), (209, 229, 240), (103, 169, 207), (33, 102, 172)], + 'rdbu8': [(178, 24, 43), (214, 96, 77), (244, 165, 130), (253, 219, 199), (209, 229, 240), (146, 197, 222), (67, 147, 195), (33, 102, 172)], + 'rdbu9': [(178, 24, 43), (214, 96, 77), (244, 165, 130), (253, 219, 199), (247, 247, 247), (209, 229, 240), (146, 197, 222), (67, 147, 195), (33, 102, 172)], + 'rdgy10': [(103, 0, 31), (26, 26, 26), (178, 24, 43), (214, 96, 77), (244, 165, 130), (253, 219, 199), (224, 224, 224), (186, 186, 186), (135, 135, 135), (77, 77, 77)], + 'rdgy11': [(103, 0, 31), (77, 77, 77), (26, 26, 26), (178, 24, 43), (214, 96, 77), (244, 165, 130), (253, 219, 199), (255, 255, 255), (224, 224, 224), (186, 186, 186), (135, 135, 135)], + 'rdgy3': [(239, 138, 98), (255, 255, 255), (153, 153, 153)], + 'rdgy4': [(202, 0, 32), (244, 165, 130), (186, 186, 186), (64, 64, 64)], + 'rdgy5': [(202, 0, 32), (244, 165, 130), (255, 255, 255), (186, 186, 186), (64, 64, 64)], + 'rdgy6': [(178, 24, 43), (239, 138, 98), (253, 219, 199), (224, 224, 224), (153, 153, 153), (77, 77, 77)], + 'rdgy7': [(178, 24, 43), (239, 138, 98), (253, 219, 199), (255, 255, 255), (224, 224, 224), (153, 153, 153), (77, 77, 77)], + 'rdgy8': [(178, 24, 43), (214, 96, 77), (244, 165, 130), (253, 219, 199), (224, 224, 224), (186, 186, 186), (135, 135, 135), (77, 77, 77)], + 'rdgy9': [(178, 24, 43), (214, 96, 77), (244, 165, 130), (253, 219, 199), (255, 255, 255), (224, 224, 224), (186, 186, 186), (135, 135, 135), (77, 77, 77)], + 'rdpu3': [(253, 224, 221), (250, 159, 181), (197, 27, 138)], + 'rdpu4': [(254, 235, 226), (251, 180, 185), (247, 104, 161), (174, 1, 126)], + 'rdpu5': [(254, 235, 226), (251, 180, 185), (247, 104, 161), (197, 27, 138), (122, 1, 119)], + 'rdpu6': [(254, 235, 226), (252, 197, 192), (250, 159, 181), (247, 104, 161), (197, 27, 138), (122, 1, 119)], + 'rdpu7': [(254, 235, 226), (252, 197, 192), (250, 159, 181), (247, 104, 161), (221, 52, 151), (174, 1, 126), (122, 1, 119)], + 'rdpu8': [(255, 247, 243), (253, 224, 221), (252, 197, 192), (250, 159, 181), (247, 104, 161), (221, 52, 151), (174, 1, 126), (122, 1, 119)], + 'rdpu9': [(255, 247, 243), (253, 224, 221), (252, 197, 192), (250, 159, 181), (247, 104, 161), (221, 52, 151), (174, 1, 126), (122, 1, 119), (73, 0, 106)], + 'rdylbu10': [(165, 0, 38), (49, 54, 149), (215, 48, 39), (244, 109, 67), (253, 174, 97), (254, 224, 144), (224, 243, 248), (171, 217, 233), (116, 173, 209), (69, 117, 180)], + 'rdylbu11': [(165, 0, 38), (69, 117, 180), (49, 54, 149), (215, 48, 39), (244, 109, 67), (253, 174, 97), (254, 224, 144), (255, 255, 191), (224, 243, 248), (171, 217, 233), (116, 173, 209)], + 'rdylbu3': [(252, 141, 89), (255, 255, 191), (145, 191, 219)], + 'rdylbu4': [(215, 25, 28), (253, 174, 97), (171, 217, 233), (44, 123, 182)], + 'rdylbu5': [(215, 25, 28), (253, 174, 97), (255, 255, 191), (171, 217, 233), (44, 123, 182)], + 'rdylbu6': [(215, 48, 39), (252, 141, 89), (254, 224, 144), (224, 243, 248), (145, 191, 219), (69, 117, 180)], + 'rdylbu7': [(215, 48, 39), (252, 141, 89), (254, 224, 144), (255, 255, 191), (224, 243, 248), (145, 191, 219), (69, 117, 180)], + 'rdylbu8': [(215, 48, 39), (244, 109, 67), (253, 174, 97), (254, 224, 144), (224, 243, 248), (171, 217, 233), (116, 173, 209), (69, 117, 180)], + 'rdylbu9': [(215, 48, 39), (244, 109, 67), (253, 174, 97), (254, 224, 144), (255, 255, 191), (224, 243, 248), (171, 217, 233), (116, 173, 209), (69, 117, 180)], + 'rdylgn10': [(165, 0, 38), (0, 104, 55), (215, 48, 39), (244, 109, 67), (253, 174, 97), (254, 224, 139), (217, 239, 139), (166, 217, 106), (102, 189, 99), (26, 152, 80)], + 'rdylgn11': [(165, 0, 38), (26, 152, 80), (0, 104, 55), (215, 48, 39), (244, 109, 67), (253, 174, 97), (254, 224, 139), (255, 255, 191), (217, 239, 139), (166, 217, 106), (102, 189, 99)], + 'rdylgn3': [(252, 141, 89), (255, 255, 191), (145, 207, 96)], + 'rdylgn4': [(215, 25, 28), (253, 174, 97), (166, 217, 106), (26, 150, 65)], + 'rdylgn5': [(215, 25, 28), (253, 174, 97), (255, 255, 191), (166, 217, 106), (26, 150, 65)], + 'rdylgn6': [(215, 48, 39), (252, 141, 89), (254, 224, 139), (217, 239, 139), (145, 207, 96), (26, 152, 80)], + 'rdylgn7': [(215, 48, 39), (252, 141, 89), (254, 224, 139), (255, 255, 191), (217, 239, 139), (145, 207, 96), (26, 152, 80)], + 'rdylgn8': [(215, 48, 39), (244, 109, 67), (253, 174, 97), (254, 224, 139), (217, 239, 139), (166, 217, 106), (102, 189, 99), (26, 152, 80)], + 'rdylgn9': [(215, 48, 39), (244, 109, 67), (253, 174, 97), (254, 224, 139), (255, 255, 191), (217, 239, 139), (166, 217, 106), (102, 189, 99), (26, 152, 80)], + 'reds3': [(254, 224, 210), (252, 146, 114), (222, 45, 38)], + 'reds4': [(254, 229, 217), (252, 174, 145), (251, 106, 74), (203, 24, 29)], + 'reds5': [(254, 229, 217), (252, 174, 145), (251, 106, 74), (222, 45, 38), (165, 15, 21)], + 'reds6': [(254, 229, 217), (252, 187, 161), (252, 146, 114), (251, 106, 74), (222, 45, 38), (165, 15, 21)], + 'reds7': [(254, 229, 217), (252, 187, 161), (252, 146, 114), (251, 106, 74), (239, 59, 44), (203, 24, 29), (153, 0, 13)], + 'reds8': [(255, 245, 240), (254, 224, 210), (252, 187, 161), (252, 146, 114), (251, 106, 74), (239, 59, 44), (203, 24, 29), (153, 0, 13)], + 'reds9': [(255, 245, 240), (254, 224, 210), (252, 187, 161), (252, 146, 114), (251, 106, 74), (239, 59, 44), (203, 24, 29), (165, 15, 21), (103, 0, 13)], + 'set13': [(228, 26, 28), (55, 126, 184), (77, 175, 74)], + 'set14': [(228, 26, 28), (55, 126, 184), (77, 175, 74), (152, 78, 163)], + 'set15': [(228, 26, 28), (55, 126, 184), (77, 175, 74), (152, 78, 163), (255, 127, 0)], + 'set16': [(228, 26, 28), (55, 126, 184), (77, 175, 74), (152, 78, 163), (255, 127, 0), (255, 255, 51)], + 'set17': [(228, 26, 28), (55, 126, 184), (77, 175, 74), (152, 78, 163), (255, 127, 0), (255, 255, 51), (166, 86, 40)], + 'set18': [(228, 26, 28), (55, 126, 184), (77, 175, 74), (152, 78, 163), (255, 127, 0), (255, 255, 51), (166, 86, 40), (247, 129, 191)], + 'set19': [(228, 26, 28), (55, 126, 184), (77, 175, 74), (152, 78, 163), (255, 127, 0), (255, 255, 51), (166, 86, 40), (247, 129, 191), (153, 153, 153)], + 'set23': [(102, 194, 165), (252, 141, 98), (141, 160, 203)], + 'set24': [(102, 194, 165), (252, 141, 98), (141, 160, 203), (231, 138, 195)], + 'set25': [(102, 194, 165), (252, 141, 98), (141, 160, 203), (231, 138, 195), (166, 216, 84)], + 'set26': [(102, 194, 165), (252, 141, 98), (141, 160, 203), (231, 138, 195), (166, 216, 84), (255, 217, 47)], + 'set27': [(102, 194, 165), (252, 141, 98), (141, 160, 203), (231, 138, 195), (166, 216, 84), (255, 217, 47), (229, 196, 148)], + 'set28': [(102, 194, 165), (252, 141, 98), (141, 160, 203), (231, 138, 195), (166, 216, 84), (255, 217, 47), (229, 196, 148), (179, 179, 179)], + 'set310': [(141, 211, 199), (188, 128, 189), (255, 255, 179), (190, 186, 218), (251, 128, 114), (128, 177, 211), (253, 180, 98), (179, 222, 105), (252, 205, 229), (217, 217, 217)], + 'set311': [(141, 211, 199), (188, 128, 189), (204, 235, 197), (255, 255, 179), (190, 186, 218), (251, 128, 114), (128, 177, 211), (253, 180, 98), (179, 222, 105), (252, 205, 229), (217, 217, 217)], + 'set312': [(141, 211, 199), (188, 128, 189), (204, 235, 197), (255, 237, 111), (255, 255, 179), (190, 186, 218), (251, 128, 114), (128, 177, 211), (253, 180, 98), (179, 222, 105), (252, 205, 229), (217, 217, 217)], + 'set33': [(141, 211, 199), (255, 255, 179), (190, 186, 218)], + 'set34': [(141, 211, 199), (255, 255, 179), (190, 186, 218), (251, 128, 114)], + 'set35': [(141, 211, 199), (255, 255, 179), (190, 186, 218), (251, 128, 114), (128, 177, 211)], + 'set36': [(141, 211, 199), (255, 255, 179), (190, 186, 218), (251, 128, 114), (128, 177, 211), (253, 180, 98)], + 'set37': [(141, 211, 199), (255, 255, 179), (190, 186, 218), (251, 128, 114), (128, 177, 211), (253, 180, 98), (179, 222, 105)], + 'set38': [(141, 211, 199), (255, 255, 179), (190, 186, 218), (251, 128, 114), (128, 177, 211), (253, 180, 98), (179, 222, 105), (252, 205, 229)], + 'set39': [(141, 211, 199), (255, 255, 179), (190, 186, 218), (251, 128, 114), (128, 177, 211), (253, 180, 98), (179, 222, 105), (252, 205, 229), (217, 217, 217)], + 'spectral10': [(158, 1, 66), (94, 79, 162), (213, 62, 79), (244, 109, 67), (253, 174, 97), (254, 224, 139), (230, 245, 152), (171, 221, 164), (102, 194, 165), (50, 136, 189)], + 'spectral11': [(158, 1, 66), (50, 136, 189), (94, 79, 162), (213, 62, 79), (244, 109, 67), (253, 174, 97), (254, 224, 139), (255, 255, 191), (230, 245, 152), (171, 221, 164), (102, 194, 165)], + 'spectral3': [(252, 141, 89), (255, 255, 191), (153, 213, 148)], + 'spectral4': [(215, 25, 28), (253, 174, 97), (171, 221, 164), (43, 131, 186)], + 'spectral5': [(215, 25, 28), (253, 174, 97), (255, 255, 191), (171, 221, 164), (43, 131, 186)], + 'spectral6': [(213, 62, 79), (252, 141, 89), (254, 224, 139), (230, 245, 152), (153, 213, 148), (50, 136, 189)], + 'spectral7': [(213, 62, 79), (252, 141, 89), (254, 224, 139), (255, 255, 191), (230, 245, 152), (153, 213, 148), (50, 136, 189)], + 'spectral8': [(213, 62, 79), (244, 109, 67), (253, 174, 97), (254, 224, 139), (230, 245, 152), (171, 221, 164), (102, 194, 165), (50, 136, 189)], + 'spectral9': [(213, 62, 79), (244, 109, 67), (253, 174, 97), (254, 224, 139), (255, 255, 191), (230, 245, 152), (171, 221, 164), (102, 194, 165), (50, 136, 189)], + 'ylgn3': [(247, 252, 185), (173, 221, 142), (49, 163, 84)], + 'ylgn4': [(255, 255, 204), (194, 230, 153), (120, 198, 121), (35, 132, 67)], + 'ylgn5': [(255, 255, 204), (194, 230, 153), (120, 198, 121), (49, 163, 84), (0, 104, 55)], + 'ylgn6': [(255, 255, 204), (217, 240, 163), (173, 221, 142), (120, 198, 121), (49, 163, 84), (0, 104, 55)], + 'ylgn7': [(255, 255, 204), (217, 240, 163), (173, 221, 142), (120, 198, 121), (65, 171, 93), (35, 132, 67), (0, 90, 50)], + 'ylgn8': [(255, 255, 229), (247, 252, 185), (217, 240, 163), (173, 221, 142), (120, 198, 121), (65, 171, 93), (35, 132, 67), (0, 90, 50)], + 'ylgn9': [(255, 255, 229), (247, 252, 185), (217, 240, 163), (173, 221, 142), (120, 198, 121), (65, 171, 93), (35, 132, 67), (0, 104, 55), (0, 69, 41)], + 'ylgnbu3': [(237, 248, 177), (127, 205, 187), (44, 127, 184)], + 'ylgnbu4': [(255, 255, 204), (161, 218, 180), (65, 182, 196), (34, 94, 168)], + 'ylgnbu5': [(255, 255, 204), (161, 218, 180), (65, 182, 196), (44, 127, 184), (37, 52, 148)], + 'ylgnbu6': [(255, 255, 204), (199, 233, 180), (127, 205, 187), (65, 182, 196), (44, 127, 184), (37, 52, 148)], + 'ylgnbu7': [(255, 255, 204), (199, 233, 180), (127, 205, 187), (65, 182, 196), (29, 145, 192), (34, 94, 168), (12, 44, 132)], + 'ylgnbu8': [(255, 255, 217), (237, 248, 177), (199, 233, 180), (127, 205, 187), (65, 182, 196), (29, 145, 192), (34, 94, 168), (12, 44, 132)], + 'ylgnbu9': [(255, 255, 217), (237, 248, 177), (199, 233, 180), (127, 205, 187), (65, 182, 196), (29, 145, 192), (34, 94, 168), (37, 52, 148), (8, 29, 88)], + 'ylorbr3': [(255, 247, 188), (254, 196, 79), (217, 95, 14)], + 'ylorbr4': [(255, 255, 212), (254, 217, 142), (254, 153, 41), (204, 76, 2)], + 'ylorbr5': [(255, 255, 212), (254, 217, 142), (254, 153, 41), (217, 95, 14), (153, 52, 4)], + 'ylorbr6': [(255, 255, 212), (254, 227, 145), (254, 196, 79), (254, 153, 41), (217, 95, 14), (153, 52, 4)], + 'ylorbr7': [(255, 255, 212), (254, 227, 145), (254, 196, 79), (254, 153, 41), (236, 112, 20), (204, 76, 2), (140, 45, 4)], + 'ylorbr8': [(255, 255, 229), (255, 247, 188), (254, 227, 145), (254, 196, 79), (254, 153, 41), (236, 112, 20), (204, 76, 2), (140, 45, 4)], + 'ylorbr9': [(255, 255, 229), (255, 247, 188), (254, 227, 145), (254, 196, 79), (254, 153, 41), (236, 112, 20), (204, 76, 2), (153, 52, 4), (102, 37, 6)], + 'ylorrd3': [(255, 237, 160), (254, 178, 76), (240, 59, 32)], + 'ylorrd4': [(255, 255, 178), (254, 204, 92), (253, 141, 60), (227, 26, 28)], + 'ylorrd5': [(255, 255, 178), (254, 204, 92), (253, 141, 60), (240, 59, 32), (189, 0, 38)], + 'ylorrd6': [(255, 255, 178), (254, 217, 118), (254, 178, 76), (253, 141, 60), (240, 59, 32), (189, 0, 38)], + 'ylorrd7': [(255, 255, 178), (254, 217, 118), (254, 178, 76), (253, 141, 60), (252, 78, 42), (227, 26, 28), (177, 0, 38)], + 'ylorrd8': [(255, 255, 204), (255, 237, 160), (254, 217, 118), (254, 178, 76), (253, 141, 60), (252, 78, 42), (227, 26, 28), (177, 0, 38)], +} + + +if __name__ == '__main__': + main() diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree_command/CHANGELOG.rst b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree_command/CHANGELOG.rst new file mode 100644 index 00000000..0a789b78 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree_command/CHANGELOG.rst @@ -0,0 +1,149 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package rqt_py_console +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.0.2 (2021-08-31) +------------------ +* Fix modern setuptools warning about dashes instead of underscores (`#11 `_) +* Contributors: Chris Lalancette + +1.0.1 (2021-04-27) +------------------ +* Changed the build type to ament_python and fixed package to run with ros2 run (`#8 `_) +* Contributors: Alejandro Hernández Cordero + +1.0.0 (2018-12-11) +------------------ +* spyderlib -> spyder (`#5 `_) +* ros2 port (`#3 `_) +* autopep8 (`#2 `_) +* Contributors: Mike Lautman + +0.4.8 (2017-04-28) +------------------ + +0.4.7 (2017-03-02) +------------------ + +0.4.6 (2017-02-27) +------------------ + +0.4.5 (2017-02-03) +------------------ + +0.4.4 (2017-01-24) +------------------ +* use Python 3 compatible syntax (`#421 `_) + +0.4.3 (2016-11-02) +------------------ + +0.4.2 (2016-09-19) +------------------ + +0.4.1 (2016-05-16) +------------------ + +0.4.0 (2016-04-27) +------------------ +* Support Qt 5 (in Kinetic and higher) as well as Qt 4 (in Jade and earlier) (`#359 `_) + +0.3.13 (2016-03-08) +------------------- + +0.3.12 (2015-07-24) +------------------- + +0.3.11 (2015-04-30) +------------------- + +0.3.10 (2014-10-01) +------------------- +* update plugin scripts to use full name to avoid future naming collisions + +0.3.9 (2014-08-18) +------------------ + +0.3.8 (2014-07-15) +------------------ + +0.3.7 (2014-07-11) +------------------ +* export architecture_independent flag in package.xml (`#254 `_) + +0.3.6 (2014-06-02) +------------------ + +0.3.5 (2014-05-07) +------------------ + +0.3.4 (2014-01-28) +------------------ + +0.3.3 (2014-01-08) +------------------ +* add groups for rqt plugins, renamed some plugins (`#167 `_) + +0.3.2 (2013-10-14) +------------------ + +0.3.1 (2013-10-09) +------------------ + +0.3.0 (2013-08-28) +------------------ + +0.2.17 (2013-07-04) +------------------- + +0.2.16 (2013-04-09 13:33) +------------------------- + +0.2.15 (2013-04-09 00:02) +------------------------- + +0.2.14 (2013-03-14) +------------------- + +0.2.13 (2013-03-11 22:14) +------------------------- + +0.2.12 (2013-03-11 13:56) +------------------------- + +0.2.11 (2013-03-08) +------------------- + +0.2.10 (2013-01-22) +------------------- + +0.2.9 (2013-01-17) +------------------ + +0.2.8 (2013-01-11) +------------------ + +0.2.7 (2012-12-24) +------------------ + +0.2.6 (2012-12-23) +------------------ + +0.2.5 (2012-12-21 19:11) +------------------------ + +0.2.4 (2012-12-21 01:13) +------------------------ + +0.2.3 (2012-12-21 00:24) +------------------------ + +0.2.2 (2012-12-20 18:29) +------------------------ + +0.2.1 (2012-12-20 17:47) +------------------------ + +0.2.0 (2012-12-20 17:39) +------------------------ +* first release of this package into groovy diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree_command/config/gui_config.yaml b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree_command/config/gui_config.yaml new file mode 100644 index 00000000..456dfaa5 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree_command/config/gui_config.yaml @@ -0,0 +1,16 @@ +groups: + - Commands: + - Arm and Takeoff: + condition_name: Auto Takeoff Commanded + - Fixed Trajectory: + condition_name: Fixed Trajectory Commanded + - Global Plan: + condition_name: Global Plan Commanded + - Pause: + condition_name: Pause Commanded + - Rewind: + condition_name: Rewind Commanded + - Disarm: + condition_name: Disarm Commanded + - Land: + condition_name: Land Commanded \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree_command/package.xml b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree_command/package.xml new file mode 100644 index 00000000..9b975765 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree_command/package.xml @@ -0,0 +1,28 @@ + + rqt_behavior_tree_command + 1.0.2 + rqt_behavior_tree_command is a Python GUI template. + John Keller + + BSD + + + + + + John Keller + + ament_index_python + python_qt_binding + qt_gui + qt_gui_py_common + rclpy + rqt_gui + rqt_gui_py + + + + + ament_python + + diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree_command/plugin.xml b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree_command/plugin.xml new file mode 100644 index 00000000..205c204e --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree_command/plugin.xml @@ -0,0 +1,17 @@ + + + + A Python GUI plugin providing an interactive Python console. + + + + + folder + Plugins related to miscellaneous tools. + + + applications-python + A Python RQT GUI template. + + + diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree_command/resource/py_console_widget.ui b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree_command/resource/py_console_widget.ui new file mode 100644 index 00000000..12810f1c --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree_command/resource/py_console_widget.ui @@ -0,0 +1,53 @@ + + + PyConsole + + + + 0 + 0 + 276 + 212 + + + + PyConsole + + + + 0 + + + 0 + + + 0 + + + 3 + + + 0 + + + + + 0 + + + + + + + + + + + PyConsoleTextEdit + QTextEdit +
rqt_py_console.py_console_text_edit
+ + + + + diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree_command/resource/rqt_behavior_tree_command b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree_command/resource/rqt_behavior_tree_command new file mode 100644 index 00000000..e69de29b diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree_command/setup.cfg b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree_command/setup.cfg new file mode 100644 index 00000000..08ee1a53 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree_command/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/rqt_behavior_tree_command +[install] +install_scripts=$base/lib/rqt_behavior_tree_command diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree_command/setup.py b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree_command/setup.py new file mode 100644 index 00000000..1c95a9b5 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree_command/setup.py @@ -0,0 +1,41 @@ +from setuptools import setup +from glob import glob + +package_name = 'rqt_behavior_tree_command' + +setup( + name=package_name, + version='1.0.2', + packages=[package_name], + package_dir={'': 'src'}, + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name + '/resource', + ['resource/py_console_widget.ui']), + ('share/' + package_name, ['package.xml']), + ('share/' + package_name, ['plugin.xml']), + ('share/' + package_name + '/config/', glob('config/*')), + ], + install_requires=['setuptools'], + zip_safe=True, + author='', + maintainer='', + maintainer_email='', + keywords=['ROS'], + classifiers=[ + '', + '', + '', + '', + ], + description=( + 'rqt_behavior_tree_command' + ), + license='BSD', + entry_points={ + 'console_scripts': [ + 'rqt_behavior_tree_command = ' + package_name + '.main:main', + ], + }, +) diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree_command/src/rqt_behavior_tree_command/__init__.py b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree_command/src/rqt_behavior_tree_command/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree_command/src/rqt_behavior_tree_command/main.py b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree_command/src/rqt_behavior_tree_command/main.py new file mode 100644 index 00000000..9a5c9376 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree_command/src/rqt_behavior_tree_command/main.py @@ -0,0 +1,12 @@ +import sys + +from rqt_gui.main import Main + + +def main(): + main = Main() + sys.exit(main.main(sys.argv, standalone='rqt_py_console.py_console.PyConsole')) + + +if __name__ == '__main__': + main() diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree_command/src/rqt_behavior_tree_command/py_console_text_edit.py b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree_command/src/rqt_behavior_tree_command/py_console_text_edit.py new file mode 100644 index 00000000..dc9ce1a0 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree_command/src/rqt_behavior_tree_command/py_console_text_edit.py @@ -0,0 +1,69 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2012, Dorian Scholz +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import sys +from code import InteractiveInterpreter + +from python_qt_binding import QT_BINDING, QT_BINDING_VERSION +from python_qt_binding.QtCore import Qt, Signal + +from qt_gui_py_common.console_text_edit import ConsoleTextEdit + + +class PyConsoleTextEdit(ConsoleTextEdit): + _color_stdin = Qt.darkGreen + _multi_line_char = ':' + _multi_line_indent = ' ' + _prompt = ('>>> ', '... ') # prompt for single and multi line + exit = Signal() + + def __init__(self, parent=None): + super(PyConsoleTextEdit, self).__init__(parent) + + self._interpreter_locals = {} + self._interpreter = InteractiveInterpreter(self._interpreter_locals) + + self._comment_writer.write('Python %s on %s\n' % + (sys.version.replace('\n', ''), sys.platform)) + self._comment_writer.write( + 'Qt bindings: %s version %s\n' % (QT_BINDING, QT_BINDING_VERSION)) + + self._add_prompt() + + def update_interpreter_locals(self, newLocals): + self._interpreter_locals.update(newLocals) + + def _exec_code(self, code): + try: + self._interpreter.runsource(code) + except SystemExit: # catch sys.exit() calls, so they don't close the whole gui + self.exit.emit() diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree_command/src/rqt_behavior_tree_command/py_console_widget.py b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree_command/src/rqt_behavior_tree_command/py_console_widget.py new file mode 100644 index 00000000..e69bde34 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree_command/src/rqt_behavior_tree_command/py_console_widget.py @@ -0,0 +1,59 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2012, Dorian Scholz +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import os +from ament_index_python.resources import get_resource + +from python_qt_binding import loadUi +from python_qt_binding.QtWidgets import QWidget +from rqt_py_console.py_console_text_edit import PyConsoleTextEdit + + +class PyConsoleWidget(QWidget): + + def __init__(self, context=None): + super(PyConsoleWidget, self).__init__() + + _, package_path = get_resource('packages', 'rqt_py_console') + ui_file = os.path.join( + package_path, 'share', 'rqt_py_console', 'resource', 'py_console_widget.ui') + + loadUi(ui_file, self, {'PyConsoleTextEdit': PyConsoleTextEdit}) + self.setObjectName('PyConsoleWidget') + + my_locals = { + 'context': context + } + self.py_console.update_interpreter_locals(my_locals) + self.py_console.print_message( + 'The variable "context" is set to the PluginContext of this plugin.') + self.py_console.exit.connect(context.close_plugin) diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree_command/src/rqt_behavior_tree_command/spyder_console_widget.py b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree_command/src/rqt_behavior_tree_command/spyder_console_widget.py new file mode 100644 index 00000000..374ef7a5 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree_command/src/rqt_behavior_tree_command/spyder_console_widget.py @@ -0,0 +1,60 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2012, Dorian Scholz +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +from python_qt_binding.QtGui import QFont + +from spyder.widgets.internalshell import InternalShell +from spyder.utils.module_completion import moduleCompletion + +class SpyderConsoleWidget(InternalShell): + + def __init__(self, context=None): + my_locals = { + 'context': context + } + super(SpyderConsoleWidget, self).__init__(namespace=my_locals) + self.setObjectName('SpyderConsoleWidget') + self.set_pythonshell_font(QFont('Mono')) + self.interpreter.restore_stds() + + def get_module_completion(self, objtxt): + """Return module completion list associated to object name""" + return moduleCompletion(objtxt) + + def run_command(self, *args): + self.interpreter.redirect_stds() + super(SpyderConsoleWidget, self).run_command(*args) + self.flush() + self.interpreter.restore_stds() + + def shutdown(self): + self.exit_interpreter() diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree_command/src/rqt_behavior_tree_command/template.py b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree_command/src/rqt_behavior_tree_command/template.py new file mode 100644 index 00000000..9e17ecf1 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_behavior_tree_command/src/rqt_behavior_tree_command/template.py @@ -0,0 +1,182 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2012, Dorian Scholz +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +from python_qt_binding.QtWidgets import QVBoxLayout, QWidget +from rqt_gui_py.plugin import Plugin +from qt_gui_py_common.simple_settings_dialog import SimpleSettingsDialog +from rqt_py_console.py_console_widget import PyConsoleWidget + +import python_qt_binding.QtWidgets as qt +import python_qt_binding.QtWidgets as QtWidgets +import python_qt_binding.QtGui as QtGui +import python_qt_binding.QtCore as QtCore + +from ament_index_python.packages import get_package_share_directory +import yaml +import os + +from behavior_tree_msgs.msg import Status, BehaviorTreeCommand, BehaviorTreeCommands + +class BehaviorTreeCommandPlugin(Plugin): + """ + Plugin providing an interactive Python console + """ + + def __init__(self, context): + super(BehaviorTreeCommandPlugin, self).__init__(context) + self.setObjectName('BehaviorTreeCommandPlugin') + + self.config_filename = '' + self.button_groups = {} + + self.context = context + self.command_pub = self.context.node.create_publisher(BehaviorTreeCommands, 'behavior_tree_commands', 10) + + # main layout + self.widget = QWidget() + self.vbox = qt.QVBoxLayout() + self.widget.setLayout(self.vbox) + context.add_widget(self.widget) + + # config widget + self.config_widget = qt.QWidget() + self.config_widget.setStyleSheet('QWidget{margin-left:-1px;}') + self.config_layout = qt.QHBoxLayout() + self.config_widget.setLayout(self.config_layout) + self.config_widget.setFixedHeight(50) + + self.config_button = qt.QPushButton('Open Config...') + self.config_button.clicked.connect(self.select_config_file) + self.config_layout.addWidget(self.config_button) + + self.config_label = qt.QLabel('config filename: ') + self.config_layout.addWidget(self.config_label) + self.vbox.addWidget(self.config_widget) + + # button widget + self.button_widget = qt.QWidget() + self.button_layout = qt.QVBoxLayout() + self.button_widget.setLayout(self.button_layout) + self.vbox.addWidget(self.button_widget) + + def select_config_file(self): + starting_path = get_package_share_directory('rqt_behavior_tree_command') + '/config/' + filename = qt.QFileDialog.getOpenFileName(self.widget, 'Open Config File', starting_path, "Config Files (*.yaml)")[0] + print(filename) + self.set_config(filename) + + def set_config(self, filename): + if filename != '': + self.config_filename = filename + if self.config_filename != None: + self.config_label.setText('config filename: ' + os.path.basename(self.config_filename)) + self.init_buttons(filename) + + def init_buttons(self, filename): + y = yaml.load(open(filename, 'r').read(), Loader=yaml.Loader) + print(y) + + def get_click_function(group, button): + def click_function(): + commands = BehaviorTreeCommands() + for i in range(len(self.button_groups[group]['buttons'])): + b = self.button_groups[group]['buttons'][i] + command = BehaviorTreeCommand() + command.condition_name = self.button_groups[group]['condition_names'][i] + + if b != button and b.isChecked(): + b.toggle() + command.status = Status.FAILURE + elif b == button and not b.isChecked(): + command.status = Status.FAILURE + elif b == button and b.isChecked(): + command.status = Status.SUCCESS + commands.commands.append(command) + self.command_pub.publish(commands) + return click_function + + for group in y['groups']: + group_name = list(group.keys())[0] + if group_name not in self.button_groups.keys(): + self.button_groups[group_name] = {'buttons' : [], 'condition_names': []} + + group_widget = qt.QWidget() + group_layout = qt.QVBoxLayout() + group_widget.setLayout(group_layout) + self.button_layout.addWidget(group_widget) + + group_layout.addWidget(qt.QLabel(group_name)) + + button_widget = qt.QWidget() + button_layout = qt.QHBoxLayout() + button_widget.setLayout(button_layout) + group_layout.addWidget(button_widget) + + for buttons in group[group_name]: + button_name = list(buttons.keys())[0] + condition_name = buttons[button_name]['condition_name'] + + button = qt.QPushButton(button_name) + button.clicked.connect(get_click_function(group_name, button)) + button.setCheckable(True) + button_layout.addWidget(button) + + #print(condition_name, bt.get_condition_topic_name(condition_name)) + self.button_groups[group_name]['buttons'].append(button) + self.button_groups[group_name]['condition_names'].append(condition_name) + + def save_settings(self, plugin_settings, instance_settings): + instance_settings.set_value('config_filename', self.config_filename) + + def restore_settings(self, plugin_settings, instance_settings): + self.set_config(instance_settings.value('config_filename')) + + def trigger_configuration(self): + options = [ + {'title': 'Option 1', + 'description': 'Description of option 1.', + 'enabled': True}, + {'title': 'Option 2', + 'description': 'Description of option 2.'}, + ] + dialog = SimpleSettingsDialog(title='Options') + dialog.add_exclusive_option_group(title='List of options:', options=options, selected_index=0) + selected_index = dialog.get_settings()[0] + if selected_index != None: + selected_index = selected_index['selected_index'] + print('selected_index ', selected_index) + + def shutdown_console_widget(self): + pass + + def shutdown_plugin(self): + self.shutdown_console_widget() diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/CHANGELOG.rst b/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/CHANGELOG.rst new file mode 100644 index 00000000..0a789b78 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/CHANGELOG.rst @@ -0,0 +1,149 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package rqt_py_console +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.0.2 (2021-08-31) +------------------ +* Fix modern setuptools warning about dashes instead of underscores (`#11 `_) +* Contributors: Chris Lalancette + +1.0.1 (2021-04-27) +------------------ +* Changed the build type to ament_python and fixed package to run with ros2 run (`#8 `_) +* Contributors: Alejandro Hernández Cordero + +1.0.0 (2018-12-11) +------------------ +* spyderlib -> spyder (`#5 `_) +* ros2 port (`#3 `_) +* autopep8 (`#2 `_) +* Contributors: Mike Lautman + +0.4.8 (2017-04-28) +------------------ + +0.4.7 (2017-03-02) +------------------ + +0.4.6 (2017-02-27) +------------------ + +0.4.5 (2017-02-03) +------------------ + +0.4.4 (2017-01-24) +------------------ +* use Python 3 compatible syntax (`#421 `_) + +0.4.3 (2016-11-02) +------------------ + +0.4.2 (2016-09-19) +------------------ + +0.4.1 (2016-05-16) +------------------ + +0.4.0 (2016-04-27) +------------------ +* Support Qt 5 (in Kinetic and higher) as well as Qt 4 (in Jade and earlier) (`#359 `_) + +0.3.13 (2016-03-08) +------------------- + +0.3.12 (2015-07-24) +------------------- + +0.3.11 (2015-04-30) +------------------- + +0.3.10 (2014-10-01) +------------------- +* update plugin scripts to use full name to avoid future naming collisions + +0.3.9 (2014-08-18) +------------------ + +0.3.8 (2014-07-15) +------------------ + +0.3.7 (2014-07-11) +------------------ +* export architecture_independent flag in package.xml (`#254 `_) + +0.3.6 (2014-06-02) +------------------ + +0.3.5 (2014-05-07) +------------------ + +0.3.4 (2014-01-28) +------------------ + +0.3.3 (2014-01-08) +------------------ +* add groups for rqt plugins, renamed some plugins (`#167 `_) + +0.3.2 (2013-10-14) +------------------ + +0.3.1 (2013-10-09) +------------------ + +0.3.0 (2013-08-28) +------------------ + +0.2.17 (2013-07-04) +------------------- + +0.2.16 (2013-04-09 13:33) +------------------------- + +0.2.15 (2013-04-09 00:02) +------------------------- + +0.2.14 (2013-03-14) +------------------- + +0.2.13 (2013-03-11 22:14) +------------------------- + +0.2.12 (2013-03-11 13:56) +------------------------- + +0.2.11 (2013-03-08) +------------------- + +0.2.10 (2013-01-22) +------------------- + +0.2.9 (2013-01-17) +------------------ + +0.2.8 (2013-01-11) +------------------ + +0.2.7 (2012-12-24) +------------------ + +0.2.6 (2012-12-23) +------------------ + +0.2.5 (2012-12-21 19:11) +------------------------ + +0.2.4 (2012-12-21 01:13) +------------------------ + +0.2.3 (2012-12-21 00:24) +------------------------ + +0.2.2 (2012-12-20 18:29) +------------------------ + +0.2.1 (2012-12-20 17:47) +------------------------ + +0.2.0 (2012-12-20 17:39) +------------------------ +* first release of this package into groovy diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/config/fixed_trajectories.yaml b/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/config/fixed_trajectories.yaml new file mode 100644 index 00000000..64c1f54d --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/config/fixed_trajectories.yaml @@ -0,0 +1,38 @@ +trajectories: + - Figure8: + attributes: + - frame_id + - velocity + - max_acceleration + - length + - width + - height + - Racetrack: + attributes: + - frame_id + - velocity + - max_acceleration + - length + - width + - height + - Circle: + attributes: + - frame_id + - velocity + - radius + - Line: + attributes: + - frame_id + - velocity + - max_acceleration + - length + - width + - height + - Point: + attributes: + - frame_id + - velocity + - max_acceleration + - x + - y + - height diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/index.html b/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/index.html new file mode 100644 index 00000000..72693404 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/index.html @@ -0,0 +1,2612 @@ + + + + + + + + + + + + + + + + + + + RQT Python FixedTrajectoryGenerator - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

RQT Python FixedTrajectoryGenerator

+

If you colcon build this package in a workspace and then run "rqt --force-discover" after sourcing the workspace, the plugin should show up as "Fixed Trajectory Generator" in "Miscellaneous Tools" in the "Plugins" menu.

+

You can use the generate_rqt_py_package.sh script to generate a new package by doing the following from the rqt_fixed_trajectory_generator directory

+
./generate_rqt_py_package.sh [package name] [class name] [plugin title]
+
+

[package name] will be the name of the package and a directory with this name will be created above rqt_fixed_trajectory_generator/. [class name] is the name of the class in src/[package name]/template.py. [plugin title] is what the plugin will be called in the "Miscellaneous Tools" menu.

+

For example,

+
cd rqt_fixed_trajectory_generator/
+./generate_rqt_py_package.sh new_rqt_package ClassName "Plugin Title"
+
+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ +
+ + + +
+
+
+ +
+ Copyright © 2024 - 2030 AirLab CMU +
+ + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + +
+ +
+
+
+ +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/package.xml b/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/package.xml new file mode 100644 index 00000000..8f12f2f8 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/package.xml @@ -0,0 +1,28 @@ + + rqt_fixed_trajectory_generator + 1.0.2 + rqt_fixed_trajectory_generator is a Python GUI template. + John Keller + + BSD + + + + + + John Keller + + ament_index_python + python_qt_binding + qt_gui + qt_gui_py_common + rclpy + rqt_gui + rqt_gui_py + + + + + ament_python + + diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/plugin.xml b/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/plugin.xml new file mode 100644 index 00000000..ec1f2a2b --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/plugin.xml @@ -0,0 +1,17 @@ + + + + A Python GUI plugin providing an interactive Python console. + + + + + folder + Plugins related to miscellaneous tools. + + + applications-python + A Python RQT GUI template. + + + diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/resource/py_console_widget.ui b/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/resource/py_console_widget.ui new file mode 100644 index 00000000..12810f1c --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/resource/py_console_widget.ui @@ -0,0 +1,53 @@ + + + PyConsole + + + + 0 + 0 + 276 + 212 + + + + PyConsole + + + + 0 + + + 0 + + + 0 + + + 3 + + + 0 + + + + + 0 + + + + + + + + + + + PyConsoleTextEdit + QTextEdit +
rqt_py_console.py_console_text_edit
+ + + + + diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/resource/rqt_fixed_trajectory_generator b/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/resource/rqt_fixed_trajectory_generator new file mode 100644 index 00000000..e69de29b diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/setup.cfg b/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/setup.cfg new file mode 100644 index 00000000..2394a82a --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/rqt_fixed_trajectory_generator +[install] +install_scripts=$base/lib/rqt_fixed_trajectory_generator diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/setup.py b/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/setup.py new file mode 100644 index 00000000..12c8b1c2 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/setup.py @@ -0,0 +1,41 @@ +from setuptools import setup +from glob import glob + +package_name = 'rqt_fixed_trajectory_generator' + +setup( + name=package_name, + version='1.0.2', + packages=[package_name], + package_dir={'': 'src'}, + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name + '/resource', + ['resource/py_console_widget.ui']), + ('share/' + package_name, ['package.xml']), + ('share/' + package_name, ['plugin.xml']), + ('share/' + package_name + '/config/', glob('config/*')), + ], + install_requires=['setuptools'], + zip_safe=True, + author='', + maintainer='', + maintainer_email='', + keywords=['ROS'], + classifiers=[ + '', + '', + '', + '', + ], + description=( + 'rqt_fixed_trajectory_generator' + ), + license='BSD', + entry_points={ + 'console_scripts': [ + 'rqt_fixed_trajectory_generator = ' + package_name + '.main:main', + ], + }, +) diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/__init__.py b/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/main.py b/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/main.py new file mode 100644 index 00000000..9a5c9376 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/main.py @@ -0,0 +1,12 @@ +import sys + +from rqt_gui.main import Main + + +def main(): + main = Main() + sys.exit(main.main(sys.argv, standalone='rqt_py_console.py_console.PyConsole')) + + +if __name__ == '__main__': + main() diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/py_console_text_edit.py b/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/py_console_text_edit.py new file mode 100644 index 00000000..dc9ce1a0 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/py_console_text_edit.py @@ -0,0 +1,69 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2012, Dorian Scholz +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import sys +from code import InteractiveInterpreter + +from python_qt_binding import QT_BINDING, QT_BINDING_VERSION +from python_qt_binding.QtCore import Qt, Signal + +from qt_gui_py_common.console_text_edit import ConsoleTextEdit + + +class PyConsoleTextEdit(ConsoleTextEdit): + _color_stdin = Qt.darkGreen + _multi_line_char = ':' + _multi_line_indent = ' ' + _prompt = ('>>> ', '... ') # prompt for single and multi line + exit = Signal() + + def __init__(self, parent=None): + super(PyConsoleTextEdit, self).__init__(parent) + + self._interpreter_locals = {} + self._interpreter = InteractiveInterpreter(self._interpreter_locals) + + self._comment_writer.write('Python %s on %s\n' % + (sys.version.replace('\n', ''), sys.platform)) + self._comment_writer.write( + 'Qt bindings: %s version %s\n' % (QT_BINDING, QT_BINDING_VERSION)) + + self._add_prompt() + + def update_interpreter_locals(self, newLocals): + self._interpreter_locals.update(newLocals) + + def _exec_code(self, code): + try: + self._interpreter.runsource(code) + except SystemExit: # catch sys.exit() calls, so they don't close the whole gui + self.exit.emit() diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/py_console_widget.py b/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/py_console_widget.py new file mode 100644 index 00000000..e69bde34 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/py_console_widget.py @@ -0,0 +1,59 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2012, Dorian Scholz +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import os +from ament_index_python.resources import get_resource + +from python_qt_binding import loadUi +from python_qt_binding.QtWidgets import QWidget +from rqt_py_console.py_console_text_edit import PyConsoleTextEdit + + +class PyConsoleWidget(QWidget): + + def __init__(self, context=None): + super(PyConsoleWidget, self).__init__() + + _, package_path = get_resource('packages', 'rqt_py_console') + ui_file = os.path.join( + package_path, 'share', 'rqt_py_console', 'resource', 'py_console_widget.ui') + + loadUi(ui_file, self, {'PyConsoleTextEdit': PyConsoleTextEdit}) + self.setObjectName('PyConsoleWidget') + + my_locals = { + 'context': context + } + self.py_console.update_interpreter_locals(my_locals) + self.py_console.print_message( + 'The variable "context" is set to the PluginContext of this plugin.') + self.py_console.exit.connect(context.close_plugin) diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/spyder_console_widget.py b/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/spyder_console_widget.py new file mode 100644 index 00000000..374ef7a5 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/spyder_console_widget.py @@ -0,0 +1,60 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2012, Dorian Scholz +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +from python_qt_binding.QtGui import QFont + +from spyder.widgets.internalshell import InternalShell +from spyder.utils.module_completion import moduleCompletion + +class SpyderConsoleWidget(InternalShell): + + def __init__(self, context=None): + my_locals = { + 'context': context + } + super(SpyderConsoleWidget, self).__init__(namespace=my_locals) + self.setObjectName('SpyderConsoleWidget') + self.set_pythonshell_font(QFont('Mono')) + self.interpreter.restore_stds() + + def get_module_completion(self, objtxt): + """Return module completion list associated to object name""" + return moduleCompletion(objtxt) + + def run_command(self, *args): + self.interpreter.redirect_stds() + super(SpyderConsoleWidget, self).run_command(*args) + self.flush() + self.interpreter.restore_stds() + + def shutdown(self): + self.exit_interpreter() diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/template.py b/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/template.py new file mode 100644 index 00000000..76c60c77 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/src/rqt_fixed_trajectory_generator/template.py @@ -0,0 +1,269 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2012, Dorian Scholz +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +from python_qt_binding.QtWidgets import QVBoxLayout, QWidget +from rqt_gui_py.plugin import Plugin +from qt_gui_py_common.simple_settings_dialog import SimpleSettingsDialog +from rqt_py_console.py_console_widget import PyConsoleWidget + +import python_qt_binding.QtWidgets as qt +import python_qt_binding.QtWidgets as QtWidgets +import python_qt_binding.QtGui as QtGui +import python_qt_binding.QtCore as QtCore + +import os +import time +import numpy as np +import yaml +import collections +import pickle +from ament_index_python.packages import get_package_share_directory + +from std_msgs.msg import Bool +from airstack_msgs.msg import FixedTrajectory +from diagnostic_msgs.msg import KeyValue + +try: + from rqt_py_console.spyder_console_widget import SpyderConsoleWidget + _has_spyderlib = True +except ImportError: + _has_spyderlib = False + + +class FixedTrajectoryGenerator(Plugin): + """ + Plugin providing an interactive Python console + """ + + def __init__(self, context): + super(FixedTrajectoryGenerator, self).__init__(context) + self.setObjectName('FixedTrajectoryGenerator') + + self.context = context + # to access ros2 node use self.context.node + + self.config_filename = '' + + self.button_dct = {} + self.attribute_settings = collections.OrderedDict() + + self.timer = self.context.node.create_timer(1./10., self.timer_callback) + + self.fixed_trajectory_pub = self.context.node.create_publisher(FixedTrajectory, 'fixed_trajectory_command', 1) + self.global_plan_fixed_trajectory_pub = self.context.node.create_publisher(FixedTrajectory, 'global_plan_fixed_trajectory', 1) + + # main layout + self.widget = QWidget() + self.vbox = qt.QVBoxLayout() + self.widget.setLayout(self.vbox) + context.add_widget(self.widget) + + # config widget + self.config_widget = qt.QWidget() + self.config_widget.setStyleSheet('QWidget{margin-left:-1px;}') + self.config_layout = qt.QHBoxLayout() + self.config_widget.setLayout(self.config_layout) + self.config_widget.setFixedHeight(50) + + self.config_button = qt.QPushButton('Open Config...') + self.config_button.clicked.connect(self.select_config_file) + self.config_layout.addWidget(self.config_button) + + self.config_label = qt.QLabel('config filename: ') + self.config_layout.addWidget(self.config_label) + self.vbox.addWidget(self.config_widget) + + # trajectory widget + self.trajectory_widget = qt.QWidget() + self.trajectory_layout = qt.QVBoxLayout() + self.trajectory_widget.setLayout(self.trajectory_layout) + self.vbox.addWidget(self.trajectory_widget) + + self.tab_widget = qt.QTabWidget() + self.trajectory_layout.addWidget(self.tab_widget) + + # button widget + self.button_widget = qt.QWidget() + self.button_layout = qt.QHBoxLayout() + self.button_widget.setLayout(self.button_layout) + self.vbox.addWidget(self.button_widget) + + self.publish_button = qt.QPushButton('Publish') + self.publish_button.clicked.connect(self.publish_trajectory) + self.button_layout.addWidget(self.publish_button) + + self.trajectory_type_label = qt.QLabel('Type: ') + self.button_layout.addWidget(self.trajectory_type_label) + + self.trajectory_type_combo_box = qt.QComboBox() + self.trajectory_type_combo_box.addItem('Fixed Trajectory') + self.trajectory_type_combo_box.addItem('Global Plan') + self.button_layout.addWidget(self.trajectory_type_combo_box) + + def publish_trajectory(self): + trajectory_type = self.trajectory_type_combo_box.currentText() + trajectory_name = self.tab_widget.tabText(self.tab_widget.currentIndex()) + msg = FixedTrajectory() + msg.type = trajectory_name + for attribute, value in iter(self.attribute_settings[trajectory_name].items()): + key_value = KeyValue() + key_value.key = attribute + key_value.value = value + msg.attributes.append(key_value) + if trajectory_type == 'Fixed Trajectory': + self.fixed_trajectory_pub.publish(msg) + elif trajectory_type == 'Global Plan': + self.global_plan_fixed_trajectory_pub.publish(msg) + + def select_config_file(self): + starting_path = get_package_share_directory('rqt_fixed_trajectory_generator') + '/config/' + print(starting_path) + filename = qt.QFileDialog.getOpenFileName(self.widget, 'Open Config File', starting_path, "Config Files (*.yaml)")[0] + self.set_config(filename) + + def set_config(self, filename): + if filename != '': + self.config_filename = filename + if self.config_filename != None: + self.config_label.setText('config filename: ' + os.path.basename(self.config_filename)) + self.init_buttons(filename) + + def init_buttons(self, filename): + y = yaml.load(open(filename, 'r').read(), Loader=yaml.Loader) + print(y) + + def get_attribute_changed_function(trajectory_name, attribute_name): + def attribute_changed(text): + if trajectory_name not in self.attribute_settings: + self.attribute_settings[trajectory_name] = {} + self.attribute_settings[trajectory_name][attribute_name] = text + return attribute_changed + + def get_publish_function(trajectory_name): + def publish_function(): + msg = FixedTrajectory() + msg.type = trajectory_name + for attribute, value in iter(self.attribute_settings[trajectory_name].items()): + key_value = KeyValue() + key_value.key = attribute + key_value.value = value + msg.attributes.append(key_value) + self.fixed_trajectory_pub.publish(msg) + return publish_function + + + for trajectory in y['trajectories']: + trajectory_name = list(trajectory.keys())[0] + attributes = trajectory[trajectory_name]['attributes'] + + trajectory_tab = qt.QWidget() + trajectory_layout = qt.QVBoxLayout() + trajectory_tab.setLayout(trajectory_layout) + + for attribute in attributes: + attribute_widget = qt.QWidget() + attribute_layout = qt.QHBoxLayout() + attribute_widget.setLayout(attribute_layout) + + attribute_label = qt.QLabel() + attribute_label.setText(attribute) + attribute_layout.addWidget(attribute_label) + + attribute_default = '0' + if attribute == 'frame_id': + attribute_default = 'world' + if trajectory_name in self.attribute_settings.keys(): + if attribute in self.attribute_settings[trajectory_name].keys(): + attribute_default = self.attribute_settings[trajectory_name][attribute] + + attribute_edit = qt.QLineEdit() + attribute_edit.textChanged.connect(get_attribute_changed_function(trajectory_name, + attribute)) + attribute_edit.setText(attribute_default) + + attribute_layout.addWidget(attribute_edit) + + trajectory_layout.addWidget(attribute_widget) + + #publish_button = qt.QPushButton('Publish') + #publish_button.clicked.connect(get_publish_function(trajectory_name)) + #trajectory_layout.addWidget(publish_button) + + self.tab_widget.addTab(trajectory_tab, trajectory_name) + + + + def timer_callback(self): + bool_msg = Bool() + for key in self.button_dct.keys(): + bool_msg.data = self.button_dct[key]['data'] + self.button_dct[key]['publisher'].publish(bool_msg) + + def save_settings(self, plugin_settings, instance_settings): + instance_settings.set_value('config_filename', self.config_filename) + instance_settings.set_value('attribute_settings', pickle.dumps(self.attribute_settings)) + + def restore_settings(self, plugin_settings, instance_settings): + attribute_settings = instance_settings.value('attribute_settings') + if attribute_settings != None: + self.attribute_settings = pickle.loads(attribute_settings) + self.set_config(instance_settings.value('config_filename')) + + ''' + def save_settings(self, plugin_settings, instance_settings): + pass + #instance_settings.set_value('some_variable', self.some_variable) + + def restore_settings(self, plugin_settings, instance_settings): + pass + #self.some_variable = instance_settings.value('some_variable', default_value) + ''' + def trigger_configuration(self): + options = [ + {'title': 'Option 1', + 'description': 'Description of option 1.', + 'enabled': True}, + {'title': 'Option 2', + 'description': 'Description of option 2.'}, + ] + dialog = SimpleSettingsDialog(title='Options') + dialog.add_exclusive_option_group(title='List of options:', options=options, selected_index=0) + selected_index = dialog.get_settings()[0] + if selected_index != None: + selected_index = selected_index['selected_index'] + print('selected_index ', selected_index) + + def shutdown_console_widget(self): + pass + + def shutdown_plugin(self): + self.shutdown_console_widget() diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/CHANGELOG.rst b/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/CHANGELOG.rst new file mode 100644 index 00000000..0a789b78 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/CHANGELOG.rst @@ -0,0 +1,149 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package rqt_py_console +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.0.2 (2021-08-31) +------------------ +* Fix modern setuptools warning about dashes instead of underscores (`#11 `_) +* Contributors: Chris Lalancette + +1.0.1 (2021-04-27) +------------------ +* Changed the build type to ament_python and fixed package to run with ros2 run (`#8 `_) +* Contributors: Alejandro Hernández Cordero + +1.0.0 (2018-12-11) +------------------ +* spyderlib -> spyder (`#5 `_) +* ros2 port (`#3 `_) +* autopep8 (`#2 `_) +* Contributors: Mike Lautman + +0.4.8 (2017-04-28) +------------------ + +0.4.7 (2017-03-02) +------------------ + +0.4.6 (2017-02-27) +------------------ + +0.4.5 (2017-02-03) +------------------ + +0.4.4 (2017-01-24) +------------------ +* use Python 3 compatible syntax (`#421 `_) + +0.4.3 (2016-11-02) +------------------ + +0.4.2 (2016-09-19) +------------------ + +0.4.1 (2016-05-16) +------------------ + +0.4.0 (2016-04-27) +------------------ +* Support Qt 5 (in Kinetic and higher) as well as Qt 4 (in Jade and earlier) (`#359 `_) + +0.3.13 (2016-03-08) +------------------- + +0.3.12 (2015-07-24) +------------------- + +0.3.11 (2015-04-30) +------------------- + +0.3.10 (2014-10-01) +------------------- +* update plugin scripts to use full name to avoid future naming collisions + +0.3.9 (2014-08-18) +------------------ + +0.3.8 (2014-07-15) +------------------ + +0.3.7 (2014-07-11) +------------------ +* export architecture_independent flag in package.xml (`#254 `_) + +0.3.6 (2014-06-02) +------------------ + +0.3.5 (2014-05-07) +------------------ + +0.3.4 (2014-01-28) +------------------ + +0.3.3 (2014-01-08) +------------------ +* add groups for rqt plugins, renamed some plugins (`#167 `_) + +0.3.2 (2013-10-14) +------------------ + +0.3.1 (2013-10-09) +------------------ + +0.3.0 (2013-08-28) +------------------ + +0.2.17 (2013-07-04) +------------------- + +0.2.16 (2013-04-09 13:33) +------------------------- + +0.2.15 (2013-04-09 00:02) +------------------------- + +0.2.14 (2013-03-14) +------------------- + +0.2.13 (2013-03-11 22:14) +------------------------- + +0.2.12 (2013-03-11 13:56) +------------------------- + +0.2.11 (2013-03-08) +------------------- + +0.2.10 (2013-01-22) +------------------- + +0.2.9 (2013-01-17) +------------------ + +0.2.8 (2013-01-11) +------------------ + +0.2.7 (2012-12-24) +------------------ + +0.2.6 (2012-12-23) +------------------ + +0.2.5 (2012-12-21 19:11) +------------------------ + +0.2.4 (2012-12-21 01:13) +------------------------ + +0.2.3 (2012-12-21 00:24) +------------------------ + +0.2.2 (2012-12-20 18:29) +------------------------ + +0.2.1 (2012-12-20 17:47) +------------------------ + +0.2.0 (2012-12-20 17:39) +------------------------ +* first release of this package into groovy diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/generate_rqt_py_package.sh b/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/generate_rqt_py_package.sh new file mode 100644 index 00000000..4a6362f6 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/generate_rqt_py_package.sh @@ -0,0 +1,30 @@ +#!/bin/bash + +if [ "$#" -ne 3 ]; then + echo "Usage: ./generate_rqt_py_package.sh [package name] [class name] [plugin title]" + exit +fi + +directory="./" +exclude_file="./generate_rqt_py_package.sh" + +package_name=$1 +class_name=$2 +plugin_title=$3 + +new_directory="../$package_name" +mkdir -p $new_directory +cp -r ./ $new_directory +cd $new_directory +rm -rf .git +rm generate_rqt_py_package.sh +cd - > /dev/null + +mv $new_directory/src/rqt_py_template $new_directory/src/$package_name +mv $new_directory/resource/rqt_py_template $new_directory/resource/$package_name + +for file in $(find "$new_directory" -type f); do + sed -i "s/Template/$class_name/g" $file + sed -i "s/rqt_py_template/$package_name/g" $file + sed -i "s/PluginTitle/$plugin_title/g" $file +done diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/index.html b/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/index.html new file mode 100644 index 00000000..19a5b1e2 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/index.html @@ -0,0 +1,2612 @@ + + + + + + + + + + + + + + + + + + + RQT Python Template - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

RQT Python Template

+

If you colcon build this package in a workspace and then run "rqt --force-discover" after sourcing the workspace, the plugin should show up as "PluginTitle" in "Miscellaneous Tools" in the "Plugins" menu.

+

You can use the generate_rqt_py_package.sh script to generate a new package by doing the following from the rqt_py_template directory

+
./generate_rqt_py_package.sh [package name] [class name] [plugin title]
+
+

[package name] will be the name of the package and a directory with this name will be created above rqt_py_template/. [class name] is the name of the class in src/[package name]/template.py. [plugin title] is what the plugin will be called in the "Miscellaneous Tools" menu.

+

For example,

+
cd rqt_py_template/
+./generate_rqt_py_package.sh new_rqt_package ClassName "Plugin Title"
+
+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ +
+ + + +
+
+
+ +
+ Copyright © 2024 - 2030 AirLab CMU +
+ + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + +
+ +
+
+
+ +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/package.xml b/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/package.xml new file mode 100644 index 00000000..4aa15428 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/package.xml @@ -0,0 +1,28 @@ + + rqt_py_template + 1.0.2 + rqt_py_template is a Python GUI template. + John Keller + + BSD + + + + + + John Keller + + ament_index_python + python_qt_binding + qt_gui + qt_gui_py_common + rclpy + rqt_gui + rqt_gui_py + + + + + ament_python + + diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/plugin.xml b/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/plugin.xml new file mode 100644 index 00000000..03d62a9f --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/plugin.xml @@ -0,0 +1,17 @@ + + + + A Python GUI plugin providing an interactive Python console. + + + + + folder + Plugins related to miscellaneous tools. + + + applications-python + A Python RQT GUI template. + + + diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/resource/py_console_widget.ui b/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/resource/py_console_widget.ui new file mode 100644 index 00000000..12810f1c --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/resource/py_console_widget.ui @@ -0,0 +1,53 @@ + + + PyConsole + + + + 0 + 0 + 276 + 212 + + + + PyConsole + + + + 0 + + + 0 + + + 0 + + + 3 + + + 0 + + + + + 0 + + + + + + + + + + + PyConsoleTextEdit + QTextEdit +
rqt_py_console.py_console_text_edit
+ + + + + diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/resource/rqt_py_template b/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/resource/rqt_py_template new file mode 100644 index 00000000..e69de29b diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/setup.cfg b/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/setup.cfg new file mode 100644 index 00000000..4cc96e39 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/rqt_py_template +[install] +install_scripts=$base/lib/rqt_py_template diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/setup.py b/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/setup.py new file mode 100644 index 00000000..1b898460 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/setup.py @@ -0,0 +1,39 @@ +from setuptools import setup + +package_name = 'rqt_py_template' + +setup( + name=package_name, + version='1.0.2', + packages=[package_name], + package_dir={'': 'src'}, + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name + '/resource', + ['resource/py_console_widget.ui']), + ('share/' + package_name, ['package.xml']), + ('share/' + package_name, ['plugin.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + author='', + maintainer='', + maintainer_email='', + keywords=['ROS'], + classifiers=[ + '', + '', + '', + '', + ], + description=( + 'rqt_py_template' + ), + license='BSD', + entry_points={ + 'console_scripts': [ + 'rqt_py_template = ' + package_name + '.main:main', + ], + }, +) diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/src/rqt_py_template/__init__.py b/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/src/rqt_py_template/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/src/rqt_py_template/main.py b/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/src/rqt_py_template/main.py new file mode 100644 index 00000000..9a5c9376 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/src/rqt_py_template/main.py @@ -0,0 +1,12 @@ +import sys + +from rqt_gui.main import Main + + +def main(): + main = Main() + sys.exit(main.main(sys.argv, standalone='rqt_py_console.py_console.PyConsole')) + + +if __name__ == '__main__': + main() diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/src/rqt_py_template/py_console_text_edit.py b/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/src/rqt_py_template/py_console_text_edit.py new file mode 100644 index 00000000..dc9ce1a0 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/src/rqt_py_template/py_console_text_edit.py @@ -0,0 +1,69 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2012, Dorian Scholz +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import sys +from code import InteractiveInterpreter + +from python_qt_binding import QT_BINDING, QT_BINDING_VERSION +from python_qt_binding.QtCore import Qt, Signal + +from qt_gui_py_common.console_text_edit import ConsoleTextEdit + + +class PyConsoleTextEdit(ConsoleTextEdit): + _color_stdin = Qt.darkGreen + _multi_line_char = ':' + _multi_line_indent = ' ' + _prompt = ('>>> ', '... ') # prompt for single and multi line + exit = Signal() + + def __init__(self, parent=None): + super(PyConsoleTextEdit, self).__init__(parent) + + self._interpreter_locals = {} + self._interpreter = InteractiveInterpreter(self._interpreter_locals) + + self._comment_writer.write('Python %s on %s\n' % + (sys.version.replace('\n', ''), sys.platform)) + self._comment_writer.write( + 'Qt bindings: %s version %s\n' % (QT_BINDING, QT_BINDING_VERSION)) + + self._add_prompt() + + def update_interpreter_locals(self, newLocals): + self._interpreter_locals.update(newLocals) + + def _exec_code(self, code): + try: + self._interpreter.runsource(code) + except SystemExit: # catch sys.exit() calls, so they don't close the whole gui + self.exit.emit() diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/src/rqt_py_template/py_console_widget.py b/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/src/rqt_py_template/py_console_widget.py new file mode 100644 index 00000000..e69bde34 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/src/rqt_py_template/py_console_widget.py @@ -0,0 +1,59 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2012, Dorian Scholz +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import os +from ament_index_python.resources import get_resource + +from python_qt_binding import loadUi +from python_qt_binding.QtWidgets import QWidget +from rqt_py_console.py_console_text_edit import PyConsoleTextEdit + + +class PyConsoleWidget(QWidget): + + def __init__(self, context=None): + super(PyConsoleWidget, self).__init__() + + _, package_path = get_resource('packages', 'rqt_py_console') + ui_file = os.path.join( + package_path, 'share', 'rqt_py_console', 'resource', 'py_console_widget.ui') + + loadUi(ui_file, self, {'PyConsoleTextEdit': PyConsoleTextEdit}) + self.setObjectName('PyConsoleWidget') + + my_locals = { + 'context': context + } + self.py_console.update_interpreter_locals(my_locals) + self.py_console.print_message( + 'The variable "context" is set to the PluginContext of this plugin.') + self.py_console.exit.connect(context.close_plugin) diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/src/rqt_py_template/spyder_console_widget.py b/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/src/rqt_py_template/spyder_console_widget.py new file mode 100644 index 00000000..374ef7a5 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/src/rqt_py_template/spyder_console_widget.py @@ -0,0 +1,60 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2012, Dorian Scholz +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +from python_qt_binding.QtGui import QFont + +from spyder.widgets.internalshell import InternalShell +from spyder.utils.module_completion import moduleCompletion + +class SpyderConsoleWidget(InternalShell): + + def __init__(self, context=None): + my_locals = { + 'context': context + } + super(SpyderConsoleWidget, self).__init__(namespace=my_locals) + self.setObjectName('SpyderConsoleWidget') + self.set_pythonshell_font(QFont('Mono')) + self.interpreter.restore_stds() + + def get_module_completion(self, objtxt): + """Return module completion list associated to object name""" + return moduleCompletion(objtxt) + + def run_command(self, *args): + self.interpreter.redirect_stds() + super(SpyderConsoleWidget, self).run_command(*args) + self.flush() + self.interpreter.restore_stds() + + def shutdown(self): + self.exit_interpreter() diff --git a/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/src/rqt_py_template/template.py b/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/src/rqt_py_template/template.py new file mode 100644 index 00000000..8a8b2fd8 --- /dev/null +++ b/robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/src/rqt_py_template/template.py @@ -0,0 +1,95 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2012, Dorian Scholz +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +from python_qt_binding.QtWidgets import QVBoxLayout, QWidget +from rqt_gui_py.plugin import Plugin +from qt_gui_py_common.simple_settings_dialog import SimpleSettingsDialog +from rqt_py_console.py_console_widget import PyConsoleWidget + +import python_qt_binding.QtWidgets as qt +import python_qt_binding.QtWidgets as QtWidgets +import python_qt_binding.QtGui as QtGui +import python_qt_binding.QtCore as QtCore + +try: + from rqt_py_console.spyder_console_widget import SpyderConsoleWidget + _has_spyderlib = True +except ImportError: + _has_spyderlib = False + + +class Template(Plugin): + """ + Plugin providing an interactive Python console + """ + + def __init__(self, context): + super(Template, self).__init__(context) + self.setObjectName('Template') + + self.context = context + # to access ros2 node use self.context.node + + self.widget = QWidget() + self.button = QtWidgets.QPushButton(text='Button') + self.layout = QtWidgets.QVBoxLayout(self.widget) + self.layout.addWidget(self.button) + self.context.add_widget(self.widget) + + def save_settings(self, plugin_settings, instance_settings): + pass + #instance_settings.set_value('some_variable', self.some_variable) + + def restore_settings(self, plugin_settings, instance_settings): + pass + #self.some_variable = instance_settings.value('some_variable', default_value) + + def trigger_configuration(self): + options = [ + {'title': 'Option 1', + 'description': 'Description of option 1.', + 'enabled': True}, + {'title': 'Option 2', + 'description': 'Description of option 2.'}, + ] + dialog = SimpleSettingsDialog(title='Options') + dialog.add_exclusive_option_group(title='List of options:', options=options, selected_index=0) + selected_index = dialog.get_settings()[0] + if selected_index != None: + selected_index = selected_index['selected_index'] + print('selected_index ', selected_index) + + def shutdown_console_widget(self): + pass + + def shutdown_plugin(self): + self.shutdown_console_widget() diff --git a/robot/ros_ws/src/autonomy/autonomy_bringup/CMakeLists.txt b/robot/ros_ws/src/autonomy/autonomy_bringup/CMakeLists.txt new file mode 100644 index 00000000..b2476c52 --- /dev/null +++ b/robot/ros_ws/src/autonomy/autonomy_bringup/CMakeLists.txt @@ -0,0 +1,32 @@ +cmake_minimum_required(VERSION 3.8) +project(autonomy_bringup) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +# Install files. +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +# install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME}) +# install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) +# install(DIRECTORY params DESTINATION share/${PROJECT_NAME}) + +ament_package() diff --git a/robot/ros_ws/src/autonomy/autonomy_bringup/LICENSE b/robot/ros_ws/src/autonomy/autonomy_bringup/LICENSE new file mode 100644 index 00000000..d6456956 --- /dev/null +++ b/robot/ros_ws/src/autonomy/autonomy_bringup/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/robot/ros_ws/src/autonomy/autonomy_bringup/launch/autonomy.launch.xml b/robot/ros_ws/src/autonomy/autonomy_bringup/launch/autonomy.launch.xml new file mode 100644 index 00000000..0380d6c7 --- /dev/null +++ b/robot/ros_ws/src/autonomy/autonomy_bringup/launch/autonomy.launch.xml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/autonomy_bringup/package.xml b/robot/ros_ws/src/autonomy/autonomy_bringup/package.xml new file mode 100644 index 00000000..eb765722 --- /dev/null +++ b/robot/ros_ws/src/autonomy/autonomy_bringup/package.xml @@ -0,0 +1,18 @@ + + + + autonomy_bringup + 0.0.0 + TODO: Package description + andrew + Apache-2.0 + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/robot/ros_ws/src/logging/logging_bringup/CMakeLists.txt b/robot/ros_ws/src/logging/logging_bringup/CMakeLists.txt new file mode 100644 index 00000000..2312ed6e --- /dev/null +++ b/robot/ros_ws/src/logging/logging_bringup/CMakeLists.txt @@ -0,0 +1,32 @@ +cmake_minimum_required(VERSION 3.8) +project(logging_bringup) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +# Install files. +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +# install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME}) +# install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) +# install(DIRECTORY params DESTINATION share/${PROJECT_NAME}) + +ament_package() diff --git a/robot/ros_ws/src/logging/logging_bringup/LICENSE b/robot/ros_ws/src/logging/logging_bringup/LICENSE new file mode 100644 index 00000000..d6456956 --- /dev/null +++ b/robot/ros_ws/src/logging/logging_bringup/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/robot/ros_ws/src/logging/logging_bringup/launch/logging.launch.xml b/robot/ros_ws/src/logging/logging_bringup/launch/logging.launch.xml new file mode 100644 index 00000000..a62ec22a --- /dev/null +++ b/robot/ros_ws/src/logging/logging_bringup/launch/logging.launch.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/robot/ros_ws/src/logging/logging_bringup/package.xml b/robot/ros_ws/src/logging/logging_bringup/package.xml new file mode 100644 index 00000000..0dc09699 --- /dev/null +++ b/robot/ros_ws/src/logging/logging_bringup/package.xml @@ -0,0 +1,18 @@ + + + + logging_bringup + 0.0.0 + TODO: Package description + andrew + Apache-2.0 + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/robot/ros_ws/src/robot_bringup/CMakeLists.txt b/robot/ros_ws/src/robot_bringup/CMakeLists.txt new file mode 100644 index 00000000..d62c02dd --- /dev/null +++ b/robot/ros_ws/src/robot_bringup/CMakeLists.txt @@ -0,0 +1,33 @@ +cmake_minimum_required(VERSION 3.8) +project(robot_bringup) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +# Install files. +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY params DESTINATION share/${PROJECT_NAME}) + +ament_package() + \ No newline at end of file diff --git a/robot/ros_ws/src/robot_bringup/config/core.perspective b/robot/ros_ws/src/robot_bringup/config/core.perspective new file mode 100644 index 00000000..cb8bdb03 --- /dev/null +++ b/robot/ros_ws/src/robot_bringup/config/core.perspective @@ -0,0 +1,128 @@ +{ + "keys": {}, + "groups": { + "mainwindow": { + "keys": { + "geometry": { + "repr(QByteArray.hex)": "QtCore.QByteArray(b'01d9d0cb00030000000008490000015c00000fc800000492000008490000018100000fc80000049200000000000000000a00000008490000018100000fc800000492')", + "type": "repr(QByteArray.hex)", + "pretty-print": " I \\ I I " + }, + "state": { + "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff00000000fd000000010000000300000780000002e8fc0100000002fc0000000000000780000003d200fffffffc0200000002fb00000042007200710074005f006200650068006100760069006f0072005f0074007200650065005f005f005000790043006f006e0073006f006c0065005f005f0031005f005f0100000014000000fe0000004a00fffffffc00000118000001e4000001e400fffffffc0100000002fb00000072007200710074005f006200650068006100760069006f0072005f0074007200650065005f0063006f006d006d0061006e0064005f005f004200650068006100760069006f007200540072006500650043006f006d006d0061006e00640050006c007500670069006e005f005f0031005f005f0100000000000005ba0000025d00fffffffb0000007a007200710074005f00660069007800650064005f007400720061006a006500630074006f00720079005f00670065006e0065007200610074006f0072005f005f00460069007800650064005400720061006a006500630074006f0072007900470065006e0065007200610074006f0072005f005f0031005f005f01000005c0000001c00000016f00fffffffb0000007a007200710074005f00660069007800650064005f007400720061006a006500630074006f00720079005f00670065006e0065007200610074006f0072005f005f00460069007800650064005400720061006a006500630074006f0072007900470065006e0065007200610074006f0072005f005f0031005f005f01000004b20000007e0000000000000000000007800000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')", + "type": "repr(QByteArray.hex)", + "pretty-print": " J rrqt_behavior_tree_command__BehaviorTreeCommandPlugin__1__ zrqt_fixed_trajectory_generator__FixedTrajectoryGenerator__1__ zrqt_fixed_trajectory_generator__FixedTrajectoryGenerator__1__ 6MinimizedDockWidgetsToolbar " + } + }, + "groups": { + "toolbar_areas": { + "keys": { + "MinimizedDockWidgetsToolbar": { + "repr": "8", + "type": "repr" + } + }, + "groups": {} + } + } + }, + "pluginmanager": { + "keys": { + "running-plugins": { + "repr": "{'rqt_behavior_tree/PyConsole': [1], 'rqt_behavior_tree_command/BehaviorTreeCommandPlugin': [1], 'rqt_fixed_trajectory_generator/FixedTrajectoryGenerator': [1]}", + "type": "repr" + } + }, + "groups": { + "plugin__rqt_behavior_tree__PyConsole__1": { + "keys": {}, + "groups": { + "dock_widget__": { + "keys": { + "dock_widget_title": { + "repr": "''", + "type": "repr" + }, + "dockable": { + "repr": "True", + "type": "repr" + }, + "parent": { + "repr": "None", + "type": "repr" + } + }, + "groups": {} + } + } + }, + "plugin__rqt_behavior_tree_command__BehaviorTreeCommandPlugin__1": { + "keys": {}, + "groups": { + "dock_widget__": { + "keys": { + "dock_widget_title": { + "repr": "''", + "type": "repr" + }, + "dockable": { + "repr": "True", + "type": "repr" + }, + "parent": { + "repr": "None", + "type": "repr" + } + }, + "groups": {} + }, + "plugin": { + "keys": { + "config_filename": { + "repr": "'/root/ros_ws/install/rqt_behavior_tree_command/share/rqt_behavior_tree_command/config/gui_config.yaml'", + "type": "repr" + } + }, + "groups": {} + } + } + }, + "plugin__rqt_fixed_trajectory_generator__FixedTrajectoryGenerator__1": { + "keys": {}, + "groups": { + "dock_widget__": { + "keys": { + "dock_widget_title": { + "repr": "''", + "type": "repr" + }, + "dockable": { + "repr": "True", + "type": "repr" + }, + "parent": { + "repr": "None", + "type": "repr" + } + }, + "groups": {} + }, + "plugin": { + "keys": { + "attribute_settings": { + "repr": "b'\\x80\\x04\\x95*\\x02\\x00\\x00\\x00\\x00\\x00\\x00\\x8c\\x0bcollections\\x94\\x8c\\x0bOrderedDict\\x94\\x93\\x94)R\\x94(\\x8c\\x07Figure8\\x94}\\x94(\\x8c\\x08frame_id\\x94\\x8c\\tbase_link\\x94\\x8c\\x08velocity\\x94\\x8c\\x012\\x94\\x8c\\x10max_acceleration\\x94\\x8c\\x012\\x94\\x8c\\x06length\\x94\\x8c\\x015\\x94\\x8c\\x05width\\x94\\x8c\\x013\\x94\\x8c\\x06height\\x94\\x8c\\x010\\x94u\\x8c\\tRacetrack\\x94}\\x94(\\x8c\\x08frame_id\\x94\\x8c\\tbase_link\\x94\\x8c\\x08velocity\\x94\\x8c\\x030.2\\x94\\x8c\\x10max_acceleration\\x94\\x8c\\x030.1\\x94\\x8c\\x06length\\x94\\x8c\\x015\\x94\\x8c\\x05width\\x94\\x8c\\x013\\x94\\x8c\\x06height\\x94\\x8c\\x010\\x94u\\x8c\\x06Circle\\x94}\\x94(\\x8c\\x08frame_id\\x94\\x8c\\tbase_link\\x94\\x8c\\x08velocity\\x94\\x8c\\x030.2\\x94\\x8c\\x06radius\\x94\\x8c\\x012\\x94u\\x8c\\x04Line\\x94}\\x94(\\x8c\\x08frame_id\\x94\\x8c\\tbase_link\\x94\\x8c\\x08velocity\\x94\\x8c\\x030.2\\x94\\x8c\\x10max_acceleration\\x94\\x8c\\x030.1\\x94\\x8c\\x06length\\x94\\x8c\\x012\\x94\\x8c\\x05width\\x94\\x8c\\x010\\x94\\x8c\\x06height\\x94\\x8c\\x010\\x94u\\x8c\\x05Point\\x94}\\x94(\\x8c\\x08frame_id\\x94\\x8c\\tbase_link\\x94\\x8c\\x08velocity\\x94\\x8c\\x030.2\\x94\\x8c\\x10max_acceleration\\x94\\x8c\\x030.1\\x94\\x8c\\x01x\\x94\\x8c\\x010\\x94\\x8c\\x01y\\x94\\x8c\\x010\\x94\\x8c\\x06height\\x94\\x8c\\x010\\x94uu.'", + "type": "repr" + }, + "config_filename": { + "repr": "'/root/ros_ws/install/rqt_fixed_trajectory_generator/share/rqt_fixed_trajectory_generator/config/fixed_trajectories.yaml'", + "type": "repr" + } + }, + "groups": {} + } + } + } + } + } + } +} \ No newline at end of file diff --git a/robot/ros_ws/src/robot_bringup/launch/robot.launch.xml b/robot/ros_ws/src/robot_bringup/launch/robot.launch.xml new file mode 100644 index 00000000..76d16918 --- /dev/null +++ b/robot/ros_ws/src/robot_bringup/launch/robot.launch.xml @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/robot/ros_ws/src/robot_bringup/launch/static_transforms.launch.xml b/robot/ros_ws/src/robot_bringup/launch/static_transforms.launch.xml new file mode 100644 index 00000000..0ebb2bfb --- /dev/null +++ b/robot/ros_ws/src/robot_bringup/launch/static_transforms.launch.xml @@ -0,0 +1,11 @@ + + + + + + + \ No newline at end of file diff --git a/robot/ros_ws/src/robot_bringup/package.xml b/robot/ros_ws/src/robot_bringup/package.xml new file mode 100644 index 00000000..87b6a3c5 --- /dev/null +++ b/robot/ros_ws/src/robot_bringup/package.xml @@ -0,0 +1,18 @@ + + + + robot_bringup + 0.0.0 + TODO: Package description + andrew + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/robot/ros_ws/src/robot_bringup/params/domain_bridge.yaml b/robot/ros_ws/src/robot_bringup/params/domain_bridge.yaml new file mode 100644 index 00000000..688eeff9 --- /dev/null +++ b/robot/ros_ws/src/robot_bringup/params/domain_bridge.yaml @@ -0,0 +1,49 @@ +name: my_domain_bridge +from_domain: 1 +to_domain: 2 +topics: + # Bridge "/foo/chatter" topic from doman ID 2 to domain ID 3 + # Automatically detect QoS settings and default to 'keep_last' history with depth 10 + robot_1/interface/mavros/state: + type: mavros_msgs/msg/State + from_domain: 1 + to_domain: 2 + + robot_2/interface/mavros/state: + type: mavros_msgs/msg/State + from_domain: 2 + to_domain: 1 + + # Bridge "/clock" topic from doman ID 2 to domain ID 3, + # Override durability to be 'volatile' and override depth to be 1 + clock: + type: rosgraph_msgs/msg/Clock + qos: + durability: volatile + depth: 1 + + # Bridge "/clock" topic from doman ID 2 to domain ID 6 + # Automatically detect QoS settings and override history to 'keep_all' + # clock: + # type: rosgraph_msgs/msg/Clock + # to_domain: 6 + # qos: + # history: keep_all + + # Bridge "/chitter" topic from domain ID 2 to domain ID 3 with the name "/chatter" + # chitter: + # type: example_interfaces/msg/String + # remap: chatter + +# services: + # Bridge "add_two_ints" service from domain ID 4 to domain ID 6 + # add_two_ints: + # type: example_interfaces/srv/AddTwoInts + # from_domain: 4 + # to_domain: 6 + +# actions: + # Bridge "fibonacci" action from domain ID 1 to domain ID 3 + # fibonacci: + # type: example_interfaces/action/Fibonacci + # from_domain: 1 \ No newline at end of file diff --git a/robot/ros_ws/src/robot_bringup/rviz/robot.rviz b/robot/ros_ws/src/robot_bringup/rviz/robot.rviz new file mode 100644 index 00000000..93da615d --- /dev/null +++ b/robot/ros_ws/src/robot_bringup/rviz/robot.rviz @@ -0,0 +1,514 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /TF1/Frames1 + - /Sensors1 + - /Local1 + - /Local1/DROAN1 + - /Local1/Trajectory Controller1 + - /Global1 + Splitter Ratio: 0.590062141418457 + Tree Height: 1085 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: Expansion Cloud +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 100 + Reference Frame: + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + American_Beech: + Value: true + Plane: + Value: true + base_link: + Value: true + base_link_frd: + Value: false + base_link_stabilized: + Value: false + front_stereo: + Value: false + left_camera: + Value: true + look_ahead_point: + Value: false + look_ahead_point_stabilized: + Value: false + map: + Value: true + map_FLU: + Value: false + map_ned: + Value: false + odom: + Value: false + odom_ned: + Value: false + ouster: + Value: false + right_camera: + Value: true + tracking_point: + Value: true + tracking_point_stabilized: + Value: false + world: + Value: false + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + world: + American_Beech: + {} + Plane: + {} + map_FLU: + map: + base_link: + base_link_frd: + {} + front_stereo: + left_camera: + {} + right_camera: + {} + ouster: + {} + base_link_stabilized: + {} + look_ahead_point: + {} + look_ahead_point_stabilized: + {} + map_ned: + {} + tracking_point: + {} + tracking_point_stabilized: + {} + Update Interval: 0 + Value: true + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Front Left RGB + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: sensors/front_stereo/left/image_rect + Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 100 + Median window: 5 + Min Value: 0 + Name: Front Left Depth + Normalize Range: false + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: sensors/front_stereo/left/depth + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 6.571824073791504 + Min Value: -0.5682187080383301 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 170; 170; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Lidar + Position Transformer: XYZ + Selectable: true + Size (Pixels): 1 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: sensors/ouster/point_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Angle Tolerance: 0 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: false + Keep: 1 + Name: Odometry + Position Tolerance: 0 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Axes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: odometry_conversion/odometry + Value: false + Enabled: true + Name: Sensors + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/Marker + Enabled: false + Name: Disparity Frustum + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_1/droan/frustum + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Disparity Map Collision Checking + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_1/droan/disparity_map_debug + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Disparity Graph Poses + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_1/droan/disparity_graph + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Trimmed Global Plan for DROAN + Namespaces: + global_plan: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: droan/local_planner_global_plan_vis + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: ExpansionPoly + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: droan/expansion_poly + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 220 + Min Color: 0; 0; 0 + Min Intensity: 120 + Name: Expansion Cloud + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: droan/expansion_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Traj Library + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: droan/trajectory_library_vis + Value: false + Enabled: true + Name: DROAN + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Traj Vis + Namespaces: + traj_controller: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: trajectory_controller/trajectory_vis + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Traj Debug + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: trajectory_controller/trajectory_controller_debug_markers + Value: false + Enabled: true + Name: Trajectory Controller + Enabled: true + Name: Local + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/Marker + Enabled: true + Name: VDB Mapping Marker + Namespaces: + "": true + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: vdb_mapping/vdb_map_visualization + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 0; 255; 255 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: Global Plan + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_1/global_plan + Value: true + Enabled: true + Name: Global + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 14.309243202209473 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 3.3486995697021484 + Y: -0.9512473344802856 + Z: 1.4642823934555054 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.47539615631103516 + Target Frame: + Value: Orbit (rviz) + Yaw: 2.9035723209381104 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Front Left Depth: + collapsed: false + Front Left RGB: + collapsed: false + Height: 1376 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001e5000004c6fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000004c6000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000002eb000000c9000000000000000000000001000001f6000004c6fc0200000007fb00000016004c006500660074002000430061006d006500720061010000003b000001880000000000000000fb00000014004c006500660074002000440065007000740068010000003b0000016a0000000000000000fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001c00460072006f006e00740020004c0065006600740020005200470042010000003b0000020e0000002800fffffffb0000002000460072006f006e00740020004c006500660074002000440065007000740068010000024f000002b20000002800fffffffb0000000a0056006900650077007300000000fd000001a8000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000009ba0000003efc0100000002fb0000000800540069006d00650100000000000009ba0000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000005d3000004c600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2490 + X: 1990 + Y: 27 diff --git a/search/search_index.json b/search/search_index.json new file mode 100644 index 00000000..3cf3e4df --- /dev/null +++ b/search/search_index.json @@ -0,0 +1 @@ +{"config":{"lang":["en"],"separator":"[\\s\\-]+","pipeline":["stopWordFilter"]},"docs":[{"location":"","title":"AirStack Boilerplate","text":"

Welcome to the AirStack Boilerplate! This repository template serves to kickstart the development of your own robotics autonomy stack. You're encouraged to customize your version in any way to best suit your project's needs.

This boilerplate is maintained by the AirLab at Carnegie Mellon University's Robotics Institute.

Please head to our Getting Started page to start.

AirStack

"},{"location":"CODE_OF_CONDUCT/","title":"Contributor Covenant Code of Conduct","text":""},{"location":"CODE_OF_CONDUCT/#our-pledge","title":"Our Pledge","text":"

In the interest of fostering an open and welcoming environment, we as contributors and maintainers pledge to making participation in our project and our community a harassment-free experience for everyone, regardless of age, body size, disability, ethnicity, gender identity and expression, level of experience, education, socio-economic status, nationality, personal appearance, race, religion, or sexual identity and orientation.

"},{"location":"CODE_OF_CONDUCT/#our-standards","title":"Our Standards","text":"

Examples of behavior that contributes to creating a positive environment include:

  • Using welcoming and inclusive language
  • Being respectful of differing viewpoints and experiences
  • Gracefully accepting constructive criticism
  • Focusing on what is best for the community
  • Showing empathy towards other community members

Examples of unacceptable behavior by participants include:

  • The use of sexualized language or imagery and unwelcome sexual attention or advances
  • Trolling, insulting/derogatory comments, and personal or political attacks
  • Public or private harassment
  • Publishing others' private information, such as a physical or electronic address, without explicit permission
  • Other conduct which could reasonably be considered inappropriate in a professional setting
"},{"location":"CODE_OF_CONDUCT/#our-responsibilities","title":"Our Responsibilities","text":"

Project maintainers are responsible for clarifying the standards of acceptable behavior and are expected to take appropriate and fair corrective action in response to any instances of unacceptable behavior.

Project maintainers have the right and responsibility to remove, edit, or reject comments, commits, code, wiki edits, issues, and other contributions that are not aligned to this Code of Conduct, or to ban temporarily or permanently any contributor for other behaviors that they deem inappropriate, threatening, offensive, or harmful.

"},{"location":"CODE_OF_CONDUCT/#scope","title":"Scope","text":"

This Code of Conduct applies both within project spaces and in public spaces when an individual is representing the project or its community. Examples of representing a project or community include using an official project e-mail address, posting via an official social media account, or acting as an appointed representative at an online or offline event. Representation of a project may be further defined and clarified by project maintainers.

"},{"location":"CODE_OF_CONDUCT/#enforcement","title":"Enforcement","text":"

Instances of abusive, harassing, or otherwise unacceptable behavior may be reported by contacting the project team at {{ email }}. All complaints will be reviewed and investigated and will result in a response that is deemed necessary and appropriate to the circumstances. The project team is obligated to maintain confidentiality with regard to the reporter of an incident. Further details of specific enforcement policies may be posted separately.

Project maintainers who do not follow or enforce the Code of Conduct in good faith may face temporary or permanent repercussions as determined by other members of the project's leadership.

"},{"location":"CODE_OF_CONDUCT/#attribution","title":"Attribution","text":"

This Code of Conduct is adapted from the Contributor Covenant, version 1.4, available at https://www.contributor-covenant.org/version/1/4/code-of-conduct.html

"},{"location":"common/","title":"Index","text":"

Common top-level files for sub workspaces.

ros_packages/ contains common ROS packages that are used between different machines, such as ground control station and the robot. This folder is mounted under the docker container under ~/ros_ws/src/common

"},{"location":"docs/","title":"AirStack Boilerplate","text":"

Welcome to the AirStack Boilerplate! This repository template serves to kickstart the development of your own robotics autonomy stack. You're encouraged to customize your version in any way to best suit your project's needs.

This boilerplate is maintained by the AirLab at Carnegie Mellon University's Robotics Institute.

Please head to our Getting Started page to start.

AirStack

"},{"location":"docs/about/","title":"About","text":"

This stack is built and maintained by the AirLab at Carnegie Mellon University's Robotics Institute.

"},{"location":"docs/about/#license","title":"License","text":"

Not sure yet but probably Apache 2.0 or MIT for the open source parts.

"},{"location":"docs/about/#faq","title":"FAQ","text":"

Phasellus posuere in sem ut cursus

Lorem ipsum dolor sit amet, consectetur adipiscing elit. Nulla et euismod nulla. Curabitur feugiat, tortor non consequat finibus, justo purus auctor massa, nec semper lorem quam in massa.

"},{"location":"docs/getting_started/","title":"Getting Started","text":"

By the end of this tutorial, you will have the autonomy stack running on your machine.

"},{"location":"docs/getting_started/#requirements","title":"Requirements","text":"

You need at least 25GB free to install the Docker image.

Have an NVIDIA GPU >= RTX 3070 to run Isaac Sim locally.

"},{"location":"docs/getting_started/#setup","title":"Setup","text":""},{"location":"docs/getting_started/#clone","title":"Clone","text":"
git clone --recursive -j8 git@github.com:castacks/AirStack.git\n
"},{"location":"docs/getting_started/#docker","title":"Docker","text":"

Follow NVIDIA's instructions for installing Docker to be compatible with NVIDIA GPUs, including adding the NVIDIA Container Toolkit. Make sure docker-compose-plugin is also installed with Docker.

"},{"location":"docs/getting_started/#configure","title":"Configure","text":"

Run ./configure.sh and follow the instructions in the prompts to do an initial configuration of the repo.

"},{"location":"docs/getting_started/#docker-images","title":"Docker Images","text":"

Now you have two options on how to proceed. You can build the docker image from scratch or pull the existing image on the airlab docker registry. Building the image from scratch can be useful if you would like to add new dependencies or add new custom functionality. For most users just pulling the existing image will be more conveninent and fast since it doesn't require access to the Nvidia registry.

Option 1: Pull From the Airlab Registry (Preferred) To use the AirLab Docker registry do the following 
cd AirStack/\ndocker login airlab-storage.andrew.cmu.edu:5001\n## <Enter your andrew id (without @andrew.cmu.edu)>\n## <Enter your andrew password>\n\n## Pull the images in the docker compose file\ndocker compose pull\n
The images will be pulled from the server automatically. This might take a while since the images are large. Option 2: Build Docker Images From Scratch 1. Download the Ascent Spirit SITL software package by running this script (pip3 is required): 
cd AirStack/\nbash simulation/isaac-sim/installation/download_sitl.bash\n
2. Next, gain access to NVIDIA NGC Containers by following these instructions. Then: 
cd AirStack/\ndocker compose build  # build the images locally\n
If you have permission you can push updated images to the docker server. 
docker compose push\n
"},{"location":"docs/getting_started/#launch","title":"Launch","text":"
xhost +  # allow docker access to X-Server\n\n# Make sure you are in the AirStack directory.\n\n# Start docker compose services. This launches Isaac Sim and the robots.\n#  You can append `--scale robot=[NUM_ROBOTS]` for more robots, default is 1\ndocker compose up -d\n

Then open the stage from the Nucleus server: airlab-storage.andrew.cmu.edu:8443/Projects/AirStack/neighborhood.scene.usd

"},{"location":"docs/getting_started/#move-robot","title":"Move Robot","text":"

Find the RQT GUI window. Hit Takeoff, then hit Publish in the trajectory window like in this video:

Note you can also use the ros2 topic pub command to move the robot. For example, to fly to a position:

# start another terminal in docker container\ndocker exec -it airstack-robot-1 bash\n# in docker\n# FLY TO POSITION. Put whatever position you want\nros2 topic pub /robot_1/interface/mavros/setpoint_position/local geometry_msgs/PoseStamped \\\n    \"{ header: { stamp: { sec: 0, nanosec: 0 }, frame_id: 'base_link' }, \\\n    pose: { position: { x: 10.0, y: 0.0, z: 20.0 }, orientation: { x: 0.0, y: 0.0, z: 0.0, w: 1.0 } } }\" -1\n
"},{"location":"docs/getting_started/#shutdown","title":"Shutdown","text":"

To shutdown and remove docker containers:

docker compose down\n
"},{"location":"docs/development/","title":"Developer Guide","text":"

Welcome developers! This guide documents how to extend the autonomy stack for your own needs. The stack has been designed with modularity in mind, and aims to make it straight forward to swap out any component.

We assume you're developing first on a local machine with simulation.

"},{"location":"docs/development/contributing/","title":"Contributing","text":"

This page describes how to merge content back into main.

"},{"location":"docs/development/contributing/#dependencies","title":"Dependencies","text":"

Make sure to add your ROS2 package dependencies to your package.xml file. These get installed when the docker image is built.

If you need to add a dependency that's not in the docker image, please add a section to the Dockerfile in the docker/ directory.

"},{"location":"docs/development/contributing/#documentation","title":"Documentation","text":"

Please make sure to document your work. Docs are under AirStack/docs/. The navigation tree is under AirStack/mkdocs.yml.

This documentation is built with Material MKDocs. Visit mkdocs.org and mkdocs-material to learn how to use it.

"},{"location":"docs/development/contributing/#commands","title":"Commands","text":"

pip install mkdocs-material\nmkdocs serve\n
Launches docs on https://localhost:8000.

  • mkdocs -h - Print help message and exit.
"},{"location":"docs/development/contributing/#project-layout","title":"Project layout","text":"
mkdocs.yml    # The configuration file.\ndocs/\n    index.md  # The documentation homepage.\n    ...       # Other markdown pages, images and other files.\n
"},{"location":"docs/development/contributing/#merge","title":"Merge","text":"

Submit a pull request.

All tests must pass before merging.

Regression tests are run so that we don't break anything.

"},{"location":"docs/development/create_new_project/","title":"Create a New Project","text":""},{"location":"docs/development/create_new_project/#option-1-generate-from-template-no-further-updates-from-upstream","title":"Option 1: Generate from Template (no further updates from upstream)","text":"

The AirStack repository is setup as a Template on GitHub. This makes it easy to create a new project from AirStack with the \"Use this template\" button.

However, generating from a template squashes the entire git history into a single starter commit in your new repository. This prevents pulling in updates from the AirStack repository in the future.

To be able to pull upstream changes from the AirStack repository, use option 2.

"},{"location":"docs/development/create_new_project/#option-2-duplicate-for-future-updates-from-upstream","title":"Option 2: Duplicate (for future updates from upstream)","text":"

Duplicating a repository preserves the entire history of the repository, making it easier to pull in updates from AirStack in the future. Unlike creating a fork, duplicating a repository allows your new repository to be private.

See GitHub's instructions to duplicate a repository.

Sample commands are provided below: 

git clone --bare https://github.com/castacks/AirStack.git my-airstack\ncd my-airstack\ngit push --mirror https://github.com/EXAMPLE-USER/my-airstack.git\n

"},{"location":"docs/development/docker_usage/","title":"General Usage with Docker Compose","text":"

To mimic interacting with multiple real world robots, we use Docker Compose to manage Docker containers that isolate the simulation, each robot, and the ground control station.

The details of the docker compose setup is in AirStack/docker-compose.yaml.

In essence, the compose file launches:

  • Isaac Sim
  • ground control station
  • robots

all get created on the same default Docker bridge network. This lets them communicate with ROS2 on the same network.

Each robot has its own ROS_DOMAIN_ID.

"},{"location":"docs/development/docker_usage/#pull-images","title":"Pull Images","text":"

To use the AirLab docker registry:

cd AirStack/\ndocker login airlab-storage.andrew.cmu.edu:5001\n## <Enter your andrew id (without @andrew.cmu.edu)>\n## <Enter your andrew password>\n\n## Pull the images in the docker compose file\ndocker compose pull\n

Catelog: AirLab Registry Images.

Available image tags: airstack-dev, isaac-sim_ros-humble

"},{"location":"docs/development/docker_usage/#build-images","title":"Build Images","text":"
docker compose build\n
"},{"location":"docs/development/docker_usage/#start-stop-and-remove","title":"Start, Stop, and Remove","text":"

Start

docker compose up -d --scale robot=[NUM_ROBOTS]\n\n# see running containers\ndocker ps -a\n

Stop

docker compose stop\n

Remove

docker compose down\n
"},{"location":"docs/development/docker_usage/#isaac-sim","title":"Isaac Sim","text":"

Start a bash shell in the Isaac Sim container:

# if the isaac container is already running, execute a bash shell in it\ndocker exec -it isaac-sim bash\n# or if not, start a new container\ndocker compose run isaac-sim bash\n

Within the isaac-sim Docker container, the alias runapp launches Isaac Sim. The --path argument can be passed with a path to a .usd file to load a scene.

It can also be run in headless mode with ./runheadless.native.sh to stream to Omniverse Streaming Client or ./runheadless.webrtc.sh to stream to a web browser.

The container also has the isaacsim ROS2 package within that can be launched with ros2 launch isaacsim run_isaacsim.launch.py.

"},{"location":"docs/development/docker_usage/#robot","title":"Robot","text":"

Start a bash shell in a robot container, e.g. for robot_1:

docker exec -it airstack-robot-1 bash\n

The previous docker compose up launches robot_bringup in a tmux session. To attach to the session within the docker container, e.g. to inspect output, run tmux attach.

The following commands are available within the robot container:

# in robot docker\ncws  # cleans workspace\nbws  # builds workspace\nbws --packages-select [your_packages] # builds only desired packages\nsws  # sources workspace\nros2 launch robot_bringup robot.launch.xml  # top-level launch\n

These aliases are in AirStack/robot/.bashrc.

Each robot has ROS_DOMAIN_ID set to its ID number. ROBOT_NAME is set to robot_$ROS_DOMAIN_ID.

"},{"location":"docs/development/docker_usage/#ground-control-station","title":"Ground Control Station","text":"

Currently the ground control station uses the same image as the robot container. This may change in the future.

Start a bash shell in a robot container:

docker exec -it ground-control-station bash\n

The available aliases within the container are currently the same.

On the GCS ROS_DOMAIN_ID is set to 0.

"},{"location":"docs/development/docker_usage/#ssh-into-robots","title":"SSH into Robots","text":"

The containers mimic the robots' onboard computers on the same network. Therefore we intend to interface with the robots through ssh.

The ground-control-station and docker-robot-* containers are setup with ssh daemon, so you can ssh into the containers using the IP address.

You can get the IP address of each container by running the following command:

docker inspect -f '{{range.NetworkSettings.Networks}}{{.IPAddress}}{{end}}' [CONTAINER-NAME]\n

Then ssh in, for example:

ssh root@172.18.0.6\n

The ssh password is airstack.

"},{"location":"docs/development/docker_usage/#container-details","title":"Container Details","text":"
graph TD\n    A(Isaac Sim) <-- Sensors and Actuation --> B\n    A <-- Sensors and Actuation --> C\n    B(Robot 1) <-- Global Info --> D(Ground Control Station)\n    C(Robot 2) <-- Global Info --> D\n\n    style A fill:#76B900,stroke:#333,stroke-width:2px\n    style B fill:#fbb,stroke:#333,stroke-width:2px\n    style C fill:#fbb,stroke:#333,stroke-width:2px\n    style D fill:#fbf,stroke:#333,stroke-width:2px\n
"},{"location":"docs/development/frame_conventions/","title":"Frame conventions","text":"

Isaac Sim follows the Forward-Left-Up (FLU) coordinate system. This means that the X-axis points forward, the Y-axis points left, and the Z-axis points up. This is the same convention used in the Isaac SDK and Isaac Sim's parent platform, Omniverse. Isaac Docs

"},{"location":"docs/development/testing/","title":"Testing","text":""},{"location":"docs/development/testing/ci_cd/","title":"CI/CD Pipeline","text":""},{"location":"docs/development/testing/integration_testing/","title":"Integration Testing","text":""},{"location":"docs/development/testing/testing_frameworks/","title":"Testing Frameworks","text":""},{"location":"docs/development/vscode/","title":"VS Code: Docker Integration and Debugger Setup","text":"

Start containers 

# optionally pass the --scale robot=N argument to start N robots\ndc compose up -d  # --scale robot=2\n

Open AirStack folder

cd AirStack\ncode .\n

Install the \"Dev Containers\" extension.

Now click the \"Remote Explorer\" icon on the left side bar, hover over a robot container, and attach to the container.

Install recommended extensions within the image. This installs the ROS, C++, and Python extensions in the container.

"},{"location":"docs/development/vscode/#build-ros-workspace","title":"Build ROS Workspace","text":"

Hit Ctrl-Shift-B to build the project. This is a shortcut for bws --cmake-args '-DCMAKE_BUILD_TYPE=Debug', which adds debug symbols to the build.

Build tasks are defined in .vscode/tasks.json.

"},{"location":"docs/development/vscode/#launch","title":"Launch","text":"

Hit F5 to launch robot.launch.xml, or click the \"Run and Debug\" button on the left side of the screen and click the green play button.

Launch tasks are defined in .vscode/launch.json.

You can now set breakpoints, view variables, step-through code, and debug as usual in VSCode.

Warning about file permissions

Folders and files created within the attached docker container will be owned by root. This can cause issues when trying to edit files from the host machine, especially when using git to switch branches. If you accidentally create files as root, you can change the owner to your user with the following command: 

sudo chown -R $USER:$USER .\n

"},{"location":"docs/ground_control_station/","title":"Ground Control Station","text":"

The Ground Control Station (GCS) is for operators to monitor and control the robots.

"},{"location":"docs/real_world/","title":"Real World Overview","text":"

Fly robots in da wild.

That's wild.

"},{"location":"docs/real_world/installation/","title":"Installation on Hardware","text":""},{"location":"docs/robot/","title":"Robot","text":""},{"location":"docs/robot/#launch-structure","title":"Launch Structure","text":"

Each high-level module has a *_bringup package that contains the launch files for that module. The launch files are located in the launch directory of the *_bringup package. The launch files are named *.launch.(xml/yaml/py) and can be launched with ros2 launch <module_name>_bringup <module_name>.launch.(xml/yaml/py).

"},{"location":"docs/robot/common_topics/","title":"Common topics","text":"Topic Type Description /$ROBOT_NAME/odometry nav_msgs/Odometry Best estimate of robot odometry /$ROBOT_NAME/global_plan nav_msgs/Path Current target global trajectory for the robot to follow. See global planning for more details."},{"location":"docs/robot/common_topics/#system-diagram","title":"System Diagram","text":""},{"location":"docs/robot/autonomy/","title":"Autonomy Modules","text":""},{"location":"docs/robot/autonomy/#modules","title":"Modules","text":"
  • 0_interface
  • 1_sensors
  • 2_perception
  • 3_local
  • 4_global
  • 5_behavior
"},{"location":"docs/robot/autonomy/#system-diagram","title":"System Diagram","text":""},{"location":"docs/robot/autonomy/0_interface/","title":"Robot Interface","text":"

The interface defines the communication between the autonomy stack running on the onboard computer and the robot's control unit. For example, for drones it converts the control commands from the autonomy stack into MAVLink messages for the flight controller.

TODO: This is not our diagram, must replace.

The code is located under AirStack/ros_ws/src/robot/autonomy/0_interface/.

"},{"location":"docs/robot/autonomy/0_interface/#launch","title":"Launch","text":"

Launch files are under src/robot/autonomy/0_interface/interface_bringup/launch.

The main launch command is ros2 launch interface_bringup interface.launch.xml.

"},{"location":"docs/robot/autonomy/0_interface/#robotinterface","title":"RobotInterface","text":"

Package robot_interface is a ROS2 node that interfaces with the robot's hardware. The RobotInterface gets robot state and forwards it to the autonomy stack, and also translates control commands from the autonomy stack into the command for the underlying hardware. Note the base class is unimplemented. Specific implementations should extend class RobotInterface in robot_interface.hpp, for example class MAVROSInterface.

"},{"location":"docs/robot/autonomy/0_interface/#state","title":"State","text":"

The RobotInterface class broadcasts the robot's pose as a TF2 transform. It also publishes the robot's odometry as a nav_msgs/Odometry message to $(env ROBOT_NAME)/0_interface/robot_0_interface/odometry.

"},{"location":"docs/robot/autonomy/0_interface/#commands","title":"Commands","text":"

The commands are variations of the two main command modes: Attitude control and Position control. These are reflected in MAVLink and supported by both PX4 and Ardupilot.

The RobotInterface node subscribes to:

  • /$(env ROBOT_NAME)/interface/cmd_attitude_thrust of type mav_msgs/AttitudeThrust.msg
  • /$(env ROBOT_NAME)/interface/cmd_rate_thrust of type mav_msgs/RateThrust.msg
  • /$(env ROBOT_NAME)/interface/cmd_roll_pitch_yawrate_thrust of type mav_msgs/RollPitchYawrateThrust.msg
  • /$(env ROBOT_NAME)/interface/cmd_torque_thrust of type mav_msgs/TorqueThrust.msg
  • /$(env ROBOT_NAME)/interface/cmd_velocity of type geometry_msgs/TwistStamped.msg
  • /$(env ROBOT_NAME)/interface/cmd_position of type geometry_msgs/PoseStamped.msg

All messages are in the robot's body frame, except velocity and position which use the frame specified by the message header.

"},{"location":"docs/robot/autonomy/0_interface/#mavrosinterface","title":"MAVROSInterface","text":"

The available implementation in AirStack is called MAVROSInterface implemented in mavros_interface.cpp. It simply forwards the control commands to the Ascent flight controller (based on Ardupilot) using MAVROS.

"},{"location":"docs/robot/autonomy/0_interface/#custom-robot-interface","title":"Custom Robot Interface","text":"

If you're using a different robot control unit with its own custom API, then you need to create an associated RobotInterface. Implementations should do the following:

"},{"location":"docs/robot/autonomy/0_interface/#broadcast-state","title":"Broadcast State","text":"

Implementations of RobotInterface should obtain the robot's pose and broadcast it as a TF2 transform.

Should look something like:

// callback function triggered by some loop\nvoid your_callback_function(){\n    // ...\n    geometry_msgs::msg::TransformStamped t;\n    // populate the transform, e.g.:\n    t.header = // some header\n    t.transform.translation.x = // some value\n    t.transform.translation.y = // some value\n    t.transform.translation.z = // some value\n    t.transform.rotation = // some quaternion\n    // Send the transformation\n    this->tf_broadcaster_->sendTransform(t);\n    // ...\n}\n

TODO: our code doesn't currently do it like this, it instead uses an external odometry_conversion node.

"},{"location":"docs/robot/autonomy/0_interface/#override-command-handling","title":"Override Command Handling","text":"

Should override all virtual functions in robot_interface.hpp:

  • cmd_attitude_thrust_callback
  • cmd_rate_thrust_callback
  • cmd_roll_pitch_yawrate_thrust_callback
  • cmd_torque_thrust_callback
  • cmd_velocity_callback
  • cmd_position_callback
  • request_control
  • arm
  • disarm
  • is_armed
  • has_control
"},{"location":"docs/robot/autonomy/1_sensors/","title":"Index","text":"

We'll fill this with different things like the ZED-X package, LiDAR, etc

"},{"location":"docs/robot/autonomy/1_sensors/#launch","title":"Launch","text":"

Launch files are under src/robot/autonomy/sensors/sensors_bringup/launch.

The main launch command is ros2 launch sensors_bringup sensors.launch.xml.

"},{"location":"docs/robot/autonomy/2_perception/","title":"Perception","text":"

These modules process raw sensor data into useful information for the robot. For example: for detecting obstacles, localizing the robot, and recognizing objects.

Perception modules typically output topics in image space or point cloud space. This information then gets aggregated into global and local world models later in the pipeline.

Common perception modules include:

  • semantic segmentation
  • VIO (Visual Inertial Odometry)
"},{"location":"docs/robot/autonomy/2_perception/#launch","title":"Launch","text":"

Launch files are under src/robot/autonomy/perception/perception_bringup/launch.

The main launch command is ros2 launch perception_bringup perception.launch.xml.

"},{"location":"docs/robot/autonomy/2_perception/state_estimation/","title":"State estimation","text":"

Hey Yuheng your stuff goes here:)

"},{"location":"docs/robot/autonomy/3_local/","title":"Local Packages","text":"

The local module includes packages that are specific to the local autonomy of the robot. This includes local mapping, planning, and control.

"},{"location":"docs/robot/autonomy/3_local/#launch","title":"Launch","text":"

Launch files are under src/robot/autonomy/local/local_bringup/launch.

The main launch command is ros2 launch local_bringup local.launch.xml.

"},{"location":"docs/robot/autonomy/3_local/controls/","title":"Controls","text":"

Controls dictate the actuation of the robot. They are responsible for taking in sensor data and producing control commands.

The controller should publish control commands directly to topics defined by the Robot Interface.

Currently the AirStack uses a custom controller called \"Trajectory Controller\".

"},{"location":"docs/robot/autonomy/3_local/planning/","title":"Local Planning","text":"

Part of the local planner is the Waypoint Manager.

The Waypoint Manager subscribes to the global waypoints and the drone's current position and publishes the next waypoint to the local planner.

We plan for this baseline to be DROAN

"},{"location":"docs/robot/autonomy/3_local/world_model/","title":"Local World Model","text":""},{"location":"docs/robot/autonomy/4_global/","title":"Global Packages","text":"

The global packages include global world models and planners.

"},{"location":"docs/robot/autonomy/4_global/#launch","title":"Launch","text":"

Launch files are under src/robot/autonomy/global/global_bringup/launch.

The main launch command is ros2 launch global_bringup global.launch.xml.

"},{"location":"docs/robot/autonomy/4_global/planning/","title":"Planning","text":"

Global planners output a high level, coarse trajectory for the robot to follow.

A trajectory is a spatial path plus a schedule. This means each waypoint in the trajectory has a time associated with it, indicating when the robot should reach that waypoint. These timestamps are fed to the local planner and controller to determine velocity and acceleration.

If a waypoint's header timestamp is empty, the local planner should assume there's no time constraint and follow the trajectory at its own pace.

The global planner should make a trajectory that is collision-free according to the global map. However, avoiding fine obstacles is delegated to the local planner that operates at a faster rate.

For the structure of the package, the global planner node should not include any logic to generate the path. This should be located in a seperate logic class and be seperated from ROS. This will allow more modularity in the future for testing and easy interface changes.

We intend the global planners to be modular. AirStack implements a basic Random Walk planner as a baseline. Feel free to implement your own through the following interfaces.

"},{"location":"docs/robot/autonomy/4_global/planning/#ros-interfaces","title":"ROS Interfaces","text":"

Global planners are meant to be modules that can be swapped out easily. They can be thought of as different high level behaviors for the robot to follow. Consider that multiple global planners may be run in parallel, for example by some ensemble planner node that chooses the best plan for the current situation.

As such, the global planner should be implemented as a ROS2 node that accepts runtime mission parameters in a custom PlanRequest.msg and publishes a plan to its local ~/global_plan topic.

The best global plan should then be forwarded or remapped to /$(env ROBOT_NAME)/global_plan for the local planner to follow.

sequenceDiagram\n  autonumber\n  Global Manager->>Global Planner: ~/plan_request (your_planner/PlanRequest.msg)\n  loop Planning\n      Global Planner-->>Global Manager: heartbeat feedback\n  end\n  Global Planner->>Global Manager: ~/global_plan (nav_msgs/Path.msg)\n  Global Manager->>Local Planner: /$ROBOT_NAME/global_plan_reference (nav_msgs/Path.msg)\n  Local Planner->>Global Manager: /$ROBOT_NAME/global_plan_eta (nav_msgs/Path.msg)
"},{"location":"docs/robot/autonomy/4_global/planning/#subscribe-plan-request","title":"Subscribe: Plan Request","text":"

Your custom PlanRequest.msg defines the parameters that your global planner needs to generate a plan. It will be sent on the ~/plan_request topic.

Some common parameters may be the following: 

# PlanRequest.msg\nstd_msgs/Duration timeout  # maximum time to spend planning\ngeometry_msgs/Polygon bounds # boundary that the plan must stay within\n

"},{"location":"docs/robot/autonomy/4_global/planning/#publish-global-plan","title":"Publish: Global Plan","text":"

The global planner must publish a message of type nav_msgs/Path to ~/global_plan. The message defines high level waypoints to reach by a given time.

The nav_msgs/Path message type contains a header field and poses field.

  • The top level header of nav_msgs/Path message should contain the coordinate frame of the trajectory, and its timestamp should indicate when the trajectory was published.
  • Within the poses field, each geometry_msgs/PoseStamped's header should contain a timestamp that indicates when that waypoint should be reached
nav_msgs/Path.msg\n    - std_msgs/Header header\n        - time stamp: when the trajectory was generated\n        - frame_id: the coordinate frame of the trajectory\n    - geometry_msgs/PoseStamped[] poses: the trajectory\n        - geometry_msgs/PoseStamped pose\n            - std_msgs/Header header\n                - time stamp: when the waypoint should be reached\n                - string frame_id: the coordinate frame of the waypoint\n            - geometry_msgs/Pose pose: the position and orientation of the waypoint\n
"},{"location":"docs/robot/autonomy/4_global/planning/#publish-heartbeat","title":"Publish: Heartbeat","text":"

For long-running global planners, it's recommended to publish a heartbeat message to ~/heartbeat. This way the calling node can know that the global planner is still running and hasn't crashed.

"},{"location":"docs/robot/autonomy/4_global/planning/#additional-subscribers","title":"Additional Subscribers","text":"

In general, the global planner needs to access components of the world model such as the map and drone state.

The most common map is Occupancy Grids that is published by TODO node.

The global planner can also access the robot's current state and expected state in the future. For example, if the global planner takes 20 seconds to plan a trajectory, it can query where the robot expects to be in 20 seconds. This ROS2 service is available under TODO.

The global planner can do whatever it wants internally with this information.

"},{"location":"docs/robot/autonomy/4_global/planning/#example-planners","title":"Example Planners","text":""},{"location":"docs/robot/autonomy/4_global/planning/#random-walk-planner","title":"Random Walk planner","text":"

The random walk planner replans when the robot is getting close to the goal. The random walk planner is a trivial planner that generates a plan by randomly selecting a direction to move in. The random walk planner is useful for testing the robot's ability to follow a plan.

"},{"location":"docs/robot/autonomy/4_global/world_model/","title":"World Model","text":"

Global world models are responsible for maintaining a representation of the world that is used by the global planner to generate a plan. This representation is typically a map of the environment, but can also include other information such as the location of other robots, obstacles, and goals.

The current placeholder world model is a voxelized map representation called VDB Mapping.

"},{"location":"docs/robot/autonomy/5_behavior/","title":"Behavior","text":"

The behavior module is responsible for the high-level decision making of the robot. This includes deciding what actions to take based on the current state of the robot and the world around it. The behavior module is responsible for coordinating the actions of the local and global modules to achieve the robot's goals.

"},{"location":"docs/robot/autonomy/5_behavior/#launch","title":"Launch","text":"

Launch files are under src/robot/autonomy/behavior/behavior_bringup/launch.

The main launch command is ros2 launch behavior_bringup behavior.launch.xml.

"},{"location":"docs/robot/autonomy/5_behavior/behavior_executive/","title":"Behavior Executive","text":"

The behavior executive reads which actions are active from the behavior tree and implements the behavior which these actions should perform and sets the status of the actions to SUCCESS, RUNNING, or FAILURE. It also sets the status of conditions as either SUCCESS or FAILURE.

A typical way of implementing the behavior for an action is the following in the 20 Hz timer callback:

if(action->is_active()){\n  if(action->active_has_changed()){\n    // This is only true when the when the action transitions between active/inactive\n    // so this block of code will only run once whenever the action goes from being inactive to active.\n    // You might put a service call here and then call action->set_success() or action->set_failure()\n    // based on the result returned by the service call.\n  }\n\n  // Code here will get executed each iteration.\n  // You might call action->set_running() while you are doing work here.\n}\n
"},{"location":"docs/robot/autonomy/5_behavior/behavior_tree/","title":"Behavior Trees","text":"

De\ufb01nes how a task in terms of conditions and actions which the user implements.

Other types of nodes, control \ufb02ow and decorator nodes, control which conditions will be checked and which actions will be activated.

Nodes have statuses of either SUCCESS, RUNNING or FAILURE.

"},{"location":"docs/robot/autonomy/5_behavior/behavior_tree/#why-behavior-trees","title":"Why Behavior Trees?","text":"

Maintainable - Easy to modify

Scalable - Parts of sub-trees are modular and can be encapsulated

Reusable - Sub-trees can be reused in different places

Clear visualization and interpretation

"},{"location":"docs/robot/autonomy/5_behavior/behavior_tree/#types-of-nodes","title":"Types of Nodes","text":"
  • Execution Nodes
    • Condition Nodes
    • Action Nodes
  • Decorator Nodes
    • Not Node
  • Control Flow Nodes
    • Sequence Nodes
    • Fallback Nodes
"},{"location":"docs/robot/autonomy/5_behavior/behavior_tree/#execution-nodes-condition-nodes","title":"Execution Nodes - Condition Nodes","text":"

Condition nodes have a status of either SUCCESS or FAILURE

"},{"location":"docs/robot/autonomy/5_behavior/behavior_tree/#execution-nodes-action-nodes","title":"Execution Nodes - Action Nodes","text":"

Action nodes can either be active or inactive

An inactive node's status is not checked by the behavior tree, it is shown in white

below

An active node's status is checked, it can either be SUCCESS (green), RUNNING (blue) or FAILURE (red)

"},{"location":"docs/robot/autonomy/5_behavior/behavior_tree/#decorator-nodes-not-nodes","title":"Decorator Nodes - Not Nodes","text":"

The not node must have one condition node has a child and inverts the status of the child.

If the child's status is SUCCESS, the not node's status will be FAILURE.

If the child's status is FAILURE, the not node's status will be SUCCESS.

"},{"location":"docs/robot/autonomy/5_behavior/behavior_tree/#control-flow-nodes-fallback-nodes","title":"Control Flow Nodes - Fallback Nodes","text":"

These nodes are shown with a ?

This node returns FAILURE if and only if all of its children return FAILURE

If one of its children return RUNNING or SUCCESS, it returns RUNNING or SUCCESS and no subsequent children's statuses are check

Below shows a typical example, where an action will only be performed if all of the preceding conditions are false. In this case a drone will only be armed if it is not already armed, it is in o\ufb00board mode and it is stationary

"},{"location":"docs/robot/autonomy/5_behavior/behavior_tree/#control-flow-nodes-sequence-nodes","title":"Control Flow Nodes - Sequence Nodes","text":"

These nodes are shown with a \"->\"

This node returns SUCCESS if and only if all of its children return SUCCESS

If one of its children return RUNNING or FAILURE, it returns RUNNING or FAILURE and no subsequent children's statuses are check

Below shows a typical example where preceding conditions must be true in order for an action to be performed. In this case the drone will land if the IMU times out and it is in o\ufb00board mode

"},{"location":"docs/robot/logging/","title":"Logging","text":""},{"location":"docs/robot/static_transforms/","title":"Index","text":""},{"location":"docs/robot/static_transforms/#frame-conventions","title":"Frame Conventions","text":"

Each robot has its own map frame that represents the starting position of the robot. The map frame is expected to be in ENU (East-North-Up) convention.

The robot is in the base_link frame.

"},{"location":"docs/simulation/","title":"Simulation","text":"

We primarily support Isaac Sim. In the future we plan to support Gazebo.

"},{"location":"docs/simulation/docker_network/","title":"Docker network","text":""},{"location":"docs/simulation/docker_network/#overview","title":"Overview","text":"

The details of the docker containers setup is in the docker-compose.yaml file in the AirStack directory.

Isaac Sim, the ground control station, and robots all get created on the same default Docker bridge network. This lets them communicate with ROS2 on the same network.

Each robot has its own ROS_DOMAIN_ID.

"},{"location":"docs/simulation/docker_network/#ssh-into-robots","title":"SSH into Robots","text":"

The ground-control-station and docker-robot containers are setup with ssh daemon, so you can ssh into the containers using the IP address.

You can get the IP address of each container by running the following command:

docker inspect -f '{{range.NetworkSettings.Networks}}{{.IPAddress}}{{end}}' [CONTAINER-NAME]\n

Then ssh in, for example:

ssh root@172.18.0.6\n

The ssh password is airstack.

"},{"location":"docs/simulation/isaac_sim/","title":"Isaac Sim","text":"

The primary simulator we support is NVIDIA Isaac Sim. We chose Isaac Sim as the best balance between photorealism and physics simulation.

"},{"location":"docs/simulation/isaac_sim/#usd-file-naming-conventions","title":"USD File Naming Conventions","text":"

AirStack uses the following file naming conventions:

Purely 3D graphics

  • *.prop.usd \u27f5 simply a 3D model with materials, typically encompassing just a single object. Used for individual assets or objects (as mentioned earlier), representing reusable props.

  • *.stage.usd \u27f5 an environment composed of many props, but with no physics, no simulation, no robots. simply scene graphics

Simulation-ready

  • *.robot.usd \u27f5 a prop representing a robot plus ROS2 topic and TF publishers, physics, etc.

  • *.scene.usd \u27f5 an environment PLUS physics, simulation, or robots

"},{"location":"docs/simulation/isaac_sim/ascent_sitl_extension/","title":"AirLab AirStack Extension","text":"

The AirStack extension for IsaacSim does two main things. It creates an Ascent Omnigraph Node which runs the Ascent SITL and updates the position of a drone model in IsaacSim based on the SITL. It also creates a panel for listing, attaching to, and killing tmux sessions.

"},{"location":"docs/simulation/isaac_sim/ascent_sitl_extension/#ascent-omnigraph-node","title":"Ascent OmniGraph Node","text":"

The Ascent OmniGraph node takes as input a domain id, node namespace and drone prim. It runs the Ascent SITL, mavproxy, and mavros and takes care of keeping the SITL time synced with IsaacSim's time. Mavros is run using the inputted domain id and node namespace. The drone prim's position is set based off of the position of the drone in the SITL. The drone prim doesn't do collision and will pass through objects in the IsaacSim world.

The way the SITL is synced with IsaacSim is by running the SITL in gdb with a breakpoint on the functin that advances the SITL time. Every time this function is called, our code is run by injecting a library using the LD_PRELOAD trick. Our code runs a client socket that talks to a server socket running in the AirStack IsaacSim extension which tells it how long to sleep based off the current SITL and IsaacSim time.

The Ascent OmniGraph node is shown below:

"},{"location":"docs/simulation/isaac_sim/ascent_sitl_extension/#tmux-panel","title":"TMUX Panel","text":"

This is a panel for listing, attaching to, and killing any running TMUX sessions. The Ascent SITL, mavproxy, and mavros are run in a TMUX sesion, so this is mainly for debugging those and probably doesn't need to be interacted with by most users. A list of TMUX sessions is displayed in the panel. It doesn't auto refresh so you have to manually click the refresh button to display any changes in the list of sessions. For each session, there is an Attach button and a Kill button. The Attach button will bring up an xterm window with the TMUX session. The Kill button will kill the TMUX session.

The TMUX panel is shown below:

"},{"location":"docs/simulation/isaac_sim/export_stages_from_unreal/","title":"Export Unreal Engine to Isaac Sim","text":"

A robot needs a scene to interact in. A scene can be created in any 3D modeling program, though we have found it easiest to export stages from Unreal Engine. This document explains how to export an Unreal Engine environment to an Isaac Sim stage, then how to convert the stage to a physics-enabled scene.

"},{"location":"docs/simulation/isaac_sim/export_stages_from_unreal/#exporting-unreal-engine-environments-to-isaac-sim-stages","title":"Exporting Unreal Engine Environments to Isaac Sim Stages","text":"

Generally, Unreal Engine environments can be found on Epic Games' Fab Marketplace. For example, the Open World Demo Collection is a free collection of outdoor environments.

The below video explains how to export an Unreal Engine environment to an Isaac Sim stage.

You can save this file as [YOUR_ENVIRONMENT_NAME].stage.usd.

"},{"location":"docs/simulation/isaac_sim/export_stages_from_unreal/#export-tips","title":"Export Tips","text":"

Complexity: Generally, it works best to export levels that are designed for UE4.27 and below. The advanced rendering features from UE5, i.e. nanite and lumen, aren't compatible with Omniverse. If the level is optimized for the older UE4, then it's more compatible.

Omniverse doesn't perform well with large amounts of vegetation. Anything with complex vegetation takes a long long time to load. Procedural foliage doesn't export well either. Simple geometries like buildings and rocks work better.

That said you can still achieve photorealism by substituting complex geometries for high quality textures. Isaac seems to do fine with high quality textures.

Optimization: After exporting, edit the file with USD Composer and run the Scene Optimizer extension for faster performance. USD Composer can be installed via Omniverse Launcher.

Verify the Scale: The Omniverse exporter exports in centimeters, but Isaac Sim natively works in meters. For consistency, follow these steps to change the scene units to be meters.

To check the scale of the scene, you can add a cube in Isaac Sim and compare it to the exported scene. The cube is 1m x 1m x 1m.

"},{"location":"docs/simulation/isaac_sim/export_stages_from_unreal/#turn-a-stage-into-a-physics-enabled-scene","title":"Turn a Stage into a Physics-Enabled Scene","text":"

Adding physics to the stage is as simple as adding a Physics property with the \"Colliders Preset\", as described in the Isaac docs. Then save the scene as [YOUR_ENVIRONMENT_NAME].scene.usd to clarify that it's a physics-enabled scene.

You're now ready to add robots to the scene on the next page.

"},{"location":"docs/simulation/isaac_sim/scene_setup/","title":"AirStack Scene Setup","text":""},{"location":"docs/simulation/isaac_sim/scene_setup/#creating-a-new-scene-with-robots","title":"Creating a New Scene with Robots","text":"

The easiest way is to reference and copy an existing scene.

"},{"location":"docs/simulation/isaac_sim/scene_setup/#ros-publishers-through-omnigraph","title":"ROS Publishers Through OmniGraph","text":""},{"location":"docs/simulation/isaac_sim/scene_setup/#configure-robot-name-ros_domain_id-and-topic-namespaces","title":"Configure Robot Name, ROS_DOMAIN_ID, and Topic Namespaces","text":"

Under the Spirit drone prim is an ActionGraph component, which is an Omnigraph. This component is used to configure the ROS publishers for the robot. The ActionGraph component has the following fields to configure:

  • robot_name: The name of the robot. This is used as the top-level namespace for ROS topics.
  • domain_id: The ROS domain ID. This is used as the ROS_DOMAIN_ID for DDS networking.

The Omnigraph has subgraphs for each ROS publisher type. For example, TFs, Images, and PointClouds. The top-level robot_name and domain_id fields get fed into the subgraphs. The Topic Namespaces field should be set to the topic namespace in the subgraphs. This is used to namespace the ROS topics.

"},{"location":"docs/simulation/isaac_sim/scene_setup/#customizing-the-omnigraph","title":"Customizing the Omnigraph","text":"

Common pre-built graphs for ROS may be added through the top menu bar: Isaac Utils > Common OmniGraphs. This is helpful for creating various sensor publishers.

We recommend organizing your work into sub-graphs. Copy your omnigraph template them into the top-level Omnigraph component, named \"ActionGraph\". Connect the robot_name and domain_id fields to your workflow. Then, select all the nodes in your workflow, right-click, and create a subgraph.

"},{"location":"ground_control_station/installation/","title":"Index","text":"

Scripts to install on machine, todo. Maybe something with ansible? Bash scripts could work too.

"},{"location":"ground_control_station/ros_ws/","title":"Index","text":"

Eventually we will have some ground control station that enables commanding all robots simultaneously.

Envision a GUI like an RTS game, where you have a global 3D map you can move around, and can also see and control your \"units\" on the map. A minimap will help with understanding where you are. Envision the map being built up over time in 3D as your agents explore.

"},{"location":"ground_control_station/ros_ws/src/mission_manager/","title":"Mission Manager","text":"

This package handles the allocation of tasks for the multiple agents. It assigned agents to search or track. For search it divides the search space.

colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release\n

Debugging this node 

ros2 run --prefix 'gdb -ex run --args'  mission_manager mission_manager_node\n

ros2 launch mission_manager mission_manager_launch.py\nros2 run rviz2 rviz2\n
"},{"location":"ground_control_station/ros_ws/src/ros2tak_tools/","title":"TAK Tools ROS 2 Package","text":"

ros2tak_tools is a ROS 2 package designed for integrating TAK (Tactical Assault Kit) functionalities within ROS 2. It includes tools for publishing and subscribing to Cursor-On-Target (CoT) events, interfacing with TAK servers, and setting up search and rescue missions.

"},{"location":"ground_control_station/ros_ws/src/ros2tak_tools/#features","title":"Features","text":"
  • ROS 2 to TAK Communication: Send ROS 2 messages to TAK using CoT events.
  • CoT to ROS 2 Communication: Receive CoT events from TAK and publish as ROS 2 messages.
  • Mission Planning: Custom tools to create and manage search missions using CoT and ROS.
"},{"location":"robot/installation/","title":"Index","text":"

Scripts to install on machine, todo. Maybe something with ansible? Bash scripts could work too.

"},{"location":"robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/","title":"README","text":"

This package generates a world representation using disparity images. This enables planning in image space by applying C-space expansion in 2.5D disparity images.

It generates a pair expansion images: one for foreground expansion and one for background expansion.

Currently this package also has the following nodes

  • disparity_expansion: Generate expanded disparity

  • disparity_pcd: Disparity image to point cloud.

"},{"location":"robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/#who-do-i-talk-to","title":"Who do I talk to?","text":"
  • geetesh dubey (geeteshdubey@gmail.com)
"},{"location":"robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/#license","title":"License","text":"

BSD, see LICENSE

"},{"location":"robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/","title":"Index","text":""},{"location":"robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/#takeofflandingplanner","title":"TakeoffLandingPlanner","text":"

Author:

"},{"location":"robot/ros_ws/src/autonomy/3_local/c_controls/mav_comm/","title":"mav_comm","text":"

This repository contains message and service definitions used for mavs. All future message definitions go in here, existing ones in other stacks should be moved here where possible.

"},{"location":"robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/","title":"px4_msgs","text":"

ROS 2 message definitions for the PX4 Autopilot project.

Building this package generates all the required interfaces to interface ROS 2 nodes with the PX4 internals.

"},{"location":"robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/#supported-versions-and-compatibility","title":"Supported versions and compatibility","text":"

Depending on the PX4 and ROS versions you want to use, you need to checkout the appropriate branch of this package:

PX4 ROS 2 Ubuntu branch v1.13 Foxy Ubuntu 20.04 release/1.13 v1.14 Foxy Ubuntu 20.04 release/1.14 v1.14 Humble Ubuntu 22.04 release/1.14 v1.14 Rolling Ubuntu 22.04 release/1.14 main Foxy Ubuntu 22.04 main main Humble Ubuntu 22.04 main main Rolling Ubuntu 22.04 main"},{"location":"robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/#messages-sync-from-px4","title":"Messages Sync from PX4","text":"

When PX4 message definitions in the main branch of PX4 Autopilot change, a CI/CD pipeline automatically copies and pushes updated ROS message definitions to this repository. This ensures that this repository main branch and the PX4-Autopilot main branch are always up to date. However, if you are using a custom PX4 version and you modified existing messages or created new one, then you have to manually synchronize them in this repository:

"},{"location":"robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/#manual-message-sync","title":"Manual Message Sync","text":"
  • Checkout the correct branch associated to the PX4 version from which you detached you custom version.
  • Delete all *.msg files in msg/ and copy all *.msg files from PX4-Autopilot/msg/ in it. Assuming that this repository and the PX4-Autopilot repository are placed in your home folder, you can run: 
    rm -f ~/px4_msgs/msg/*.msg\ncp ~/PX4-Autopilot/msg/*.msg ~/px4_msgs/msg/\n
"},{"location":"robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/#install-build-and-usage","title":"Install, build and usage","text":"

Check Using colcon to build packages to understand how this can be built inside a workspace. Check the PX4 ROS 2 User Guide section on the PX4 documentation for further details on how this integrates PX4 and how to exchange messages with the autopilot.

"},{"location":"robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/#bug-tracking-and-feature-requests","title":"Bug tracking and feature requests","text":"

Use the Issues section to create a new issue. Report your issue or feature request here.

"},{"location":"robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/#questions-and-troubleshooting","title":"Questions and troubleshooting","text":"

Reach the PX4 development team on the PX4 Discord Server.

"},{"location":"robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/CONTRIBUTING/","title":"Contributing","text":"

Do not commit changes directly to this repository that change the message definitions. All the message definitions are directly generated from the uORB msg definitions on the PX4 Firmware repository. Any fixes or improvements one finds suitable to apply to the message definitions should be directly done on the uORB message files. The deployment of these are taken care by a Jenkins CI/CD stage.

"},{"location":"robot/ros_ws/src/autonomy/3_local/c_controls/px4_msgs/CONTRIBUTING/#contributing-to-the-px4-firmware-repository-or-to-this-repository-not-including-message-definitions","title":"Contributing to the PX4 Firmware repository (or to this repository, not including message definitions)","text":"

Follow the Contributing guide from the PX4 Firmware repo.

"},{"location":"robot/ros_ws/src/autonomy/3_local/c_controls/trajectory_controller/","title":"Trajectory Controller","text":""},{"location":"robot/ros_ws/src/autonomy/3_local/c_controls/trajectory_controller/#overview","title":"Overview","text":"

The trajectory controller takes in a trajectory and publishes a tracking point, which a controller should make the drone fly to, and a look ahead point, which a planner should plan from. The trajectory controller can interpret a trajectory as a standalone complete track, like a figure eight or racetrack pattern for tuning controls, or as separate segments that should be stitched together, for example trajectories output by a local planner.

The trajectory controller tries to keep the tracking point ahead of the robot in a pure pursuit fashion. The robot's position, the red X in the figure below, is projected onto the trajectory. A sphere, shown in 2d as the cyan circle, is placed around this projected point and the intersection between the sphere and the forward point on the trajectory is used as the tracking point, the blue X. The lookahead point, the orange X, is a fixed time duration along the trajectory.

"},{"location":"robot/ros_ws/src/autonomy/3_local/c_controls/trajectory_controller/#parameters","title":"Parameters","text":"Parameter Description tf_prefix The tf names published are prefixed with the string in this parameter. Tfs are published at the tracking point and lookahead point. There are stabilized versions, the same but with zero pitch and roll, of these two tfs that are also published. target_frame The tracking point and lookahead point are published in this frame. look_ahead_time How far ahead of the tracking point the lookahead point will be in seconds. min_virtual_tracking_velocity If the velocity on the trajectory is less than this, the tracking point will just move forward in time instead of using the sphere to keep the tracking point ahead of the drone. The units are m/s. sphere_radius This is the radius of the sphere used to determine the position of the tracking point. Making it larger pushes the tracking point farther ahead. search_ahead_factor To search for the point on the trajectory that intersects with the sphere, the algorithm checks a certain distance ahead along the trajectory. This distance is given by sphere_radius * search_ahead_factor. If the trajectory zigzags a lot relative to the size of the sphere, it's possible that the algorithm wouldn't iterate far enough along the trajectory to find the point where it intersects with the sphere. If a large value for this parameter is used and the trajectory loops back on itself, it is possible that this would cause the tracking point to jump ahead and skip a portion of the trajectory. In almost all cases, this parameter shouldn't need to be changed. traj_vis_thickness The thickness of the trajectory visualization markers. ## Services"},{"location":"robot/ros_ws/src/autonomy/3_local/c_controls/trajectory_controller/#trajectory-modes","title":"Trajectory Modes","text":"

There are several modes that the trajectory controller can be placed in with a service call to the set_trajectory_mode service: See TrajectoryMode.srv.

Mode Description TRACK This interprets a trajectory subscribed on ~/trajectory_override as a complete trajectory that the controller will follow. It is usually used for taking off, landing, and tuning controls on fixed trajectories like figure eights, racetracks, circles, etc... ADD_SEGMENT This interprets a trajectory subscribed on ~/trajectory_segment_to_add as a segment of a trajectory which will get stitched onto the current trajectory at the closest point to the start of the new segment. This is usually published by a local planner. Ideally it is published at the location of the lookahead point, which is a fixed time ahead of the tracking point. This fixed time should be greater than the time it takes to plan. For example, if the lookahead point is one second ahead of the tracking point, the local planner should be always take less than one second to plan otherwise the tracking point would already be past the start of the plan. If this happens, the trajectory will fail to be stitched and will be ignored. PAUSE This causes the tracking point to stop where it is. REWIND This makes the tracking point go backwards along the trajectory. This mode is usually used to make the drone blindly backtrack along its trajectory to get it out of a situation it is stuck in. ROBOT_POSE This makes the tracking point and lookahead point always be at the same position as the drone's odometry. This is useful for before takeoff, when the robot may be carried around so that the location where the takeoff starts is at the drone's position."},{"location":"robot/ros_ws/src/autonomy/3_local/c_controls/trajectory_controller/#subscriptions","title":"Subscriptions","text":"Topic Type Description ~/odometry nav_msgs/Odometry Odometry of the robot. ~/trajectory_segment_to_add airstack_msgs/TrajectoryXYZVYaw For ADD_SEGMENT mode, this is the trajectory segment to add to the current trajectory. ~/trajectory_override airstack_msgs/TrajectoryXYZVYaw For TRACK mode, this overrides the current trajectory and makes the robot follow this directly."},{"location":"robot/ros_ws/src/autonomy/3_local/c_controls/trajectory_controller/#publications","title":"Publications","text":"Topic Type Description ~/tracking_point TODO TODO ~/look_ahead TODO TODO ~/traj_drone_point TODO TODO ~/virtual_tracking_point TODO TODO ~/closest_point TODO TODO ~/trajectory_completion_percentage TODO TODO ~/trajectory_time TODO TODO ~/tracking_error TODO TODO ~/velocity_pub TODO TODO ~/debug_markers TODO TODO ~/trajectory_vis TODO TODO"},{"location":"robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/","title":"VDB Mapping Core Library","text":"

DISCLAIMER: This library is still under development. Be warned that some interfaces will be changed and/or extended in the future.

The VDB Mapping core library was primarily developed to be used in combination with the corresponding ROS wrapper or ROS2 wrapper

"},{"location":"robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/#getting-started","title":"Getting Started","text":""},{"location":"robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/#requirements","title":"Requirements","text":"

This library requires OpenVDB as it is build around it. This library was initially developed using Version 5.0 and should work with all versions above.

As the apt packages are quite outdated for most systems, we recommend building at least OpenVDB v9.0.0 from source using the provided build instructions

"},{"location":"robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/#build-instructions","title":"Build instructions","text":"

The library can be either used as plain c++ library or in combination with the afore mentioned ROS wrapper.

"},{"location":"robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/#dependencies","title":"Dependencies","text":"

The library requires the following dependencies to build correctly 

apt-get install -y libeigen3-dev\napt-get install -y libtbb-dev\napt-get install -y libpcl-dev\napt-get install -y libilmbase-dev\n

"},{"location":"robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/#plain-cmake","title":"Plain cmake","text":"

To build this package as a standalone library, follow the usual cmake building steps: 

git clone https://github.com/fzi-forschungszentrum-informatik/vdb_mapping\ncd vdb_mapping\nmkdir build && cd build\ncmake ..\nmake -j8\nmake install\n

"},{"location":"robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/#ros-workspace","title":"ROS Workspace","text":"

In case you want build this library inside of a ROS workspace in combination with VDB Mapping ROS, you cannot use catkin_make since this library is not a catkin package. Instead you have to use catkin build or catkin_make_isolated to build the workspace.

# source global ros\nsource /opt/ros/<your_ros_version>/setup.{zsh/bash}\n\n# create a catkin workspace\nmkdir -p ~/catkin_ws/src && cd catkin_ws\n\n# clone packages\ngit clone https://github.com/fzi-forschungszentrum-informatik/vdb_mapping\n\n# install dependencies\nsudo apt update\nrosdep update\nrosdep install --from-paths src --ignore-src -y\n\n# build the workspace.\ncatkin build\n\n# source the workspace\nsource deve/setup.bash\n
"},{"location":"robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/#ros2-workspace","title":"ROS2 Workspace","text":"
# source global ros\nsource /opt/ros/<your_ros_version>/setup.{zsh/bash}\n\n# create a catkin workspace\nmkdir -p ~/colcon_ws/src && cd ~/colcon_ws/src\n\n# clone packages\ngit clone https://github.com/fzi-forschungszentrum-informatik/vdb_mapping\ngit clone https://github.com/fzi-forschungszentrum-informatik/vdb_mapping_ros2\n\n# install dependencies\nsudo apt update\nrosdep update\nrosdep install --from-paths src --ignore-src -y\n\n# build the workspace.  \ncolcon build\n\n# source the workspace\nsource install/setup.bash\n
"},{"location":"robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/#citation","title":"Citation","text":"

Thanks that you read until here and please let us know if you run into any issues or have suggestions for improvements. If you use our work in your publications please feel free to cite our manuscript: 

  @inproceedings{besselmann2021vdb,\n  title={VDB-Mapping: a high resolution and real-time capable 3D mapping framework for versatile mobile robots},\n  author={Besselmann, M Grosse and Puck, Lennart and Steffen, Lea and Roennau, Arne and Dillmann, R{\\\"u}diger},\n  booktitle={2021 IEEE 17th International Conference on Automation Science and Engineering (CASE)},\n  pages={448--454},\n  year={2021},\n  organization={IEEE}\n  doi={10.1109/CASE49439.2021.9551430}}\n}\n
or for the remote mapping case: 
@incollection{besselmann2022remote,\n  title={Remote VDB-Mapping: A Level-Based Data Reduction Framework for Distributed Mapping},\n  author={Besselmann, Marvin Grosse and R{\\\"o}nnau, Arne and Dillmann, R{\\\"u}diger},\n  booktitle={Robotics in Natural Settings: CLAWAR 2022},\n  pages={448--459},\n  year={2022},\n  publisher={Springer}\n  doi={10.1007/978-3-031-15226-9_42}\n}\n

"},{"location":"robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping/#acknowledgement","title":"Acknowledgement","text":"

The research leading to this package has received funding from the German Federal Ministry of Education and Research under grant agreement No. 13N14679:

ROBDEKON - Robotic systems for decontamination in hazardous environments More information: robdekon.de

"},{"location":"robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/","title":"VDB Mapping ROS2 Package","text":"

DISCLAIMER: This library is still under development. Be warned that some interfaces will be changed and/or extended in the future.

The VDB Mapping ROS2 Package is a ROS2 wrapper around VDB Mapping

"},{"location":"robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/#getting-started","title":"Getting Started","text":""},{"location":"robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/#requirements","title":"Requirements","text":"

This library requires OpenVDB as it is build around it. This library was initially developed using Version 5.0 and should work with all versions above. Either use the apt package which will be automatically installed via rosdep or compile the package from source using the provided build instructions

"},{"location":"robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/#build-instructions","title":"Build instructions","text":"

Since the required VDB Mapping library is a plain C++ package, you cannot use catkin_make directly. Instead you have to use colcon build to build the workspace.

# source global ros\nsource /opt/ros/<your_ros_version>/setup.bash\n\n# create a catkin workspace\nmkdir -p ~/colcon_ws/src && cd ~/colcon_ws/src\n\n# clone packages\ngit clone https://github.com/fzi-forschungszentrum-informatik/vdb_mapping\ngit clone https://github.com/fzi-forschungszentrum-informatik/vdb_mapping_ros2\n\n# install dependencies\nsudo apt update\nrosdep update\nrosdep install --from-paths src --ignore-src -y\n\n# build the workspace.  \ncolcon build\n\n# source the workspace\nsource install/setup.bash\n
"},{"location":"robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/#remote-mapping","title":"Remote Mapping","text":"

The remote mapping module is at the moment mainly designed to provide an external access to the map, without saving and loading it in a different node. In our case we use it to visualize the map data on a remote base station which is then used to control the robots from far away and give the user an overview of the area the robot operates in. Basically it is a full additional mapping node which can access the data chunks generated by the main mapping on the robot. These can the be integrated into the remote mapping instance to generate the same or a similar map depending on the chosen parameter settings. Diving deeper into the remote mapping functionality you might stumble over the term of updates, overwrites and sections. These are different mechanics to update a remote mapping instance. The first two are just a byproduct of the normal mapping process. Therefore there is no additional computation needed to generate the data for a remote mapping instance. However all three mechanics use OpenVDB grids which are compressed and serialized as a bitstream before sending them out over the network.

"},{"location":"robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/#updates","title":"Updates","text":"

Updates are grids generated during the raycasting process and are also used for the internal map update. Here all sensor data is previously accumulated into an update grid which specifies whether the occupancy probability in the map should be updated as occupied or free. This two step process helps to prevent discretization issues due to the larger resolution of the grid compared to the raw sensor data. It is also possible to accumulate multiple sensor sources and measurements into one update grid (see accumulation parameters). The update grid can then be applied to the local or remote instance to generate the same map on both sides.

"},{"location":"robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/#overwrites","title":"Overwrites","text":"

Overwrite grids are similar to the update grid but only contain the voxel that have changed their occupancy state in the last iteration. Therefore they are more lightweight in terms of necessary bandwith. This can be useful in cases where the network is rather limited but has the downside, that the probabilistic framework is lost on the remote side.

"},{"location":"robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/#sections","title":"Sections","text":"

In contrast to updates and overwrites, sections are not bound to the sensor data rate but are generated with a defined rate and create additional processing load. Here the binary occupancy state of a section around a specified frame is copied out of the map and transformed into an update grid that can be applied to a remote instance. Although this created additional overhead it is still highly useful in certain scenarios. In our case we had to handle transmission delay and package loss where both the update and overwrites would result in incomplete maps since grids were lost. Sections on the other hand contain the complete chunk of the map while still being small with respect to the bandwidth.

"},{"location":"robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/#ros-api","title":"ROS API","text":""},{"location":"robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/#ros-parameters","title":"ROS Parameters","text":"

VDB Mapping is highly configurable using ROS parameters. Below is a complete list of all available parameters. Internally we store them in a yaml configuration file. An example can be found here.

"},{"location":"robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/#basic-parameters","title":"Basic Parameters","text":"

Listed below are the general parameters to configure the basic behavior of vdb_mapping

Parameter Name Type Default Information map_frame string ' ' Coordinate frame of the map robot_frame string ' ' Coordinate frame of the robot max_range double 15.0 Global maximum raycasting range (can also be set for each sensor source individually) resolution double 0.07 Map resolution prob_hit double 0.7 Probability update if a beam hits a voxel prob_miss double 0.4 Probability update if a beam misses a voxel prob_thres_min double 0.12 Lower occupancy threshold of a voxel prob_thres_max double 0.97 Upper occupancy threshold of a voxel map_save_dir string ' ' Storage location for saved maps accumulate_updates boolean false Specifies whether the data of multiple sensor measurement should be accumulated before integrating it into the map. accumulation_period double 1 Specifies how long updates should be accumulated before integration visualization_rate double 1 Specifies in which rate the visualization is published publish_pointcloud boolean true Specifies whether the map should be published as pointcloud publish_vis_marker boolean true Specifies whether the map should be published as visual marker apply_raw_sensor_data boolean true Specify whether raw sensor data (e.g. Pointclouds) should be integrated into the map. This flag becomes relevant when the user wants to use a pure remote instance. If set to true all sensor sources will be periodically integrated into the map. sources string[] [] List of sensor sources remote_sources string[] [] List of remote sources"},{"location":"robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/#sensor-sources","title":"Sensor Sources","text":"

VDB Mapping is able to integrate an arbitrary amount of different sensor sources (given they are available as PointCloud2 msg). To add a new sensor source the user has to add it to the list of sources in the basic parameters. Below are listed the parameters that can be used to configure the individual sensor source. These have to be put into the corresponding namespace in the yaml file. For an example see this configuration file.

Parameter Name Type Default Information topic string ' ' Topic name of the sensor msg sensor_origin_frame string ' ' Sensor frame of the measurement. This parameter is optional and in general the frame id of the msg is used. However in some cases like already frame aligned point clouds this info is no longer the origin of the sensor. For this purpose the user can specify here from which frame the raycasting should be performed. max_range double 0 Individual per sensor max raycasting range. This parameter is optional and per default the global max range is used."},{"location":"robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/#remote-sources","title":"Remote Sources","text":"

VDB Mapping provides a remote operation mode. Here the map data is shared between multiple instances to generate the same or similar maps (depending on the chosen parameter settings). For this remote sources can be specified over the parameter server.

"},{"location":"robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/#local-side","title":"Local side","text":"Parameter Name Type Default Information publish_updates boolean false Specifies whether updates are published publish_overwrites boolean false Specifies whether overwrites are published publish_sections boolean false Specifies whether sections are published section_update/rate double 1 Rate in which the section update is published section_update/frame string robot_frame Center of the update section section_update/min_coord/{x,y,z} double -10 Min coordinate of the section bounding box centered around the section frame section_update/max_coord/{x,y,z} double 10 Max coordinate of the section bounding box centered around the section frame"},{"location":"robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/#remote-side","title":"Remote side","text":"

Below are listed the parameters that can be used to configure the individual remote source. These have to be put into the corresponding namespace in the yaml file. An example config file can be found here.

Parameter Name Type Default Information namespace string ' ' Namespace of the remote mapping instance apply_remote_updates boolean false Specifies whether remote updates should be applied to the map apply_remote_overwrites boolean false Specifies whether remote overwrites should be applied to the map apply_remote_sections boolean false Specifies whether remote sections should be applied to the map"},{"location":"robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/#advertised-ros-topics","title":"Advertised ROS Topics","text":"Topic Name Type Information ~/vdb_map_visualization visualization_msgs/Marker Map visualization topic as voxel marker ~/vdb_map_pointcloud sensor_msgs/PointCloud2 Map visualization topic as pointcloud ~/vdb_map_updates vdb_mapping_msgs/UpdateGrid Map updates topic ~/vdb_map_overwrites vdb_mapping_msgs/UpdateGrid Map overwrites topic ~/vdb_map_sections vdb_mapping_msgs/UpdateGrid Map sections topic"},{"location":"robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/#subscribed-ros-topics","title":"Subscribed ROS Topics","text":"Topic Name Type Information {Parameter:sensor_source}/{Parameter:topic} sensor_msgs/PointCloud2 Pointcloud sensor message {Parameter:remote_source}/vdb_map_updates vdb_mapping_msgs/UpdateGrid Map updates topic {Parameter:remote_source}/vdb_map_overwrites vdb_mapping_msgs/UpdateGrid Map overwrites topic {Parameter:remote_source}/vdb_map_sections vdb_mapping_msgs/UpdateGrid Map sections topic"},{"location":"robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/#ros-services","title":"ROS Services","text":"Service Name Type Information ~/load_map vdb_mapping_msgs/LoadMap Loads the map specified in the msg ~/save_map std_srvs/Trigger Saves the current map in the destination specified in the map_save_dir parameter ~/reset_map std_srvs/Trigger Resets the current map ~/raytrace vdb_mapping_msgs/Raytrace Raytraces a point and returns coordinate where the ray first intersected the map ~/trigger_map_section_update vdb_mapping_msgs/TriggerMapSectionUpdate Triggers a map section update on a remote instance"},{"location":"robot/ros_ws/src/autonomy/4_global/a_world_models/vdb_mapping_ros2/#citation","title":"Citation","text":"

Thanks that you read until here and please let us know if you run into any issues or have suggestions for improvements. If you use our work in your publications please feel free to cite our manuscript: 

  @inproceedings{besselmann2021vdb,\n  title={VDB-Mapping: a high resolution and real-time capable 3D mapping framework for versatile mobile robots},\n  author={Besselmann, M Grosse and Puck, Lennart and Steffen, Lea and Roennau, Arne and Dillmann, R{\\\"u}diger},\n  booktitle={2021 IEEE 17th International Conference on Automation Science and Engineering (CASE)},\n  pages={448--454},\n  year={2021},\n  organization={IEEE}\n  doi={10.1109/CASE49439.2021.9551430}}\n}\n
or for the remote mapping case: 
@incollection{besselmann2022remote,\n  title={Remote VDB-Mapping: A Level-Based Data Reduction Framework for Distributed Mapping},\n  author={Besselmann, Marvin Grosse and R{\\\"o}nnau, Arne and Dillmann, R{\\\"u}diger},\n  booktitle={Robotics in Natural Settings: CLAWAR 2022},\n  pages={448--459},\n  year={2022},\n  publisher={Springer}\n  doi={10.1007/978-3-031-15226-9_42}\n}\n

"},{"location":"robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/","title":"Random Walk Global Planner Baseline","text":""},{"location":"robot/ros_ws/src/autonomy/5_behavior/rqt_fixed_trajectory_generator/","title":"RQT Python FixedTrajectoryGenerator","text":"

If you colcon build this package in a workspace and then run \"rqt --force-discover\" after sourcing the workspace, the plugin should show up as \"Fixed Trajectory Generator\" in \"Miscellaneous Tools\" in the \"Plugins\" menu.

You can use the generate_rqt_py_package.sh script to generate a new package by doing the following from the rqt_fixed_trajectory_generator directory

./generate_rqt_py_package.sh [package name] [class name] [plugin title]\n

[package name] will be the name of the package and a directory with this name will be created above rqt_fixed_trajectory_generator/. [class name] is the name of the class in src/[package name]/template.py. [plugin title] is what the plugin will be called in the \"Miscellaneous Tools\" menu.

For example,

cd rqt_fixed_trajectory_generator/\n./generate_rqt_py_package.sh new_rqt_package ClassName \"Plugin Title\"\n
"},{"location":"robot/ros_ws/src/autonomy/5_behavior/rqt_py_template/","title":"RQT Python Template","text":"

If you colcon build this package in a workspace and then run \"rqt --force-discover\" after sourcing the workspace, the plugin should show up as \"PluginTitle\" in \"Miscellaneous Tools\" in the \"Plugins\" menu.

You can use the generate_rqt_py_package.sh script to generate a new package by doing the following from the rqt_py_template directory

./generate_rqt_py_package.sh [package name] [class name] [plugin title]\n

[package name] will be the name of the package and a directory with this name will be created above rqt_py_template/. [class name] is the name of the class in src/[package name]/template.py. [plugin title] is what the plugin will be called in the \"Miscellaneous Tools\" menu.

For example,

cd rqt_py_template/\n./generate_rqt_py_package.sh new_rqt_package ClassName \"Plugin Title\"\n
"},{"location":"simulation/gazebo/","title":"Index","text":"

In the future we could support Gazebo.

"},{"location":"simulation/isaac-sim/sitl_integration/docs/","title":"Usage","text":"

To enable this extension, run Isaac Sim with the flags --ext-folder {path_to_ext_folder} --enable {ext_directory_name}

"},{"location":"simulation/isaac-sim/sitl_integration/docs/CHANGELOG/","title":"Changelog","text":""},{"location":"simulation/isaac-sim/sitl_integration/docs/CHANGELOG/#010-2024-07-10","title":"[0.1.0] - 2024-07-10","text":""},{"location":"simulation/isaac-sim/sitl_integration/docs/CHANGELOG/#added","title":"Added","text":"
  • Initial version of TEST_EXTENSION_TITLE Extension
"},{"location":"simulation/isaac-sim/sitl_integration/drag_and_drop/","title":"Index","text":"

Download the AscentAeroSystemsSITLPackage.zip from Google Drive and unzip into this directory.

"}]} \ No newline at end of file diff --git a/simulation/gazebo/index.html b/simulation/gazebo/index.html new file mode 100644 index 00000000..634a77bc --- /dev/null +++ b/simulation/gazebo/index.html @@ -0,0 +1,2600 @@ + + + + + + + + + + + + + + + + + + + Index - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

Index

+ +

In the future we could support Gazebo.

+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ +
+ + + +
+
+
+ +
+ Copyright © 2024 - 2030 AirLab CMU +
+ + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + +
+ +
+
+
+ +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/simulation/isaac-sim/assets/scenes/simple_tree_one_drone.scene.usd b/simulation/isaac-sim/assets/scenes/simple_tree_one_drone.scene.usd new file mode 100644 index 00000000..91385f46 Binary files /dev/null and b/simulation/isaac-sim/assets/scenes/simple_tree_one_drone.scene.usd differ diff --git a/simulation/isaac-sim/docker/Dockerfile.isaac-ros b/simulation/isaac-sim/docker/Dockerfile.isaac-ros new file mode 100644 index 00000000..2a87aa2c --- /dev/null +++ b/simulation/isaac-sim/docker/Dockerfile.isaac-ros @@ -0,0 +1,77 @@ +# expects context to be the root of the repository, i.e. AirStack/. this is so we can access AirStack/ros_ws/ +FROM nvcr.io/nvidia/isaac-sim:4.1.0 + +WORKDIR /isaac-sim + +# setup timezone +RUN echo 'Etc/UTC' > /etc/timezone && \ + ln -s /usr/share/zoneinfo/Etc/UTC /etc/localtime && \ + apt-get update && \ + apt-get install -q -y --no-install-recommends tzdata && \ + rm -rf /var/lib/apt/lists/* + +# install packages +RUN apt-get update && apt-get install -q -y --no-install-recommends \ + dirmngr \ + gnupg2 \ + unzip \ + tmux \ + && rm -rf /var/lib/apt/lists/* + +# setup keys +RUN set -eux; \ + key='C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654'; \ + export GNUPGHOME="$(mktemp -d)"; \ + gpg --batch --keyserver keyserver.ubuntu.com --recv-keys "$key"; \ + mkdir -p /usr/share/keyrings; \ + gpg --batch --export "$key" > /usr/share/keyrings/ros2-latest-archive-keyring.gpg; \ + gpgconf --kill all; \ + rm -rf "$GNUPGHOME" + +# setup sources.list +RUN echo "deb [ signed-by=/usr/share/keyrings/ros2-latest-archive-keyring.gpg ] http://packages.ros.org/ros2/ubuntu jammy main" > /etc/apt/sources.list.d/ros2-latest.list + +# setup environment +ENV LANG=C.UTF-8 +ENV LC_ALL=C.UTF-8 +ENV ROS_DISTRO=humble + +# Install ROS2 packages +RUN apt update && apt install -y --no-install-recommends curl emacs vim nano tmux gdb xterm \ + cmake \ + git \ + ros-humble-desktop \ + ros-dev-tools \ + python3-pip \ + python3-rosdep \ + ros-humble-tf2* \ + ros-humble-mavros + +RUN /opt/ros/humble/lib/mavros/install_geographiclib_datasets.sh + +RUN /isaac-sim/python.sh -m pip install git+https://github.com/dronekit/dronekit-python#egg=dronekit +RUN pip install PyYAML mavproxy tmuxp scipy + +# Install Isaac Sim ROS2 workspace so that we can run the isaacsim ROS package +RUN cd /tmp/ && \ + curl -L -O https://github.com/isaac-sim/IsaacSim-ros_workspaces/archive/main.zip && \ + unzip main.zip && \ + mv IsaacSim-ros_workspaces-main/humble_ws /humble_ws && \ + cd /humble_ws && \ + . /opt/ros/humble/setup.sh && \ + colcon build --symlink-install && \ + rm -rf /tmp/IsaacSim-ros_workspaces-main + +# copy over the AscentAeroSystemsSITLPackage +COPY docker/isaac-sim/AscentAeroSystemsSITLPackage /AscentAeroSystemsSITLPackage + +# the user.config.json file specifies to auto load the SITL extensions +COPY docker/isaac-sim/user.config.json /root/.local/share/ov/data/Kit/Isaac-Sim/4.1/user.config.json + +COPY ros_ws/fastdds.xml /isaac-sim/fastdds.xml + +# Cleanup. Prevent people accidentally doing git commits as root in Docker +RUN apt purge -y git && apt autoremove -y \ + && apt clean -y \ + && rm -rf /var/lib/apt/lists/* + diff --git a/simulation/isaac-sim/docker/docker-compose.yaml b/simulation/isaac-sim/docker/docker-compose.yaml new file mode 100644 index 00000000..6dd5e68e --- /dev/null +++ b/simulation/isaac-sim/docker/docker-compose.yaml @@ -0,0 +1,74 @@ +services: +# ============================================================================== + isaac-sim: + image: &isaac-sim-image airlab-storage.andrew.cmu.edu:5001/shared/isaac-sim_ros-humble:v1.0.2 + build: + context: ../ + dockerfile: docker/Dockerfile.isaac-ros + tags: + - *isaac-sim-image + container_name: isaac-sim + entrypoint: "" # override the default entrypoint with nothing + # for some reason the tmux session manager only works when isaac is running in tmux + command: > + bash -c " + tmux new -d -s isaac + && tmux send-keys -t isaac 'runapp' ENTER + && sleep infinity" + # Interactive shell + stdin_open: true # docker run -i + tty: true # docker run -t + ipc: host + privileged: true + networks: + airstack_network: + ipv4_address: 172.31.0.200 + env_file: ./omni_pass.env + environment: + # NVIDIA stuff + - ACCEPT_EULA=Y + - DISPLAY + - QT_X11_NO_MITSHM=1 + # ROS2 stuff + - RMW_IMPLEMENTATION=rmw_fastrtps_cpp + - FASTRTPS_DEFAULT_PROFILES_FILE=/isaac-sim/fastdds.xml + deploy: + # let it use the GPU + resources: + reservations: + devices: + - driver: nvidia # https://stackoverflow.com/a/70761193 + count: 1 + capabilities: [ gpu ] + volumes: + # display + - $HOME/.Xauthority:/root/.Xauthority + - /tmp/.X11-unix:/tmp/.X11-unix + # isaac sim stuff + - ~/docker/isaac-sim/cache/kit:/isaac-sim/kit/cache:rw + - ~/docker/isaac-sim/cache/ov:/root/.cache/ov:rw + - ~/docker/isaac-sim/cache/pip:/root/.cache/pip:rw + - ~/docker/isaac-sim/cache/glcache:/root/.cache/nvidia/GLCache:rw + - ~/docker/isaac-sim/cache/computecache:/root/.nv/ComputeCache:rw + - ~/docker/isaac-sim/logs:/root/.nvidia-omniverse/logs:rw + - ~/docker/isaac-sim/data:/root/.local/share/ov/data:rw + - ~/docker/isaac-sim/documents:/root/Documents:rw + - ./user.config.json:/root/.local/share/ov/data/Kit/Isaac-Sim/4.1/user.config.json:rw + - ./ui.py:/isaac-sim/kit/exts/omni.kit.widget.nucleus_connector/omni/kit/widget/nucleus_connector/ui.py:rw + # developer stuff + - .dev:/root/.dev:rw # developer config + - .bashrc:/root/.bashrc:rw # bash config + # code + - ../sitl_integration/kit-app-template/source/extensions/:/root/Documents/Kit/shared/exts/ + - ../sitl_integration:/sitl_integration:rw + - ../sitl_integration/inputrc:/etc/inputrc + - ../sitl_integration/config:/root/.nvidia-omniverse/config:rw + +# ============================================================================== +networks: + airstack_network: + driver: bridge + ipam: + driver: default + config: + - subnet: 172.31.0.0/24 diff --git a/simulation/isaac-sim/docker/omni_pass_TEMPLATE.env b/simulation/isaac-sim/docker/omni_pass_TEMPLATE.env new file mode 100644 index 00000000..92732b35 --- /dev/null +++ b/simulation/isaac-sim/docker/omni_pass_TEMPLATE.env @@ -0,0 +1,41 @@ +######################################################################### +## Custom settings for the Isaac-Sim container +######################################################################### + +# Copy this file and rename it to omni_pass.env to paste in your info. + +######################################################################### +## Nucleus Login information +# This can either be your username and password or the nucleus login token +# The login token method is preferred. You can get the token by going to +# the nucleus server website. For us +# https://airlab-storage.andrew.cmu.edu:8443/omni/web3/ +# logging in. +# Then right clicking on the cloud and click the "API Tokens" window +# to generate an API token and copy it to "OMNI_PASS" + +######################################################################### + +# api token method +OMNI_USER='$omni-api-token' # don't touch this line +OMNI_PASS=PASTE-YOUR-API-TOKEN + + +# username password method +#OMNI_USER= +#OMNI_PASS= + +######################################################################### +# Set the default Nucleus server to use for issacsim: + +OMNI_SERVER="https://airlab-storage.andrew.cmu.edu:8443/NVIDIA/Assets/Isaac/4.1" + +######################################################################### +#Accept the licenses and privacy + +ACCEPT_EULA=Y +OMNI_ENV_PRIVACY_CONSENT=Y + + +######################################################################### + diff --git a/simulation/isaac-sim/docker/ui.py b/simulation/isaac-sim/docker/ui.py new file mode 100644 index 00000000..f4af59cf --- /dev/null +++ b/simulation/isaac-sim/docker/ui.py @@ -0,0 +1,456 @@ +# Copyright (c) 2018-2020, NVIDIA CORPORATION. All rights reserved. +# +# NVIDIA CORPORATION and its licensors retain all intellectual property +# and proprietary rights in and to this software, related documentation +# and any modifications thereto. Any use, reproduction, disclosure or +# distribution of this software and related documentation without an express +# license agreement from NVIDIA CORPORATION is strictly prohibited. +# +"""This module provides user interface components for establishing and managing Nucleus connections in the Omniverse environment.""" + + +import os +import sys +import tempfile + +import carb +import carb.settings +import omni.ui as ui +import asyncio +import omni.kit.app +from omni.kit.async_engine import run_coroutine + +from typing import Coroutine, Any, Callable, Union +from omni.kit.window.popup_dialog.dialog import PopupDialog +from omni.kit.window.popup_dialog.form_dialog import FormDialog, FormWidget +from .style import UI_STYLES, ICON_PATH + + +class ConnectorDialog(PopupDialog): + """The main connection dialog for establishing Nucleus connections. + + This dialog provides a user interface to add new Omniverse connections with an optional name. It manages the UI elements and tasks associated with the connection process, such as displaying progress, handling cancellations, and showing alerts. + """ + + def __init__(self): + super().__init__(width=400, title="Add Nucleus connection", ok_label="Ok", cancel_label="Cancel", modal=True) + self._form_widget = None + self._progress_bar = None + self._alert_pane = None + self._task = None + self._build_ui() + + def _build_ui(self): + field_defs = [ + FormDialog.FieldDef("url", "Omniverse:// ", ui.StringField, "", True), + FormDialog.FieldDef("name", "Optional Name: ", ui.StringField, ""), + ] + with self._window.frame: + with ui.VStack(style=UI_STYLES, style_type_name_override="Dialog"): + # OM-81873: This is a temporary solution for the authentication dialog; add the form widget in a frame + # so that it could be easily hidden/shown + self._form_frame = ui.Frame() + with self._form_frame: + self._form_widget = FormWidget("Add a new connection with optional name.", field_defs) + with ui.ZStack(): + ui.Spacer(height=60) + with ui.VStack(): + ui.Spacer(height=8) + self._progress_bar = LoopProgressBar(puck_size=40) + ui.Spacer() + # OM-86699: Fix issue with randomly resized connector dialog, specify ZStack width with window width + # OM-98771: Substract padding from window width when specifying AlertPane width + self._alert_pane = AlertPane(width=self._window.width - 24) + self._build_ok_cancel_buttons() + self.hide() + self.set_cancel_clicked_fn(lambda _: self._on_cancel_fn()) + + def _on_cancel_fn(self): + self.cancel_task() + self.hide() + + def get_value(self, name: str) -> str: + if self._form_widget: + return self._form_widget.get_value(name) + return None + + def set_value(self, name: str, value: str): + if self._form_widget: + field = self._form_widget.get_field(name) + if field: + field.model.set_value(value or "") + + def destroy(self): + self.cancel_task() + if self._form_widget: + self._form_widget.destroy() + self._form_widget = None + if self._form_frame: + self._form_frame = None + self._progress_bar = None + if self._alert_pane: + self._alert_pane.destroy() + self._alert_pane = None + super().destroy() + + def __del__(self): + self.destroy() + + async def run_cancellable_task(self, task: Coroutine) -> Any: + """Manages running and cancelling the given task""" + if self._task: + self.cancel_task() + self._task = asyncio.create_task(task) + + try: + return await self._task + except asyncio.CancelledError: + carb.log_info(f"Cancelling task ... {self._task}") + raise + except Exception as e: + raise + finally: + self._task = None + + def cancel_task(self): + """Cancels the managed task""" + if self._task is not None: + self._task.cancel() + self._task = None + + @property + def frame(self): + return self._window.frame + + @property + def visible(self) -> bool: + if self._window: + return self._window.visible + return False + + def show(self, name: str = None, url: str = None): + """Shows the dialog after resetting to a default state""" + broken_url = omni.client.break_url(url or "") + host = broken_url.host if broken_url.scheme == "omniverse" else None + self.set_value("url", host) + self.set_value("name", name) + if self._progress_bar: + self._progress_bar.hide() + if self._alert_pane: + self._alert_pane.hide() + if not self._okay_button: + self._build_ok_cancel_buttons() + self._okay_button.enabled = True + self._cancel_button.enabled = True + self._okay_button.visible = True + if self._form_frame: + self._form_frame.visible = True + + self._window.visible = True + + # focus fields on show + if self._form_widget: + self._form_widget.focus() + + def hide(self): + self._window.visible = False + # reset window title on hide; authentication mode might have reset the window title. + self._window.title = self._title + + def show_waiting(self): + if self._alert_pane: + self._alert_pane.hide() + if self._progress_bar: + self._progress_bar.show() + # Disable apply button + if self._okay_button: + self._okay_button.enabled = False + + def show_alert(self, msg: str = "", alert_type: int = 0): + # hide form widget and OK button + if self._okay_button: + self._okay_button.visible = False + # Disable apply button + self._okay_button.enabled = False + if self._form_frame: + self._form_frame.visible = False + + if self._alert_pane: + self._alert_pane.show(msg, alert_type=alert_type) + if self._progress_bar: + self._progress_bar.hide() + self._window.visible = True + + def show_authenticate(self, name: str, url: str): + """Shows the dialog for authentication, when the name and url is already given.""" + # hide form widget and OK button + if self._okay_button: + self._okay_button.visible = False + if self._form_frame: + self._form_frame.visible = False + + # show login info + broken_url = omni.client.break_url(url or "") + host = broken_url.host if broken_url.scheme == "omniverse" else None + if self._alert_pane: + msg = f"Connecting to ({host}) requires login authentication. Please login from your web browser." + carb.log_warn(msg) + #self._alert_pane.show(msg, alert_type=AlertPane.Info) + + # update window title + self._window.title = "Login Required" + #self._window.visible = True + + +class LoopProgressBar: + """A looping progress bar for visual indication of ongoing processes. + + This class creates a graphical progress bar that loops to indicate an ongoing process without a defined end. It is commonly used in UIs to show that an action is taking place. + + Args: + puck_size (int): Size of the moving element in the progress bar. + period (float): The time it takes for the puck to complete one loop.""" + + def __init__(self, puck_size: int = 20, period: float = 120): + self._puck_size = puck_size + self._period = period + self._widget = None + self._spacer_left = None + self._spacer_right = None + self._loop_task = None + self._build_ui() + + def _build_ui(self): + self._widget = ui.Frame(visible=False) + with self._widget: + with ui.ZStack(height=20): + ui.Rectangle(style_type_name_override="ProgressBar") + with ui.HStack(): + self._spacer_left = ui.Spacer(width=ui.Fraction(0)) + ui.Rectangle(width=self._puck_size, style_type_name_override="ProgressBar.Puck") + self._spacer_right = ui.Spacer(width=ui.Fraction(1)) + + async def _inf_loop(self): + counter = 0 + while True: + await omni.kit.app.get_app().next_update_async() + width = float(counter % self._period) / self._period + # Ping-pong + width = width * 2 + if width > 1.0: + width = 2.0 - width + self._spacer_left.width = ui.Fraction(width) + self._spacer_right.width = ui.Fraction(1.0 - width) + counter += 1 + + def __del__(self): + self.hide() + self._widget = None + + def show(self): + if self._widget: + self._loop_task = asyncio.ensure_future(self._inf_loop()) + self._widget.visible = True + + def hide(self): + if self._widget: + if self._loop_task is not None: + self._loop_task.cancel() + self._loop_task = None + self._widget.visible = False + + +class AlertPane: + """A class representing an alert pane within an application dialog. + + This pane can be used to display informational messages or warnings to the user in a stylized format. + + Args: + width: int + The width of the alert pane in pixels.""" + + Info = 0 + Warn = 1 + + def __init__(self, width: int = 400): + self._widget: ui.Frame = None + self._frame: ui.Frame = None + self._icon_frame: ui.Frame = None + self._label: ui.Label = None + self._build_ui(width=width) + + def destroy(self): + self._label = None + self._frame = None + self._widget = None + + def _build_ui(self, width: int = 400): + self._widget = ui.Frame(visible=False, height=0) + with self._widget: + with ui.ZStack(width=width, style=UI_STYLES): + self._frame = ui.Frame() + with ui.HStack(spacing=4, style_type_name_override="AlertPane.Content"): + self._icon_frame = ui.Frame() + self._label = ui.Label("", height=20, word_wrap=True, style_type_name_override="Label") + + def show(self, msg: str = "", alert_type: int = 0): + if self._widget: + with self._frame: + style_name = "warn" if alert_type == AlertPane.Warn else "info" + ui.Rectangle(style_type_name_override="AlertPane", name=style_name) + with self._icon_frame: + icon = f"{ICON_PATH}/warn.svg" if alert_type == AlertPane.Warn else f"{ICON_PATH}/info.svg" + ui.ImageWithProvider( + icon, + width=16, + height=16, + fill_policy=ui.IwpFillPolicy.IWP_PRESERVE_ASPECT_FIT, + style_type_name_override="Image", + ) + if self._label: + self._label.text = msg + self._widget.visible = True + + def hide(self): + if self._widget: + self._widget.visible = False + + +# OM-98450: Add UI for device auth flow +class DeviceAuthFlowDialog(PopupDialog): + """A dialog for facilitating device authentication flow. + + This class manages the user interface for device authentication, providing a QR code for the user + to scan and input a code for authentication. It handles countdowns for code expiration and + retries for expired codes. + + Args: + params (omni.client.AuthDeviceFlowParams): Parameters for device authentication including URL, server, and expiration. + """ + + TEMP_DIR = os.path.join(tempfile.gettempdir(), "kit_device_auth_qrcode/") + + """The main authentication dialog for device auth flow.""" + + def __init__(self, params: omni.client.AuthDeviceFlowParams): + super().__init__(width=400, title="Device Authentication", ok_label="Retry", cancel_label="Cancel", modal=True) + self._image = None + self._time_remaining = params.expiration + self._server = params.server + self._expiration_frame = None + self._build_ui(params) + self._count_down_task = run_coroutine(self._count_expiration_async()) + self._generate_qrcode(params.url) + + @property + def visible(self) -> bool: + if self._window: + return self._window.visible + return False + + def _build_ui(self, params: omni.client.AuthDeviceFlowParams): + with self._window.frame: + with ui.VStack(style=UI_STYLES, style_type_name_override="Dialog"): + ui.Label(f"Connecting to {params.server}", alignment=ui.Alignment.CENTER) + with ui.HStack(): + ui.Spacer() + self._image = ui.Image(width=128, height=128, style_type_name_override="QrCode") + ui.Spacer() + with ui.HStack(): + ui.Label(f"Please go to ", width=0) + # OM-102092: Change URL to be a string field so it is selectable and could be copied + # TODO: Note that if the URL is too long, currently it could get clipped and will not display the + # entire url; Solution could be: + # 1) Have both a label and a string field, and when mouse pressed, switch to string field for + # selection; which is a bit hacky, since the string field height need to be dynamically adjusted + # 2) have omni.ui.Label support selection of text + ui.StringField( + ui.SimpleStringModel(params.url), + style=UI_STYLES, + style_type_name_override="StringField.Url", + read_only=True, + ) + ui.Label("or scan the qrcode above, and enter the code below:") + ui.Spacer(height=2) + ui.Label(params.code, style_type_name_override="Label.Code") + ui.Spacer(height=2) + self._expiration_frame = ui.Frame() + with self._expiration_frame: + with ui.HStack(spacing=4): + ui.Spacer() + ui.Label("Code expires in", width=0) + self._time_remaining_label = ui.Label( + str(self._time_remaining), style_type_name_override="Label.TimeRemaining", width=0 + ) + ui.Label("seconds", width=0) + ui.Spacer() + ui.Spacer(height=10) + self._build_ok_cancel_buttons() + ui.Spacer(height=30) + self.set_cancel_clicked_fn(lambda _: self._on_cancel_fn()) + self.set_okay_clicked_fn(lambda _: self._on_retry()) + self._okay_button.visible = False + + def _on_cancel_fn(self): + self._count_down_task.cancel() + self.hide() + + def _on_retry(self): + self._on_cancel() + if not self._server.startswith("omniverse://"): + server = "omniverse://" + self._server + else: + server = self._server + omni.client.reconnect(server) + + def destroy(self): + if self._count_down_task: + self._count_down_task.cancel() + self._count_down_task = None + super().destroy() + + def __del__(self): + self.destroy() + + def hide(self): + self._window.visible = False + # reset window title on hide; authentication mode might have reset the window title. + self._window.title = self._title + + def show_expired(self): + """Show the user when the code has expired.""" + with self._expiration_frame: + ui.Label("Code has expired. Please try again.", style_type_name_override="Label.Expired") + self._okay_button.visible = True + + def _generate_qrcode(self, url: str): # pragma: no cover + """Generates a qrcode from the given url.""" + if not os.path.exists(self.TEMP_DIR): + os.mkdir(self.TEMP_DIR) + # Swap out the : && / since the url we are dealing with are usually something like: "https://a.b.c/x/y", + # replacing these special characters with "_" here so we don't end up violating OS naming conventions + img_name = url.replace("/", "_").replace(":", "_") + img_path = os.path.join(self.TEMP_DIR, f"qr_{img_name}.png") + + # we could skip generating the qrcode for the same url, since the image would be the same with the same encoding + if not os.path.exists(img_path): + # try to use qrcode module to generate the image + try: + import qrcode + except ImportError: + carb.log_warn("Package qrcode not installed. Skipping qr code generation.") + return + + img = qrcode.make(url) + img.save(img_path) + + # update image source url + self._image.source_url = img_path + + async def _count_expiration_async(self): + """Counts down the remaining time in seconds before expiration.""" + while self._time_remaining > 0: + await asyncio.sleep(1) + self._time_remaining -= 1 + self._time_remaining_label.text = str(self._time_remaining) + self.show_expired() diff --git a/simulation/isaac-sim/docker/user_TEMPLATE.config.json b/simulation/isaac-sim/docker/user_TEMPLATE.config.json new file mode 100644 index 00000000..5addac52 --- /dev/null +++ b/simulation/isaac-sim/docker/user_TEMPLATE.config.json @@ -0,0 +1,2715 @@ +{ + "persistent": { + "isaac": { + "asset_root": { + "default": "https://airlab-storage.andrew.cmu.edu:8443/omni/web3/NVIDIA/Assets/Isaac/4.1", + "nvidia": "omniverse://localhost/NVIDIA", + "isaac": "omniverse://localhost/NVIDIA/Assets/Isaac/4.1/Isaac", + "cloud": "http://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/4.1", + "timeout": 5.0 + } + }, + "simulation": { + "minFrameRate": 60, + "defaultMetersPerUnit": 1.0 + }, + "omni": { + "replicator": { + "captureOnPlay": true + } + }, + "renderer": { + "startupMessageDisplayed": false, + "raytracingOmm": { + "enabled": false + } + }, + "resourcemonitor": { + "timeBetweenQueries": 100, + "sendDeviceMemoryWarning": true, + "deviceMemoryWarnMB": 1, + "deviceMemoryWarnFraction": 0.10000000149011612, + "sendHostMemoryWarning": true, + "hostMemoryWarnMB": 1, + "hostMemoryWarnFraction": 0.10000000149011612 + }, + "audio": { + "context": { + "streamerFile": "", + "speakerMode": "auto-detect", + "deviceName": "", + "captureDeviceName": "", + "autoStreamThreshold": 0, + "audioPlayerAutoStreamThreshold": 256, + "masterVolume": 1.0, + "usdVolume": 1.0, + "spatialVolume": 1.0, + "nonSpatialVolume": 1.0, + "closeAudioPlayerOnStop": false, + "uiVolume": 1.0 + } + }, + "ext": { + "omni.kit.usd.layers": { + "auto_reload_sublayers": false + }, + "omni.kit.widget.stage": { + "show_prim_displayname": false, + "auto_reload_non_sublayers": false + }, + "omni.usd": { + "keep_children_order": false + } + }, + "omnigraph": { + "updateToUsd": false, + "useSchemaPrims": true, + "disablePrimNodes": true, + "updateMeshPointsToHydra": false, + "generator": { + "pyOptimize": false + }, + "enablePathChangedCallback": true, + "deprecationsAreErrors": false, + "disableInfoNoticeHandlingInPlayback": false, + "autoInstancingEnabled": true, + "settingsVersion": 5, + "showNameAsType": true + }, + "exts": { + "omni.anim.navigation.core": { + "navMesh": { + "viewNavMesh": false, + "config": { + "autoRebakeOnChanges": false + } + } + }, + "omni.kit.manipulator.camera": { + "lookSpeed": { + "0": 180.0, + "1": 90.0 + }, + "tumbleSpeed": 360.0, + "moveSpeed": { + "0": 1.0, + "1": 1.0, + "2": 1.0 + }, + "flyViewLock": false + }, + "omni.kit.window.script_editor": { + "clearAfterExecute": false, + "editorPalette": "Dark", + "executeOnReload": false, + "fontSize": 14, + "font": "" + }, + "omni.kit.thumbnails.usd": { + "thumbnail_on_save": true, + "wait_stage_load": false, + "wait_stage_load_timeout": 50 + }, + "omni.kit.manipulator.selector": { + "orders": { + "omni.physxsupportui": -10, + "omni.kit.manipulator.prim.core": -1 + } + }, + "omni.kit.manipulator.prim.core": { + "manipulator": { + "placement": "Authored Pivot" + } + }, + "omni.kit.viewport.manipulator.transform": { + "manipulator": { + "freeRotationType": "Clamped", + "freeRotationEnabled": true, + "scaleMultiplier": 1.4, + "intersectionThickness": 10 + } + }, + "omni.kit.manipulator.tool.snap": { + "conformToTarget": false, + "conformUpAxis": "Stage", + "explicitTransform": { + "rotate": 1.0, + "scale": 1.0, + "translate": 1.0 + }, + "keepSpacing": true + }, + "omni.kit.manipulator.transform": { + "manipulator": { + "freeRotationType": "Clamped", + "freeRotationEnabled": true, + "scaleMultiplier": 1.4, + "intersectionThickness": 10, + "omniScaleDirection": "XY" + }, + "tools": { + "defaultCollapsed": true + } + }, + "omni.kit.window.extensions": { + "publishingEnabled": false, + "openInVSCode": false + }, + "omni.kit.property.usd": { + "large_selection": 100, + "raw_widget_multi_selection_limit": 1, + "references_check_missing": true, + "references_hide_max": 5 + }, + "omni.kit.viewport.window": { + "cameraSpeedMessage": { + "collapsed": false + } + }, + "omni.kit.widget.stage": { + "showColumnVisibility": true, + "showColumnReload": true, + "showColumnType": true + }, + "omni.kit.material.library": { + "browserPath": "" + }, + "omni.kit.window.content_browser": { + "show_grid_view": true, + "grid_view_scale": 2 + }, + "omni.kit.window.filepicker": { + "window_split": 306, + "show_grid_view": true, + "grid_view_scale": 2 + }, + "omni.importer.onshape.settings": { + "filter_unsupported": false, + "import_physics": true, + "chord_tolerance": 0.001, + "angle_tolerance": 15, + "max_chord": 0.2, + "default_import_folder": "/root/Documents", + "import_in_place": true, + "USE_API_KEY": false, + "API_KEY": "", + "API_SECRET": "", + "BASE_URL": "https://cad.onshape.com", + "AUTH_URL": "https://oauth.onshape.com/oauth/authorize", + "TOKEN_URL": "https://oauth.onshape.com/oauth/token" + }, + "omni.kit.viewport.menubar.core": { + "spacer": { + "order": 0 + } + }, + "omni.kit.viewport.menubar.display": { + "visible": true, + "order": -80 + }, + "omni.syntheticdata": { + "menubar": { + "visible": true, + "order": -1 + } + }, + "omni.kit.viewport.menubar.lighting": { + "autoLightRig": { + "enabled": true, + "searchFromDefaultPrim": false + }, + "visible": true, + "order": 100 + }, + "omni.kit.window.stage": { + "columns": "Visibility|Type" + }, + "omni.kit.widget.extended_searchfield": { + "engine": "" + }, + "omni.kit.viewport.menubar.camera": { + "expand": false, + "visible": true, + "order": -10 + }, + "omni.kit.viewport.menubar.render": { + "visible": true, + "order": -90 + }, + "omni.kit.viewport.menubar.settings": { + "visible": true, + "order": -100 + }, + "omni.kit.registry.nucleus": { + "userRegistries": {} + } + }, + "physics": { + "visualizationDisplayJoints": false, + "visualizationSimulationOutput": false, + "useActiveCudaContext": false, + "visualizationQueryUsdrtForTraversal": false, + "visualizationDisplayAttachmentsHideActor1": false, + "visualizationDisplayAttachmentsHideActor0": false, + "visualizationDisplayParticlesShowDeformableMesh": false, + "visualizationDisplayParticlesShowFluidSurface": false, + "visualizationDisplayParticlesClothMeshLines": false, + "visualizationDisplayParticlesShowDiffuseParticles": false, + "visualizationDisplayColliderNormals": false, + "visualizationSimulationDataVisualizer": false, + "visualizationDisplayParticlesShowDeformableParticles": true, + "pvdStreamToFile": false, + "pvdEnabled": false, + "pvdDebug": true, + "pvdProfile": true, + "visualizationDisplayParticlesShowParticleSetParticles": true, + "pvdMemory": true, + "useLocalMeshCache": true, + "autocreatePhysicsScene": true, + "massDistributionManipulator": false, + "visualizationDisplayParticlesParticleRadius": 2, + "visualizationDisplayParticles": 0, + "visualizationDisplayAttachments": 0, + "visualizationDisplayDeformableBodyType": 0, + "visualizationDisplayTendons": 0, + "visualizationDisplayColliders": 0, + "backwardCompatibilityCheckMode": 2, + "localMeshCacheSizeMB": 1024, + "cooking": { + "ujitsoCookingMaxProcessCount": 16 + }, + "visualizationDisplayMassProperties": 0, + "visualizationDisplayParticlesParticlePositions": 0, + "numThreads": 8, + "visualizationDisplayDeformableBodies": 0, + "overrideGPUSettings": -1, + "visualizationSimplifyAtDistance": 2500.0, + "visualizationGap": 0.20000000298023224, + "omniPvdOvdRecordingDirectory": "", + "testRunnerFilter": "", + "pvdOutputDirectory": "", + "pvdIP": "127.0.0.1", + "testRunnerSelection": {}, + "supportUiActionBarEnabled": false, + "supportUiLoggingEnabled": false, + "supportUiFloatingNotificationsEnabled": true, + "supportUiAutomaticCollisionCreationEnabled": false, + "supportUiAsyncCookingAtStageLoad": true, + "supportUiCustomManipulatorEnabled": true, + "supportUiManipModeAllowRigidBodyTraversal": 0, + "supportUiManipModeAllowRotWhileTranslating": 1, + "supportUiManipModeAllowTranOnOtherAxesWhileTranslating": 1, + "supportUiManipModeAllowTranWhileRotating": 1, + "supportUiManipModeAllowRotOnOtherAxesWhileRotating": 1, + "supportUiToolbarButtonsWithText": false, + "supportUiAvoidChangingExistingColliders": false, + "supportUiStaticColliderSimplificationType": 0, + "supportUiDynamicColliderSimplificationType": 0, + "resetOnStop": true + }, + "omnihydra": { + "useFastSceneDelegate": true, + "useSceneGraphInstancing": false, + "useCachedXformTimeSamples": false, + "useSkelAdapter": true, + "useSkelAdapterBlendShape": true, + "useSkelAdapterDeformGraph": false, + "skinDeformerExtLoaded": false + }, + "rtx": { + "resourcemanager": { + "placeholderTextureColor": { + "0": 0.0, + "1": 1.0, + "2": 1.0 + } + }, + "mdltranslator": { + "mdlDistilling": true + }, + "sceneDb": { + "allowDuplicateAhsInvocation": true + }, + "hydra": { + "points": { + "defaultWidth": 1.0 + }, + "readTransformsFromFabricInRenderDelegate": false + } + }, + "app": { + "file": { + "recentFiles": {} + }, + "transform": { + "gizmoUseSRT": true + }, + "camera": { + "controllerUseSRT": true + }, + "renderer": { + "resolution": { + "custom": { + "list": {} + } + } + }, + "newStage": { + "defaultTemplate": "sunlight", + "templatePath": [ + "${app_documents}/scripts/new_stage" + ] + }, + "rendering": { + "whiteModeExceptions": "GizmoTex, Gizmo, OmniGlass, SunsetSkyMat" + }, + "usd": { + "muteUsdCodingError": false, + "usdLuxUnprefixedCompat": true + }, + "layersinterface": { + "continousUpdate": true + }, + "primCreation": { + "DefaultXformOpType": "Scale, Orient, Translate", + "DefaultXformOpOrder": "xformOp:translate, xformOp:orient, xformOp:scale", + "typedDefaults": { + "camera": { + "clippingRange": [ + 0.01, + 10000000.0 + ], + "focalLength": 18.147562, + "focusDistance": 400 + }, + "orthoCamera": { + "clippingRange": [ + 1.0, + 10000000.0 + ] + } + }, + "DefaultRotationOrder": "XYZ", + "DefaultCameraRotationOrder": "YXZ", + "highQuality": true, + "PrimCreationWithDefaultXformOps": true, + "DefaultXformOpPrecision": "Double" + }, + "stage": { + "upAxis": "Z", + "timeCodeRange": [ + 0, + 1000000 + ], + "movePrimInPlace": false, + "instanceableOnCreatingReference": false, + "materialStrength": "weakerThanDescendants", + "interpolationType": "Linear", + "timeCodesPerSecond": 60.0, + "defaultPrimName": "World", + "dragDropImport": "payload", + "nestedGprimsAuthoring": false + }, + "datetime": { + "format": "MM/DD/YYYY" + }, + "material": { + "dragDropMaterialPath": "Relative" + }, + "layerwindow": { + "showLayerContents": true, + "showSessionLayer": false, + "showMetricsAssemblerLayer": false, + "showLayerFileExtension": true, + "filedialog": { + "showRootLayerLocation": true + }, + "showInfoNotification": true, + "showWarningNotification": true, + "enableAutoAuthoringMode": false, + "enableSpecLinkingMode": false, + "showMissingReference": false, + "showMergeOrFlattenWarning": true + }, + "captureFrame": { + "path": "/root/Documents/Kit/shared/screenshots/", + "viewport": true + }, + "thumbnailsGenerator": { + "usd": { + "renderer": "RayTracing", + "iterations": 8 + }, + "mdl": { + "renderer": "PathTracing", + "iterations": 512, + "usd_template_path": "/Library/AEC/Materials/Library_Default.thumbnail.usd", + "standin_usd_path": "/Stage/ShaderKnob" + } + }, + "omniverse": { + "content_browser": { + "options_menu": { + "show_details": true + } + }, + "filepicker": { + "options_menu": { + "show_details": true + } + }, + "gamepadCameraControl": false + }, + "hydra": { + "displayPurpose": { + "guide": false, + "proxy": false, + "render": true + }, + "material": { + "renderContext": "mdl" + } + }, + "viewport": { + "defaults": { + "tickRate": 120 + }, + "pickingMode": "kind:model.ALL", + "camMoveVelocity": 0.05, + "previewOnPeek": false, + "snapToSurface": false, + "displayOptions": 31951, + "pickingModeNoKinds": true, + "pickingModeIncludeRef": true, + "autoFrame": { + "mode": "", + "singleCamera": true, + "implicitOnly": true + }, + "outline": { + "width": 2, + "intersection": { + "color": { + "0": 1.0, + "1": 0.6, + "2": 0.0, + "3": 1.0 + } + }, + "color": { + "0": 0.0, + "1": 0.0, + "2": 0.5714285969734192, + "3": 1.0, + "4": 0.0, + "5": 0.8999999761581421, + "6": 0.0, + "7": 1.0, + "8": 0.8999999761581421, + "9": 0.0, + "10": 0.0, + "11": 1.0, + "12": 1.0, + "13": 0.6000000238418579, + "14": 0.0, + "15": 1.0, + "16": 1.0, + "17": 0.6000000238418579, + "18": 0.0, + "19": 1.0, + "20": 0.0, + "21": 0.1428571492433548, + "22": 0.7142857313156128, + "23": 1.0, + "24": 0.0, + "25": 0.1428571492433548, + "26": 0.8571428656578064, + "27": 1.0, + "28": 0.0, + "29": 0.1428571492433548, + "30": 1.0, + "31": 1.0, + "32": 0.0, + "33": 0.2857142984867096, + "34": 0.5714285969734192, + "35": 1.0, + "36": 0.0, + "37": 0.2857142984867096, + "38": 0.7142857313156128, + "39": 1.0, + "40": 0.0, + "41": 0.2857142984867096, + "42": 0.8571428656578064, + "43": 1.0, + "44": 0.0, + "45": 0.2857142984867096, + "46": 1.0, + "47": 1.0, + "48": 0.0, + "49": 0.4285714328289032, + "50": 0.5714285969734192, + "51": 1.0, + "52": 0.0, + "53": 0.4285714328289032, + "54": 0.7142857313156128, + "55": 1.0, + "56": 0.0, + "57": 0.4285714328289032, + "58": 0.8571428656578064, + "59": 1.0, + "60": 0.0, + "61": 0.4285714328289032, + "62": 1.0, + "63": 1.0, + "64": 0.0, + "65": 0.5714285969734192, + "66": 0.5714285969734192, + "67": 1.0, + "68": 0.0, + "69": 0.5714285969734192, + "70": 0.7142857313156128, + "71": 1.0, + "72": 0.0, + "73": 0.5714285969734192, + "74": 0.8571428656578064, + "75": 1.0, + "76": 0.0, + "77": 0.5714285969734192, + "78": 1.0, + "79": 1.0, + "80": 0.0, + "81": 0.7142857313156128, + "82": 0.5714285969734192, + "83": 1.0, + "84": 0.0, + "85": 0.7142857313156128, + "86": 0.7142857313156128, + "87": 1.0, + "88": 0.0, + "89": 0.7142857313156128, + "90": 0.8571428656578064, + "91": 1.0, + "92": 0.0, + "93": 0.7142857313156128, + "94": 1.0, + "95": 1.0, + "96": 0.0, + "97": 0.8571428656578064, + "98": 0.5714285969734192, + "99": 1.0, + "100": 0.0, + "101": 0.8571428656578064, + "102": 0.7142857313156128, + "103": 1.0, + "104": 0.0, + "105": 0.8571428656578064, + "106": 0.8571428656578064, + "107": 1.0, + "108": 0.0, + "109": 0.8571428656578064, + "110": 1.0, + "111": 1.0, + "112": 0.0, + "113": 1.0, + "114": 0.5714285969734192, + "115": 1.0, + "116": 0.0, + "117": 1.0, + "118": 0.7142857313156128, + "119": 1.0, + "120": 0.0, + "121": 1.0, + "122": 0.8571428656578064, + "123": 1.0, + "124": 0.0, + "125": 1.0, + "126": 1.0, + "127": 1.0, + "128": 0.1428571492433548, + "129": 0.0, + "130": 0.5714285969734192, + "131": 1.0, + "132": 0.1428571492433548, + "133": 0.0, + "134": 0.7142857313156128, + "135": 1.0, + "136": 0.1428571492433548, + "137": 0.0, + "138": 0.8571428656578064, + "139": 1.0, + "140": 0.1428571492433548, + "141": 0.0, + "142": 1.0, + "143": 1.0, + "144": 0.1428571492433548, + "145": 0.1428571492433548, + "146": 0.5714285969734192, + "147": 1.0, + "148": 0.1428571492433548, + "149": 0.1428571492433548, + "150": 0.7142857313156128, + "151": 1.0, + "152": 0.1428571492433548, + "153": 0.1428571492433548, + "154": 0.8571428656578064, + "155": 1.0, + "156": 0.1428571492433548, + "157": 0.1428571492433548, + "158": 1.0, + "159": 1.0, + "160": 0.1428571492433548, + "161": 0.2857142984867096, + "162": 0.5714285969734192, + "163": 1.0, + "164": 0.1428571492433548, + "165": 0.2857142984867096, + "166": 0.7142857313156128, + "167": 1.0, + "168": 0.1428571492433548, + "169": 0.2857142984867096, + "170": 0.8571428656578064, + "171": 1.0, + "172": 0.1428571492433548, + "173": 0.2857142984867096, + "174": 1.0, + "175": 1.0, + "176": 0.1428571492433548, + "177": 0.4285714328289032, + "178": 0.5714285969734192, + "179": 1.0, + "180": 0.1428571492433548, + "181": 0.4285714328289032, + "182": 0.7142857313156128, + "183": 1.0, + "184": 0.1428571492433548, + "185": 0.4285714328289032, + "186": 0.8571428656578064, + "187": 1.0, + "188": 0.1428571492433548, + "189": 0.4285714328289032, + "190": 1.0, + "191": 1.0, + "192": 0.1428571492433548, + "193": 0.5714285969734192, + "194": 0.5714285969734192, + "195": 1.0, + "196": 0.1428571492433548, + "197": 0.5714285969734192, + "198": 0.7142857313156128, + "199": 1.0, + "200": 0.1428571492433548, + "201": 0.5714285969734192, + "202": 0.8571428656578064, + "203": 1.0, + "204": 0.1428571492433548, + "205": 0.5714285969734192, + "206": 1.0, + "207": 1.0, + "208": 0.1428571492433548, + "209": 0.7142857313156128, + "210": 0.5714285969734192, + "211": 1.0, + "212": 0.1428571492433548, + "213": 0.7142857313156128, + "214": 0.7142857313156128, + "215": 1.0, + "216": 0.1428571492433548, + "217": 0.7142857313156128, + "218": 0.8571428656578064, + "219": 1.0, + "220": 0.1428571492433548, + "221": 0.7142857313156128, + "222": 1.0, + "223": 1.0, + "224": 0.1428571492433548, + "225": 0.8571428656578064, + "226": 0.5714285969734192, + "227": 1.0, + "228": 0.1428571492433548, + "229": 0.8571428656578064, + "230": 0.7142857313156128, + "231": 1.0, + "232": 0.1428571492433548, + "233": 0.8571428656578064, + "234": 0.8571428656578064, + "235": 1.0, + "236": 0.1428571492433548, + "237": 0.8571428656578064, + "238": 1.0, + "239": 1.0, + "240": 0.1428571492433548, + "241": 1.0, + "242": 0.5714285969734192, + "243": 1.0, + "244": 0.1428571492433548, + "245": 1.0, + "246": 0.7142857313156128, + "247": 1.0, + "248": 0.1428571492433548, + "249": 1.0, + "250": 0.8571428656578064, + "251": 1.0, + "252": 0.1428571492433548, + "253": 1.0, + "254": 1.0, + "255": 1.0, + "256": 0.2857142984867096, + "257": 0.0, + "258": 0.5714285969734192, + "259": 1.0, + "260": 0.2857142984867096, + "261": 0.0, + "262": 0.7142857313156128, + "263": 1.0, + "264": 0.2857142984867096, + "265": 0.0, + "266": 0.8571428656578064, + "267": 1.0, + "268": 0.2857142984867096, + "269": 0.0, + "270": 1.0, + "271": 1.0, + "272": 0.2857142984867096, + "273": 0.1428571492433548, + "274": 0.5714285969734192, + "275": 1.0, + "276": 0.2857142984867096, + "277": 0.1428571492433548, + "278": 0.7142857313156128, + "279": 1.0, + "280": 0.2857142984867096, + "281": 0.1428571492433548, + "282": 0.8571428656578064, + "283": 1.0, + "284": 0.2857142984867096, + "285": 0.1428571492433548, + "286": 1.0, + "287": 1.0, + "288": 0.2857142984867096, + "289": 0.2857142984867096, + "290": 0.5714285969734192, + "291": 1.0, + "292": 0.2857142984867096, + "293": 0.2857142984867096, + "294": 0.7142857313156128, + "295": 1.0, + "296": 0.2857142984867096, + "297": 0.2857142984867096, + "298": 0.8571428656578064, + "299": 1.0, + "300": 0.2857142984867096, + "301": 0.2857142984867096, + "302": 1.0, + "303": 1.0, + "304": 0.2857142984867096, + "305": 0.4285714328289032, + "306": 0.5714285969734192, + "307": 1.0, + "308": 0.2857142984867096, + "309": 0.4285714328289032, + "310": 0.7142857313156128, + "311": 1.0, + "312": 0.2857142984867096, + "313": 0.4285714328289032, + "314": 0.8571428656578064, + "315": 1.0, + "316": 0.2857142984867096, + "317": 0.4285714328289032, + "318": 1.0, + "319": 1.0, + "320": 0.2857142984867096, + "321": 0.5714285969734192, + "322": 0.5714285969734192, + "323": 1.0, + "324": 0.2857142984867096, + "325": 0.5714285969734192, + "326": 0.7142857313156128, + "327": 1.0, + "328": 0.2857142984867096, + "329": 0.5714285969734192, + "330": 0.8571428656578064, + "331": 1.0, + "332": 0.2857142984867096, + "333": 0.5714285969734192, + "334": 1.0, + "335": 1.0, + "336": 0.2857142984867096, + "337": 0.7142857313156128, + "338": 0.5714285969734192, + "339": 1.0, + "340": 0.2857142984867096, + "341": 0.7142857313156128, + "342": 0.7142857313156128, + "343": 1.0, + "344": 0.2857142984867096, + "345": 0.7142857313156128, + "346": 0.8571428656578064, + "347": 1.0, + "348": 0.2857142984867096, + "349": 0.7142857313156128, + "350": 1.0, + "351": 1.0, + "352": 0.2857142984867096, + "353": 0.8571428656578064, + "354": 0.5714285969734192, + "355": 1.0, + "356": 0.2857142984867096, + "357": 0.8571428656578064, + "358": 0.7142857313156128, + "359": 1.0, + "360": 0.2857142984867096, + "361": 0.8571428656578064, + "362": 0.8571428656578064, + "363": 1.0, + "364": 0.2857142984867096, + "365": 0.8571428656578064, + "366": 1.0, + "367": 1.0, + "368": 0.2857142984867096, + "369": 1.0, + "370": 0.5714285969734192, + "371": 1.0, + "372": 0.2857142984867096, + "373": 1.0, + "374": 0.7142857313156128, + "375": 1.0, + "376": 0.2857142984867096, + "377": 1.0, + "378": 0.8571428656578064, + "379": 1.0, + "380": 0.2857142984867096, + "381": 1.0, + "382": 1.0, + "383": 1.0, + "384": 0.4285714328289032, + "385": 0.0, + "386": 0.5714285969734192, + "387": 1.0, + "388": 0.4285714328289032, + "389": 0.0, + "390": 0.7142857313156128, + "391": 1.0, + "392": 0.4285714328289032, + "393": 0.0, + "394": 0.8571428656578064, + "395": 1.0, + "396": 0.4285714328289032, + "397": 0.0, + "398": 1.0, + "399": 1.0, + "400": 0.4285714328289032, + "401": 0.1428571492433548, + "402": 0.5714285969734192, + "403": 1.0, + "404": 0.4285714328289032, + "405": 0.1428571492433548, + "406": 0.7142857313156128, + "407": 1.0, + "408": 0.4285714328289032, + "409": 0.1428571492433548, + "410": 0.8571428656578064, + "411": 1.0, + "412": 0.4285714328289032, + "413": 0.1428571492433548, + "414": 1.0, + "415": 1.0, + "416": 0.4285714328289032, + "417": 0.2857142984867096, + "418": 0.5714285969734192, + "419": 1.0, + "420": 0.4285714328289032, + "421": 0.2857142984867096, + "422": 0.7142857313156128, + "423": 1.0, + "424": 0.4285714328289032, + "425": 0.2857142984867096, + "426": 0.8571428656578064, + "427": 1.0, + "428": 0.4285714328289032, + "429": 0.2857142984867096, + "430": 1.0, + "431": 1.0, + "432": 0.4285714328289032, + "433": 0.4285714328289032, + "434": 0.5714285969734192, + "435": 1.0, + "436": 0.4285714328289032, + "437": 0.4285714328289032, + "438": 0.7142857313156128, + "439": 1.0, + "440": 0.4285714328289032, + "441": 0.4285714328289032, + "442": 0.8571428656578064, + "443": 1.0, + "444": 0.4285714328289032, + "445": 0.4285714328289032, + "446": 1.0, + "447": 1.0, + "448": 0.4285714328289032, + "449": 0.5714285969734192, + "450": 0.5714285969734192, + "451": 1.0, + "452": 0.4285714328289032, + "453": 0.5714285969734192, + "454": 0.7142857313156128, + "455": 1.0, + "456": 0.4285714328289032, + "457": 0.5714285969734192, + "458": 0.8571428656578064, + "459": 1.0, + "460": 0.4285714328289032, + "461": 0.5714285969734192, + "462": 1.0, + "463": 1.0, + "464": 0.4285714328289032, + "465": 0.7142857313156128, + "466": 0.5714285969734192, + "467": 1.0, + "468": 0.4285714328289032, + "469": 0.7142857313156128, + "470": 0.7142857313156128, + "471": 1.0, + "472": 0.4285714328289032, + "473": 0.7142857313156128, + "474": 0.8571428656578064, + "475": 1.0, + "476": 0.4285714328289032, + "477": 0.7142857313156128, + "478": 1.0, + "479": 1.0, + "480": 0.4285714328289032, + "481": 0.8571428656578064, + "482": 0.5714285969734192, + "483": 1.0, + "484": 0.4285714328289032, + "485": 0.8571428656578064, + "486": 0.7142857313156128, + "487": 1.0, + "488": 0.4285714328289032, + "489": 0.8571428656578064, + "490": 0.8571428656578064, + "491": 1.0, + "492": 0.4285714328289032, + "493": 0.8571428656578064, + "494": 1.0, + "495": 1.0, + "496": 0.4285714328289032, + "497": 1.0, + "498": 0.5714285969734192, + "499": 1.0, + "500": 0.4285714328289032, + "501": 1.0, + "502": 0.7142857313156128, + "503": 1.0, + "504": 0.4285714328289032, + "505": 1.0, + "506": 0.8571428656578064, + "507": 1.0, + "508": 0.4285714328289032, + "509": 1.0, + "510": 1.0, + "511": 1.0, + "512": 0.5714285969734192, + "513": 0.0, + "514": 0.5714285969734192, + "515": 1.0, + "516": 0.5714285969734192, + "517": 0.0, + "518": 0.7142857313156128, + "519": 1.0, + "520": 0.5714285969734192, + "521": 0.0, + "522": 0.8571428656578064, + "523": 1.0, + "524": 0.5714285969734192, + "525": 0.0, + "526": 1.0, + "527": 1.0, + "528": 0.5714285969734192, + "529": 0.1428571492433548, + "530": 0.5714285969734192, + "531": 1.0, + "532": 0.5714285969734192, + "533": 0.1428571492433548, + "534": 0.7142857313156128, + "535": 1.0, + "536": 0.5714285969734192, + "537": 0.1428571492433548, + "538": 0.8571428656578064, + "539": 1.0, + "540": 0.5714285969734192, + "541": 0.1428571492433548, + "542": 1.0, + "543": 1.0, + "544": 0.5714285969734192, + "545": 0.2857142984867096, + "546": 0.5714285969734192, + "547": 1.0, + "548": 0.5714285969734192, + "549": 0.2857142984867096, + "550": 0.7142857313156128, + "551": 1.0, + "552": 0.5714285969734192, + "553": 0.2857142984867096, + "554": 0.8571428656578064, + "555": 1.0, + "556": 0.5714285969734192, + "557": 0.2857142984867096, + "558": 1.0, + "559": 1.0, + "560": 0.5714285969734192, + "561": 0.4285714328289032, + "562": 0.5714285969734192, + "563": 1.0, + "564": 0.5714285969734192, + "565": 0.4285714328289032, + "566": 0.7142857313156128, + "567": 1.0, + "568": 0.5714285969734192, + "569": 0.4285714328289032, + "570": 0.8571428656578064, + "571": 1.0, + "572": 0.5714285969734192, + "573": 0.4285714328289032, + "574": 1.0, + "575": 1.0, + "576": 0.5714285969734192, + "577": 0.5714285969734192, + "578": 0.5714285969734192, + "579": 1.0, + "580": 0.5714285969734192, + "581": 0.5714285969734192, + "582": 0.7142857313156128, + "583": 1.0, + "584": 0.5714285969734192, + "585": 0.5714285969734192, + "586": 0.8571428656578064, + "587": 1.0, + "588": 0.5714285969734192, + "589": 0.5714285969734192, + "590": 1.0, + "591": 1.0, + "592": 0.5714285969734192, + "593": 0.7142857313156128, + "594": 0.5714285969734192, + "595": 1.0, + "596": 0.5714285969734192, + "597": 0.7142857313156128, + "598": 0.7142857313156128, + "599": 1.0, + "600": 0.5714285969734192, + "601": 0.7142857313156128, + "602": 0.8571428656578064, + "603": 1.0, + "604": 0.5714285969734192, + "605": 0.7142857313156128, + "606": 1.0, + "607": 1.0, + "608": 0.5714285969734192, + "609": 0.8571428656578064, + "610": 0.5714285969734192, + "611": 1.0, + "612": 0.5714285969734192, + "613": 0.8571428656578064, + "614": 0.7142857313156128, + "615": 1.0, + "616": 0.5714285969734192, + "617": 0.8571428656578064, + "618": 0.8571428656578064, + "619": 1.0, + "620": 0.5714285969734192, + "621": 0.8571428656578064, + "622": 1.0, + "623": 1.0, + "624": 0.5714285969734192, + "625": 1.0, + "626": 0.5714285969734192, + "627": 1.0, + "628": 0.5714285969734192, + "629": 1.0, + "630": 0.7142857313156128, + "631": 1.0, + "632": 0.5714285969734192, + "633": 1.0, + "634": 0.8571428656578064, + "635": 1.0, + "636": 0.5714285969734192, + "637": 1.0, + "638": 1.0, + "639": 1.0, + "640": 0.7142857313156128, + "641": 0.0, + "642": 0.5714285969734192, + "643": 1.0, + "644": 0.7142857313156128, + "645": 0.0, + "646": 0.7142857313156128, + "647": 1.0, + "648": 0.7142857313156128, + "649": 0.0, + "650": 0.8571428656578064, + "651": 1.0, + "652": 0.7142857313156128, + "653": 0.0, + "654": 1.0, + "655": 1.0, + "656": 0.7142857313156128, + "657": 0.1428571492433548, + "658": 0.5714285969734192, + "659": 1.0, + "660": 0.7142857313156128, + "661": 0.1428571492433548, + "662": 0.7142857313156128, + "663": 1.0, + "664": 0.7142857313156128, + "665": 0.1428571492433548, + "666": 0.8571428656578064, + "667": 1.0, + "668": 0.7142857313156128, + "669": 0.1428571492433548, + "670": 1.0, + "671": 1.0, + "672": 0.7142857313156128, + "673": 0.2857142984867096, + "674": 0.5714285969734192, + "675": 1.0, + "676": 0.7142857313156128, + "677": 0.2857142984867096, + "678": 0.7142857313156128, + "679": 1.0, + "680": 0.7142857313156128, + "681": 0.2857142984867096, + "682": 0.8571428656578064, + "683": 1.0, + "684": 0.7142857313156128, + "685": 0.2857142984867096, + "686": 1.0, + "687": 1.0, + "688": 0.7142857313156128, + "689": 0.4285714328289032, + "690": 0.5714285969734192, + "691": 1.0, + "692": 0.7142857313156128, + "693": 0.4285714328289032, + "694": 0.7142857313156128, + "695": 1.0, + "696": 0.7142857313156128, + "697": 0.4285714328289032, + "698": 0.8571428656578064, + "699": 1.0, + "700": 0.7142857313156128, + "701": 0.4285714328289032, + "702": 1.0, + "703": 1.0, + "704": 0.7142857313156128, + "705": 0.5714285969734192, + "706": 0.5714285969734192, + "707": 1.0, + "708": 0.7142857313156128, + "709": 0.5714285969734192, + "710": 0.7142857313156128, + "711": 1.0, + "712": 0.7142857313156128, + "713": 0.5714285969734192, + "714": 0.8571428656578064, + "715": 1.0, + "716": 0.7142857313156128, + "717": 0.5714285969734192, + "718": 1.0, + "719": 1.0, + "720": 0.7142857313156128, + "721": 0.7142857313156128, + "722": 0.5714285969734192, + "723": 1.0, + "724": 0.7142857313156128, + "725": 0.7142857313156128, + "726": 0.7142857313156128, + "727": 1.0, + "728": 0.7142857313156128, + "729": 0.7142857313156128, + "730": 0.8571428656578064, + "731": 1.0, + "732": 0.7142857313156128, + "733": 0.7142857313156128, + "734": 1.0, + "735": 1.0, + "736": 0.7142857313156128, + "737": 0.8571428656578064, + "738": 0.5714285969734192, + "739": 1.0, + "740": 0.7142857313156128, + "741": 0.8571428656578064, + "742": 0.7142857313156128, + "743": 1.0, + "744": 0.7142857313156128, + "745": 0.8571428656578064, + "746": 0.8571428656578064, + "747": 1.0, + "748": 0.7142857313156128, + "749": 0.8571428656578064, + "750": 1.0, + "751": 1.0, + "752": 0.7142857313156128, + "753": 1.0, + "754": 0.5714285969734192, + "755": 1.0, + "756": 0.7142857313156128, + "757": 1.0, + "758": 0.7142857313156128, + "759": 1.0, + "760": 0.7142857313156128, + "761": 1.0, + "762": 0.8571428656578064, + "763": 1.0, + "764": 0.7142857313156128, + "765": 1.0, + "766": 1.0, + "767": 1.0, + "768": 0.8571428656578064, + "769": 0.0, + "770": 0.5714285969734192, + "771": 1.0, + "772": 0.8571428656578064, + "773": 0.0, + "774": 0.7142857313156128, + "775": 1.0, + "776": 0.8571428656578064, + "777": 0.0, + "778": 0.8571428656578064, + "779": 1.0, + "780": 0.8571428656578064, + "781": 0.0, + "782": 1.0, + "783": 1.0, + "784": 0.8571428656578064, + "785": 0.1428571492433548, + "786": 0.5714285969734192, + "787": 1.0, + "788": 0.8571428656578064, + "789": 0.1428571492433548, + "790": 0.7142857313156128, + "791": 1.0, + "792": 0.8571428656578064, + "793": 0.1428571492433548, + "794": 0.8571428656578064, + "795": 1.0, + "796": 0.8571428656578064, + "797": 0.1428571492433548, + "798": 1.0, + "799": 1.0, + "800": 0.8571428656578064, + "801": 0.2857142984867096, + "802": 0.5714285969734192, + "803": 1.0, + "804": 0.8571428656578064, + "805": 0.2857142984867096, + "806": 0.7142857313156128, + "807": 1.0, + "808": 0.8571428656578064, + "809": 0.2857142984867096, + "810": 0.8571428656578064, + "811": 1.0, + "812": 0.8571428656578064, + "813": 0.2857142984867096, + "814": 1.0, + "815": 1.0, + "816": 0.8571428656578064, + "817": 0.4285714328289032, + "818": 0.5714285969734192, + "819": 1.0, + "820": 0.8571428656578064, + "821": 0.4285714328289032, + "822": 0.7142857313156128, + "823": 1.0, + "824": 0.8571428656578064, + "825": 0.4285714328289032, + "826": 0.8571428656578064, + "827": 1.0, + "828": 0.8571428656578064, + "829": 0.4285714328289032, + "830": 1.0, + "831": 1.0, + "832": 0.8571428656578064, + "833": 0.5714285969734192, + "834": 0.5714285969734192, + "835": 1.0, + "836": 0.8571428656578064, + "837": 0.5714285969734192, + "838": 0.7142857313156128, + "839": 1.0, + "840": 0.8571428656578064, + "841": 0.5714285969734192, + "842": 0.8571428656578064, + "843": 1.0, + "844": 0.8571428656578064, + "845": 0.5714285969734192, + "846": 1.0, + "847": 1.0, + "848": 0.8571428656578064, + "849": 0.7142857313156128, + "850": 0.5714285969734192, + "851": 1.0, + "852": 0.8571428656578064, + "853": 0.7142857313156128, + "854": 0.7142857313156128, + "855": 1.0, + "856": 0.8571428656578064, + "857": 0.7142857313156128, + "858": 0.8571428656578064, + "859": 1.0, + "860": 0.8571428656578064, + "861": 0.7142857313156128, + "862": 1.0, + "863": 1.0, + "864": 0.8571428656578064, + "865": 0.8571428656578064, + "866": 0.5714285969734192, + "867": 1.0, + "868": 0.8571428656578064, + "869": 0.8571428656578064, + "870": 0.7142857313156128, + "871": 1.0, + "872": 0.8571428656578064, + "873": 0.8571428656578064, + "874": 0.8571428656578064, + "875": 1.0, + "876": 0.8571428656578064, + "877": 0.8571428656578064, + "878": 1.0, + "879": 1.0, + "880": 0.8571428656578064, + "881": 1.0, + "882": 0.5714285969734192, + "883": 1.0, + "884": 0.8571428656578064, + "885": 1.0, + "886": 0.7142857313156128, + "887": 1.0, + "888": 0.8571428656578064, + "889": 1.0, + "890": 0.8571428656578064, + "891": 1.0, + "892": 0.8571428656578064, + "893": 1.0, + "894": 1.0, + "895": 1.0, + "896": 1.0, + "897": 0.0, + "898": 0.5714285969734192, + "899": 1.0, + "900": 1.0, + "901": 0.0, + "902": 0.7142857313156128, + "903": 1.0, + "904": 1.0, + "905": 0.0, + "906": 0.8571428656578064, + "907": 1.0, + "908": 1.0, + "909": 0.0, + "910": 1.0, + "911": 1.0, + "912": 1.0, + "913": 0.1428571492433548, + "914": 0.5714285969734192, + "915": 1.0, + "916": 1.0, + "917": 0.1428571492433548, + "918": 0.7142857313156128, + "919": 1.0, + "920": 1.0, + "921": 0.1428571492433548, + "922": 0.8571428656578064, + "923": 1.0, + "924": 1.0, + "925": 0.1428571492433548, + "926": 1.0, + "927": 1.0, + "928": 1.0, + "929": 0.2857142984867096, + "930": 0.5714285969734192, + "931": 1.0, + "932": 1.0, + "933": 0.2857142984867096, + "934": 0.7142857313156128, + "935": 1.0, + "936": 1.0, + "937": 0.2857142984867096, + "938": 0.8571428656578064, + "939": 1.0, + "940": 1.0, + "941": 0.2857142984867096, + "942": 1.0, + "943": 1.0, + "944": 1.0, + "945": 0.4285714328289032, + "946": 0.5714285969734192, + "947": 1.0, + "948": 1.0, + "949": 0.4285714328289032, + "950": 0.7142857313156128, + "951": 1.0, + "952": 1.0, + "953": 0.4285714328289032, + "954": 0.8571428656578064, + "955": 1.0, + "956": 1.0, + "957": 0.4285714328289032, + "958": 1.0, + "959": 1.0, + "960": 1.0, + "961": 0.5714285969734192, + "962": 0.5714285969734192, + "963": 1.0, + "964": 1.0, + "965": 0.5714285969734192, + "966": 0.7142857313156128, + "967": 1.0, + "968": 1.0, + "969": 0.5714285969734192, + "970": 0.8571428656578064, + "971": 1.0, + "972": 1.0, + "973": 0.5714285969734192, + "974": 1.0, + "975": 1.0, + "976": 1.0, + "977": 0.7142857313156128, + "978": 0.5714285969734192, + "979": 1.0, + "980": 1.0, + "981": 0.7142857313156128, + "982": 0.7142857313156128, + "983": 1.0, + "984": 1.0, + "985": 0.7142857313156128, + "986": 0.8571428656578064, + "987": 1.0, + "988": 1.0, + "989": 0.7142857313156128, + "990": 1.0, + "991": 1.0, + "992": 1.0, + "993": 0.8571428656578064, + "994": 0.5714285969734192, + "995": 1.0, + "996": 1.0, + "997": 0.8571428656578064, + "998": 0.7142857313156128, + "999": 1.0, + "1000": 1.0, + "1001": 0.8571428656578064, + "1002": 0.8571428656578064, + "1003": 1.0, + "1004": 1.0, + "1005": 0.8571428656578064, + "1006": 1.0, + "1007": 1.0, + "1008": 1.0, + "1009": 1.0, + "1010": 0.5714285969734192, + "1011": 1.0, + "1012": 1.0, + "1013": 1.0, + "1014": 0.7142857313156128, + "1015": 1.0, + "1016": 1.0, + "1017": 1.0, + "1018": 0.8571428656578064, + "1019": 1.0, + "1020": 1.0, + "1021": 0.6000000238418579, + "1022": 0.0, + "1023": 1.0 + }, + "shadeColor": { + "0": 0.0, + "1": 0.0, + "2": 0.5714285969734192, + "3": 1.0, + "4": 0.0, + "5": 0.0, + "6": 0.0, + "7": 0.0, + "8": 0.0, + "9": 0.0, + "10": 0.0, + "11": 0.0, + "12": 1.0, + "13": 1.0, + "14": 1.0, + "15": 0.0, + "16": 1.0, + "17": 1.0, + "18": 1.0, + "19": 0.0, + "20": 0.0, + "21": 0.1428571492433548, + "22": 0.7142857313156128, + "23": 1.0, + "24": 0.0, + "25": 0.1428571492433548, + "26": 0.8571428656578064, + "27": 1.0, + "28": 0.0, + "29": 0.1428571492433548, + "30": 1.0, + "31": 1.0, + "32": 0.0, + "33": 0.2857142984867096, + "34": 0.5714285969734192, + "35": 1.0, + "36": 0.0, + "37": 0.2857142984867096, + "38": 0.7142857313156128, + "39": 1.0, + "40": 0.0, + "41": 0.2857142984867096, + "42": 0.8571428656578064, + "43": 1.0, + "44": 0.0, + "45": 0.2857142984867096, + "46": 1.0, + "47": 1.0, + "48": 0.0, + "49": 0.4285714328289032, + "50": 0.5714285969734192, + "51": 1.0, + "52": 0.0, + "53": 0.4285714328289032, + "54": 0.7142857313156128, + "55": 1.0, + "56": 0.0, + "57": 0.4285714328289032, + "58": 0.8571428656578064, + "59": 1.0, + "60": 0.0, + "61": 0.4285714328289032, + "62": 1.0, + "63": 1.0, + "64": 0.0, + "65": 0.5714285969734192, + "66": 0.5714285969734192, + "67": 1.0, + "68": 0.0, + "69": 0.5714285969734192, + "70": 0.7142857313156128, + "71": 1.0, + "72": 0.0, + "73": 0.5714285969734192, + "74": 0.8571428656578064, + "75": 1.0, + "76": 0.0, + "77": 0.5714285969734192, + "78": 1.0, + "79": 1.0, + "80": 0.0, + "81": 0.7142857313156128, + "82": 0.5714285969734192, + "83": 1.0, + "84": 0.0, + "85": 0.7142857313156128, + "86": 0.7142857313156128, + "87": 1.0, + "88": 0.0, + "89": 0.7142857313156128, + "90": 0.8571428656578064, + "91": 1.0, + "92": 0.0, + "93": 0.7142857313156128, + "94": 1.0, + "95": 1.0, + "96": 0.0, + "97": 0.8571428656578064, + "98": 0.5714285969734192, + "99": 1.0, + "100": 0.0, + "101": 0.8571428656578064, + "102": 0.7142857313156128, + "103": 1.0, + "104": 0.0, + "105": 0.8571428656578064, + "106": 0.8571428656578064, + "107": 1.0, + "108": 0.0, + "109": 0.8571428656578064, + "110": 1.0, + "111": 1.0, + "112": 0.0, + "113": 1.0, + "114": 0.5714285969734192, + "115": 1.0, + "116": 0.0, + "117": 1.0, + "118": 0.7142857313156128, + "119": 1.0, + "120": 0.0, + "121": 1.0, + "122": 0.8571428656578064, + "123": 1.0, + "124": 0.0, + "125": 1.0, + "126": 1.0, + "127": 1.0, + "128": 0.1428571492433548, + "129": 0.0, + "130": 0.5714285969734192, + "131": 1.0, + "132": 0.1428571492433548, + "133": 0.0, + "134": 0.7142857313156128, + "135": 1.0, + "136": 0.1428571492433548, + "137": 0.0, + "138": 0.8571428656578064, + "139": 1.0, + "140": 0.1428571492433548, + "141": 0.0, + "142": 1.0, + "143": 1.0, + "144": 0.1428571492433548, + "145": 0.1428571492433548, + "146": 0.5714285969734192, + "147": 1.0, + "148": 0.1428571492433548, + "149": 0.1428571492433548, + "150": 0.7142857313156128, + "151": 1.0, + "152": 0.1428571492433548, + "153": 0.1428571492433548, + "154": 0.8571428656578064, + "155": 1.0, + "156": 0.1428571492433548, + "157": 0.1428571492433548, + "158": 1.0, + "159": 1.0, + "160": 0.1428571492433548, + "161": 0.2857142984867096, + "162": 0.5714285969734192, + "163": 1.0, + "164": 0.1428571492433548, + "165": 0.2857142984867096, + "166": 0.7142857313156128, + "167": 1.0, + "168": 0.1428571492433548, + "169": 0.2857142984867096, + "170": 0.8571428656578064, + "171": 1.0, + "172": 0.1428571492433548, + "173": 0.2857142984867096, + "174": 1.0, + "175": 1.0, + "176": 0.1428571492433548, + "177": 0.4285714328289032, + "178": 0.5714285969734192, + "179": 1.0, + "180": 0.1428571492433548, + "181": 0.4285714328289032, + "182": 0.7142857313156128, + "183": 1.0, + "184": 0.1428571492433548, + "185": 0.4285714328289032, + "186": 0.8571428656578064, + "187": 1.0, + "188": 0.1428571492433548, + "189": 0.4285714328289032, + "190": 1.0, + "191": 1.0, + "192": 0.1428571492433548, + "193": 0.5714285969734192, + "194": 0.5714285969734192, + "195": 1.0, + "196": 0.1428571492433548, + "197": 0.5714285969734192, + "198": 0.7142857313156128, + "199": 1.0, + "200": 0.1428571492433548, + "201": 0.5714285969734192, + "202": 0.8571428656578064, + "203": 1.0, + "204": 0.1428571492433548, + "205": 0.5714285969734192, + "206": 1.0, + "207": 1.0, + "208": 0.1428571492433548, + "209": 0.7142857313156128, + "210": 0.5714285969734192, + "211": 1.0, + "212": 0.1428571492433548, + "213": 0.7142857313156128, + "214": 0.7142857313156128, + "215": 1.0, + "216": 0.1428571492433548, + "217": 0.7142857313156128, + "218": 0.8571428656578064, + "219": 1.0, + "220": 0.1428571492433548, + "221": 0.7142857313156128, + "222": 1.0, + "223": 1.0, + "224": 0.1428571492433548, + "225": 0.8571428656578064, + "226": 0.5714285969734192, + "227": 1.0, + "228": 0.1428571492433548, + "229": 0.8571428656578064, + "230": 0.7142857313156128, + "231": 1.0, + "232": 0.1428571492433548, + "233": 0.8571428656578064, + "234": 0.8571428656578064, + "235": 1.0, + "236": 0.1428571492433548, + "237": 0.8571428656578064, + "238": 1.0, + "239": 1.0, + "240": 0.1428571492433548, + "241": 1.0, + "242": 0.5714285969734192, + "243": 1.0, + "244": 0.1428571492433548, + "245": 1.0, + "246": 0.7142857313156128, + "247": 1.0, + "248": 0.1428571492433548, + "249": 1.0, + "250": 0.8571428656578064, + "251": 1.0, + "252": 0.1428571492433548, + "253": 1.0, + "254": 1.0, + "255": 1.0, + "256": 0.2857142984867096, + "257": 0.0, + "258": 0.5714285969734192, + "259": 1.0, + "260": 0.2857142984867096, + "261": 0.0, + "262": 0.7142857313156128, + "263": 1.0, + "264": 0.2857142984867096, + "265": 0.0, + "266": 0.8571428656578064, + "267": 1.0, + "268": 0.2857142984867096, + "269": 0.0, + "270": 1.0, + "271": 1.0, + "272": 0.2857142984867096, + "273": 0.1428571492433548, + "274": 0.5714285969734192, + "275": 1.0, + "276": 0.2857142984867096, + "277": 0.1428571492433548, + "278": 0.7142857313156128, + "279": 1.0, + "280": 0.2857142984867096, + "281": 0.1428571492433548, + "282": 0.8571428656578064, + "283": 1.0, + "284": 0.2857142984867096, + "285": 0.1428571492433548, + "286": 1.0, + "287": 1.0, + "288": 0.2857142984867096, + "289": 0.2857142984867096, + "290": 0.5714285969734192, + "291": 1.0, + "292": 0.2857142984867096, + "293": 0.2857142984867096, + "294": 0.7142857313156128, + "295": 1.0, + "296": 0.2857142984867096, + "297": 0.2857142984867096, + "298": 0.8571428656578064, + "299": 1.0, + "300": 0.2857142984867096, + "301": 0.2857142984867096, + "302": 1.0, + "303": 1.0, + "304": 0.2857142984867096, + "305": 0.4285714328289032, + "306": 0.5714285969734192, + "307": 1.0, + "308": 0.2857142984867096, + "309": 0.4285714328289032, + "310": 0.7142857313156128, + "311": 1.0, + "312": 0.2857142984867096, + "313": 0.4285714328289032, + "314": 0.8571428656578064, + "315": 1.0, + "316": 0.2857142984867096, + "317": 0.4285714328289032, + "318": 1.0, + "319": 1.0, + "320": 0.2857142984867096, + "321": 0.5714285969734192, + "322": 0.5714285969734192, + "323": 1.0, + "324": 0.2857142984867096, + "325": 0.5714285969734192, + "326": 0.7142857313156128, + "327": 1.0, + "328": 0.2857142984867096, + "329": 0.5714285969734192, + "330": 0.8571428656578064, + "331": 1.0, + "332": 0.2857142984867096, + "333": 0.5714285969734192, + "334": 1.0, + "335": 1.0, + "336": 0.2857142984867096, + "337": 0.7142857313156128, + "338": 0.5714285969734192, + "339": 1.0, + "340": 0.2857142984867096, + "341": 0.7142857313156128, + "342": 0.7142857313156128, + "343": 1.0, + "344": 0.2857142984867096, + "345": 0.7142857313156128, + "346": 0.8571428656578064, + "347": 1.0, + "348": 0.2857142984867096, + "349": 0.7142857313156128, + "350": 1.0, + "351": 1.0, + "352": 0.2857142984867096, + "353": 0.8571428656578064, + "354": 0.5714285969734192, + "355": 1.0, + "356": 0.2857142984867096, + "357": 0.8571428656578064, + "358": 0.7142857313156128, + "359": 1.0, + "360": 0.2857142984867096, + "361": 0.8571428656578064, + "362": 0.8571428656578064, + "363": 1.0, + "364": 0.2857142984867096, + "365": 0.8571428656578064, + "366": 1.0, + "367": 1.0, + "368": 0.2857142984867096, + "369": 1.0, + "370": 0.5714285969734192, + "371": 1.0, + "372": 0.2857142984867096, + "373": 1.0, + "374": 0.7142857313156128, + "375": 1.0, + "376": 0.2857142984867096, + "377": 1.0, + "378": 0.8571428656578064, + "379": 1.0, + "380": 0.2857142984867096, + "381": 1.0, + "382": 1.0, + "383": 1.0, + "384": 0.4285714328289032, + "385": 0.0, + "386": 0.5714285969734192, + "387": 1.0, + "388": 0.4285714328289032, + "389": 0.0, + "390": 0.7142857313156128, + "391": 1.0, + "392": 0.4285714328289032, + "393": 0.0, + "394": 0.8571428656578064, + "395": 1.0, + "396": 0.4285714328289032, + "397": 0.0, + "398": 1.0, + "399": 1.0, + "400": 0.4285714328289032, + "401": 0.1428571492433548, + "402": 0.5714285969734192, + "403": 1.0, + "404": 0.4285714328289032, + "405": 0.1428571492433548, + "406": 0.7142857313156128, + "407": 1.0, + "408": 0.4285714328289032, + "409": 0.1428571492433548, + "410": 0.8571428656578064, + "411": 1.0, + "412": 0.4285714328289032, + "413": 0.1428571492433548, + "414": 1.0, + "415": 1.0, + "416": 0.4285714328289032, + "417": 0.2857142984867096, + "418": 0.5714285969734192, + "419": 1.0, + "420": 0.4285714328289032, + "421": 0.2857142984867096, + "422": 0.7142857313156128, + "423": 1.0, + "424": 0.4285714328289032, + "425": 0.2857142984867096, + "426": 0.8571428656578064, + "427": 1.0, + "428": 0.4285714328289032, + "429": 0.2857142984867096, + "430": 1.0, + "431": 1.0, + "432": 0.4285714328289032, + "433": 0.4285714328289032, + "434": 0.5714285969734192, + "435": 1.0, + "436": 0.4285714328289032, + "437": 0.4285714328289032, + "438": 0.7142857313156128, + "439": 1.0, + "440": 0.4285714328289032, + "441": 0.4285714328289032, + "442": 0.8571428656578064, + "443": 1.0, + "444": 0.4285714328289032, + "445": 0.4285714328289032, + "446": 1.0, + "447": 1.0, + "448": 0.4285714328289032, + "449": 0.5714285969734192, + "450": 0.5714285969734192, + "451": 1.0, + "452": 0.4285714328289032, + "453": 0.5714285969734192, + "454": 0.7142857313156128, + "455": 1.0, + "456": 0.4285714328289032, + "457": 0.5714285969734192, + "458": 0.8571428656578064, + "459": 1.0, + "460": 0.4285714328289032, + "461": 0.5714285969734192, + "462": 1.0, + "463": 1.0, + "464": 0.4285714328289032, + "465": 0.7142857313156128, + "466": 0.5714285969734192, + "467": 1.0, + "468": 0.4285714328289032, + "469": 0.7142857313156128, + "470": 0.7142857313156128, + "471": 1.0, + "472": 0.4285714328289032, + "473": 0.7142857313156128, + "474": 0.8571428656578064, + "475": 1.0, + "476": 0.4285714328289032, + "477": 0.7142857313156128, + "478": 1.0, + "479": 1.0, + "480": 0.4285714328289032, + "481": 0.8571428656578064, + "482": 0.5714285969734192, + "483": 1.0, + "484": 0.4285714328289032, + "485": 0.8571428656578064, + "486": 0.7142857313156128, + "487": 1.0, + "488": 0.4285714328289032, + "489": 0.8571428656578064, + "490": 0.8571428656578064, + "491": 1.0, + "492": 0.4285714328289032, + "493": 0.8571428656578064, + "494": 1.0, + "495": 1.0, + "496": 0.4285714328289032, + "497": 1.0, + "498": 0.5714285969734192, + "499": 1.0, + "500": 0.4285714328289032, + "501": 1.0, + "502": 0.7142857313156128, + "503": 1.0, + "504": 0.4285714328289032, + "505": 1.0, + "506": 0.8571428656578064, + "507": 1.0, + "508": 0.4285714328289032, + "509": 1.0, + "510": 1.0, + "511": 1.0, + "512": 0.5714285969734192, + "513": 0.0, + "514": 0.5714285969734192, + "515": 1.0, + "516": 0.5714285969734192, + "517": 0.0, + "518": 0.7142857313156128, + "519": 1.0, + "520": 0.5714285969734192, + "521": 0.0, + "522": 0.8571428656578064, + "523": 1.0, + "524": 0.5714285969734192, + "525": 0.0, + "526": 1.0, + "527": 1.0, + "528": 0.5714285969734192, + "529": 0.1428571492433548, + "530": 0.5714285969734192, + "531": 1.0, + "532": 0.5714285969734192, + "533": 0.1428571492433548, + "534": 0.7142857313156128, + "535": 1.0, + "536": 0.5714285969734192, + "537": 0.1428571492433548, + "538": 0.8571428656578064, + "539": 1.0, + "540": 0.5714285969734192, + "541": 0.1428571492433548, + "542": 1.0, + "543": 1.0, + "544": 0.5714285969734192, + "545": 0.2857142984867096, + "546": 0.5714285969734192, + "547": 1.0, + "548": 0.5714285969734192, + "549": 0.2857142984867096, + "550": 0.7142857313156128, + "551": 1.0, + "552": 0.5714285969734192, + "553": 0.2857142984867096, + "554": 0.8571428656578064, + "555": 1.0, + "556": 0.5714285969734192, + "557": 0.2857142984867096, + "558": 1.0, + "559": 1.0, + "560": 0.5714285969734192, + "561": 0.4285714328289032, + "562": 0.5714285969734192, + "563": 1.0, + "564": 0.5714285969734192, + "565": 0.4285714328289032, + "566": 0.7142857313156128, + "567": 1.0, + "568": 0.5714285969734192, + "569": 0.4285714328289032, + "570": 0.8571428656578064, + "571": 1.0, + "572": 0.5714285969734192, + "573": 0.4285714328289032, + "574": 1.0, + "575": 1.0, + "576": 0.5714285969734192, + "577": 0.5714285969734192, + "578": 0.5714285969734192, + "579": 1.0, + "580": 0.5714285969734192, + "581": 0.5714285969734192, + "582": 0.7142857313156128, + "583": 1.0, + "584": 0.5714285969734192, + "585": 0.5714285969734192, + "586": 0.8571428656578064, + "587": 1.0, + "588": 0.5714285969734192, + "589": 0.5714285969734192, + "590": 1.0, + "591": 1.0, + "592": 0.5714285969734192, + "593": 0.7142857313156128, + "594": 0.5714285969734192, + "595": 1.0, + "596": 0.5714285969734192, + "597": 0.7142857313156128, + "598": 0.7142857313156128, + "599": 1.0, + "600": 0.5714285969734192, + "601": 0.7142857313156128, + "602": 0.8571428656578064, + "603": 1.0, + "604": 0.5714285969734192, + "605": 0.7142857313156128, + "606": 1.0, + "607": 1.0, + "608": 0.5714285969734192, + "609": 0.8571428656578064, + "610": 0.5714285969734192, + "611": 1.0, + "612": 0.5714285969734192, + "613": 0.8571428656578064, + "614": 0.7142857313156128, + "615": 1.0, + "616": 0.5714285969734192, + "617": 0.8571428656578064, + "618": 0.8571428656578064, + "619": 1.0, + "620": 0.5714285969734192, + "621": 0.8571428656578064, + "622": 1.0, + "623": 1.0, + "624": 0.5714285969734192, + "625": 1.0, + "626": 0.5714285969734192, + "627": 1.0, + "628": 0.5714285969734192, + "629": 1.0, + "630": 0.7142857313156128, + "631": 1.0, + "632": 0.5714285969734192, + "633": 1.0, + "634": 0.8571428656578064, + "635": 1.0, + "636": 0.5714285969734192, + "637": 1.0, + "638": 1.0, + "639": 1.0, + "640": 0.7142857313156128, + "641": 0.0, + "642": 0.5714285969734192, + "643": 1.0, + "644": 0.7142857313156128, + "645": 0.0, + "646": 0.7142857313156128, + "647": 1.0, + "648": 0.7142857313156128, + "649": 0.0, + "650": 0.8571428656578064, + "651": 1.0, + "652": 0.7142857313156128, + "653": 0.0, + "654": 1.0, + "655": 1.0, + "656": 0.7142857313156128, + "657": 0.1428571492433548, + "658": 0.5714285969734192, + "659": 1.0, + "660": 0.7142857313156128, + "661": 0.1428571492433548, + "662": 0.7142857313156128, + "663": 1.0, + "664": 0.7142857313156128, + "665": 0.1428571492433548, + "666": 0.8571428656578064, + "667": 1.0, + "668": 0.7142857313156128, + "669": 0.1428571492433548, + "670": 1.0, + "671": 1.0, + "672": 0.7142857313156128, + "673": 0.2857142984867096, + "674": 0.5714285969734192, + "675": 1.0, + "676": 0.7142857313156128, + "677": 0.2857142984867096, + "678": 0.7142857313156128, + "679": 1.0, + "680": 0.7142857313156128, + "681": 0.2857142984867096, + "682": 0.8571428656578064, + "683": 1.0, + "684": 0.7142857313156128, + "685": 0.2857142984867096, + "686": 1.0, + "687": 1.0, + "688": 0.7142857313156128, + "689": 0.4285714328289032, + "690": 0.5714285969734192, + "691": 1.0, + "692": 0.7142857313156128, + "693": 0.4285714328289032, + "694": 0.7142857313156128, + "695": 1.0, + "696": 0.7142857313156128, + "697": 0.4285714328289032, + "698": 0.8571428656578064, + "699": 1.0, + "700": 0.7142857313156128, + "701": 0.4285714328289032, + "702": 1.0, + "703": 1.0, + "704": 0.7142857313156128, + "705": 0.5714285969734192, + "706": 0.5714285969734192, + "707": 1.0, + "708": 0.7142857313156128, + "709": 0.5714285969734192, + "710": 0.7142857313156128, + "711": 1.0, + "712": 0.7142857313156128, + "713": 0.5714285969734192, + "714": 0.8571428656578064, + "715": 1.0, + "716": 0.7142857313156128, + "717": 0.5714285969734192, + "718": 1.0, + "719": 1.0, + "720": 0.7142857313156128, + "721": 0.7142857313156128, + "722": 0.5714285969734192, + "723": 1.0, + "724": 0.7142857313156128, + "725": 0.7142857313156128, + "726": 0.7142857313156128, + "727": 1.0, + "728": 0.7142857313156128, + "729": 0.7142857313156128, + "730": 0.8571428656578064, + "731": 1.0, + "732": 0.7142857313156128, + "733": 0.7142857313156128, + "734": 1.0, + "735": 1.0, + "736": 0.7142857313156128, + "737": 0.8571428656578064, + "738": 0.5714285969734192, + "739": 1.0, + "740": 0.7142857313156128, + "741": 0.8571428656578064, + "742": 0.7142857313156128, + "743": 1.0, + "744": 0.7142857313156128, + "745": 0.8571428656578064, + "746": 0.8571428656578064, + "747": 1.0, + "748": 0.7142857313156128, + "749": 0.8571428656578064, + "750": 1.0, + "751": 1.0, + "752": 0.7142857313156128, + "753": 1.0, + "754": 0.5714285969734192, + "755": 1.0, + "756": 0.7142857313156128, + "757": 1.0, + "758": 0.7142857313156128, + "759": 1.0, + "760": 0.7142857313156128, + "761": 1.0, + "762": 0.8571428656578064, + "763": 1.0, + "764": 0.7142857313156128, + "765": 1.0, + "766": 1.0, + "767": 1.0, + "768": 0.8571428656578064, + "769": 0.0, + "770": 0.5714285969734192, + "771": 1.0, + "772": 0.8571428656578064, + "773": 0.0, + "774": 0.7142857313156128, + "775": 1.0, + "776": 0.8571428656578064, + "777": 0.0, + "778": 0.8571428656578064, + "779": 1.0, + "780": 0.8571428656578064, + "781": 0.0, + "782": 1.0, + "783": 1.0, + "784": 0.8571428656578064, + "785": 0.1428571492433548, + "786": 0.5714285969734192, + "787": 1.0, + "788": 0.8571428656578064, + "789": 0.1428571492433548, + "790": 0.7142857313156128, + "791": 1.0, + "792": 0.8571428656578064, + "793": 0.1428571492433548, + "794": 0.8571428656578064, + "795": 1.0, + "796": 0.8571428656578064, + "797": 0.1428571492433548, + "798": 1.0, + "799": 1.0, + "800": 0.8571428656578064, + "801": 0.2857142984867096, + "802": 0.5714285969734192, + "803": 1.0, + "804": 0.8571428656578064, + "805": 0.2857142984867096, + "806": 0.7142857313156128, + "807": 1.0, + "808": 0.8571428656578064, + "809": 0.2857142984867096, + "810": 0.8571428656578064, + "811": 1.0, + "812": 0.8571428656578064, + "813": 0.2857142984867096, + "814": 1.0, + "815": 1.0, + "816": 0.8571428656578064, + "817": 0.4285714328289032, + "818": 0.5714285969734192, + "819": 1.0, + "820": 0.8571428656578064, + "821": 0.4285714328289032, + "822": 0.7142857313156128, + "823": 1.0, + "824": 0.8571428656578064, + "825": 0.4285714328289032, + "826": 0.8571428656578064, + "827": 1.0, + "828": 0.8571428656578064, + "829": 0.4285714328289032, + "830": 1.0, + "831": 1.0, + "832": 0.8571428656578064, + "833": 0.5714285969734192, + "834": 0.5714285969734192, + "835": 1.0, + "836": 0.8571428656578064, + "837": 0.5714285969734192, + "838": 0.7142857313156128, + "839": 1.0, + "840": 0.8571428656578064, + "841": 0.5714285969734192, + "842": 0.8571428656578064, + "843": 1.0, + "844": 0.8571428656578064, + "845": 0.5714285969734192, + "846": 1.0, + "847": 1.0, + "848": 0.8571428656578064, + "849": 0.7142857313156128, + "850": 0.5714285969734192, + "851": 1.0, + "852": 0.8571428656578064, + "853": 0.7142857313156128, + "854": 0.7142857313156128, + "855": 1.0, + "856": 0.8571428656578064, + "857": 0.7142857313156128, + "858": 0.8571428656578064, + "859": 1.0, + "860": 0.8571428656578064, + "861": 0.7142857313156128, + "862": 1.0, + "863": 1.0, + "864": 0.8571428656578064, + "865": 0.8571428656578064, + "866": 0.5714285969734192, + "867": 1.0, + "868": 0.8571428656578064, + "869": 0.8571428656578064, + "870": 0.7142857313156128, + "871": 1.0, + "872": 0.8571428656578064, + "873": 0.8571428656578064, + "874": 0.8571428656578064, + "875": 1.0, + "876": 0.8571428656578064, + "877": 0.8571428656578064, + "878": 1.0, + "879": 1.0, + "880": 0.8571428656578064, + "881": 1.0, + "882": 0.5714285969734192, + "883": 1.0, + "884": 0.8571428656578064, + "885": 1.0, + "886": 0.7142857313156128, + "887": 1.0, + "888": 0.8571428656578064, + "889": 1.0, + "890": 0.8571428656578064, + "891": 1.0, + "892": 0.8571428656578064, + "893": 1.0, + "894": 1.0, + "895": 1.0, + "896": 1.0, + "897": 0.0, + "898": 0.5714285969734192, + "899": 1.0, + "900": 1.0, + "901": 0.0, + "902": 0.7142857313156128, + "903": 1.0, + "904": 1.0, + "905": 0.0, + "906": 0.8571428656578064, + "907": 1.0, + "908": 1.0, + "909": 0.0, + "910": 1.0, + "911": 1.0, + "912": 1.0, + "913": 0.1428571492433548, + "914": 0.5714285969734192, + "915": 1.0, + "916": 1.0, + "917": 0.1428571492433548, + "918": 0.7142857313156128, + "919": 1.0, + "920": 1.0, + "921": 0.1428571492433548, + "922": 0.8571428656578064, + "923": 1.0, + "924": 1.0, + "925": 0.1428571492433548, + "926": 1.0, + "927": 1.0, + "928": 1.0, + "929": 0.2857142984867096, + "930": 0.5714285969734192, + "931": 1.0, + "932": 1.0, + "933": 0.2857142984867096, + "934": 0.7142857313156128, + "935": 1.0, + "936": 1.0, + "937": 0.2857142984867096, + "938": 0.8571428656578064, + "939": 1.0, + "940": 1.0, + "941": 0.2857142984867096, + "942": 1.0, + "943": 1.0, + "944": 1.0, + "945": 0.4285714328289032, + "946": 0.5714285969734192, + "947": 1.0, + "948": 1.0, + "949": 0.4285714328289032, + "950": 0.7142857313156128, + "951": 1.0, + "952": 1.0, + "953": 0.4285714328289032, + "954": 0.8571428656578064, + "955": 1.0, + "956": 1.0, + "957": 0.4285714328289032, + "958": 1.0, + "959": 1.0, + "960": 1.0, + "961": 0.5714285969734192, + "962": 0.5714285969734192, + "963": 1.0, + "964": 1.0, + "965": 0.5714285969734192, + "966": 0.7142857313156128, + "967": 1.0, + "968": 1.0, + "969": 0.5714285969734192, + "970": 0.8571428656578064, + "971": 1.0, + "972": 1.0, + "973": 0.5714285969734192, + "974": 1.0, + "975": 1.0, + "976": 1.0, + "977": 0.7142857313156128, + "978": 0.5714285969734192, + "979": 1.0, + "980": 1.0, + "981": 0.7142857313156128, + "982": 0.7142857313156128, + "983": 1.0, + "984": 1.0, + "985": 0.7142857313156128, + "986": 0.8571428656578064, + "987": 1.0, + "988": 1.0, + "989": 0.7142857313156128, + "990": 1.0, + "991": 1.0, + "992": 1.0, + "993": 0.8571428656578064, + "994": 0.5714285969734192, + "995": 1.0, + "996": 1.0, + "997": 0.8571428656578064, + "998": 0.7142857313156128, + "999": 1.0, + "1000": 1.0, + "1001": 0.8571428656578064, + "1002": 0.8571428656578064, + "1003": 1.0, + "1004": 1.0, + "1005": 0.8571428656578064, + "1006": 1.0, + "1007": 1.0, + "1008": 1.0, + "1009": 1.0, + "1010": 0.5714285969734192, + "1011": 1.0, + "1012": 1.0, + "1013": 1.0, + "1014": 0.7142857313156128, + "1015": 1.0, + "1016": 1.0, + "1017": 1.0, + "1018": 0.8571428656578064, + "1019": 1.0, + "1020": 1.0, + "1021": 0.6000000238418579, + "1022": 0.0, + "1023": 0.0 + } + }, + "memory": { + "lowFraction": 0.2 + }, + "stepMove": { + "x": 50.0, + "y": 50.0, + "z": 50.0 + }, + "stepRotate": 1.0, + "stepScale": 1.0, + "camInertiaEnabled": false, + "camInertiaAmount": 0.55, + "manipulator": { + "camera": { + "flyAcceleration": 1000.0, + "flyDampening": 10.0, + "lookAcceleration": 2000.0, + "lookDampening": 20.0 + } + }, + "camVelocityMin": 0.01, + "camVelocityMax": 50, + "camVelocityScalerMin": 1, + "camVelocityScalerMax": 10, + "camVelocityScalerMultAmount": 1.1, + "camShowSpeedOnStart": false, + "camVelocityCOINormalization": 0.0, + "camStopOnMouseUp": true, + "camRotSmoothEnabled": true, + "camRotSmoothScale": 20.0, + "camRotSmoothAlways": false, + "camGestureEnabled": false, + "camGestureTime": 0.12, + "camGestureRadius": 20, + "ui": { + "background": { + "opacity": 1.0 + }, + "brightness": 0.84 + }, + "objectCentricNavigation": 0, + "coiDoubleClick": false, + "Viewport": { + "Viewport0": { + "guide": { + "axis": { + "visible": true + }, + "grid": { + "visible": true + }, + "selection": { + "visible": true + } + }, + "scene": { + "cameras": { + "visible": false + }, + "lights": { + "visible": false + }, + "skeletons": { + "visible": false + }, + "meshes": { + "visible": true + }, + "audio": { + "visible": true + } + }, + "hud": { + "renderFPS": { + "visible": true + }, + "renderResolution": { + "visible": true + }, + "renderProgress": { + "visible": true + }, + "deviceMemory": { + "visible": true + }, + "hostMemory": { + "visible": true + }, + "processMemory": { + "visible": true + }, + "visible": true, + "cameraSpeed": { + "visible": true + }, + "toastMessage": { + "visible": true + } + }, + "resolution": { + "0": 1280, + "1": 720 + } + } + }, + "grid": { + "scale": 1.0, + "lineWidth": 1, + "lineFadeOutStartDistance": 10.0, + "lineColor": { + "0": 0.3, + "1": 0.3, + "2": 0.3 + }, + "lineFadeOutEndDistance": 20.0, + "offset": 0.009999999776482582 + }, + "boundingBoxes": { + "lineColor": { + "0": 0.886, + "1": 0.447, + "2": 0.447 + }, + "hideWhenManipulating": false + }, + "gizmo": { + "scale": 0.01, + "lineWidth": 1.0, + "constantScaleEnabled": true, + "constantScale": 10.0, + "minFadeOut": 1.0, + "maxFadeOut": 50, + "constantScaleCamera": false + } + }, + "extensions": { + "console": { + "filterLevel": 0, + "sources": { + "carb.crashreporter-breakpad.plugin": true, + "carb.windowing-glfw.gamepad": true, + "omni.replicator.core.scripts.annotators": true, + "omni.kit.widget.cache_indicator.cache_state_menu": true, + "omni.kit.profiler.window": true, + "carb.audio.device": true, + "carb.audio.output": true, + "carb.audio.context": true + } + } + }, + "exts": { + "enabled": { + "0": "airlab.airstack-1.19.1", + } + }, + "window": { + "uiStyle": "NvidiaDark", + "maximized": false, + "width": 1440, + "height": 900 + } + } + } +} diff --git a/simulation/isaac-sim/installation/download_sitl.bash b/simulation/isaac-sim/installation/download_sitl.bash new file mode 100644 index 00000000..569af25e --- /dev/null +++ b/simulation/isaac-sim/installation/download_sitl.bash @@ -0,0 +1,20 @@ +#!/bin/bash + +echo "Installing gdown package" + +pip install gdown + +echo "Downloading the SITL package from Google Drive" + +gdown https://drive.google.com/uc\?id\=1UxgezaTrHe4WJ28zsVeRhv1VYfOU5VK8 + + +SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )" + +UNZIP_DIR=$SCRIPT_DIR/../sitl_integration + +echo "Unzipping the SITL package to $(readlink -f $UNZIP_DIR)" + +unzip AscentAeroSystemsSITLPackage.zip -d $UNZIP_DIR + +rm AscentAeroSystemsSITLPackage.zip diff --git a/simulation/isaac-sim/sitl_integration/config/extension.toml b/simulation/isaac-sim/sitl_integration/config/extension.toml new file mode 100644 index 00000000..b640daac --- /dev/null +++ b/simulation/isaac-sim/sitl_integration/config/extension.toml @@ -0,0 +1,25 @@ +[core] +reloadable = true +order = 0 + +[package] +version = "1.0.0" +category = "Simulation" +title = "TEST_EXTENSION_TITLE" +description = "EXTENSION_DESCRIPTION" +authors = ["NVIDIA"] +repository = "" +keywords = [] +changelog = "docs/CHANGELOG.md" +readme = "docs/README.md" +preview_image = "data/preview.png" +icon = "data/icon.png" + + +[dependencies] +"omni.kit.uiapp" = {} +"omni.isaac.ui" = {} +"omni.isaac.core" = {} + +[[python.module]] +name = "TEST_EXTENSION_TITLE_python" \ No newline at end of file diff --git a/simulation/isaac-sim/sitl_integration/config/omniverse.toml b/simulation/isaac-sim/sitl_integration/config/omniverse.toml new file mode 100644 index 00000000..acd5cb4b --- /dev/null +++ b/simulation/isaac-sim/sitl_integration/config/omniverse.toml @@ -0,0 +1,4 @@ + +[bookmarks] +"AirLab Nucleus Server" = "omniverse://airlab-storage.andrew.cmu.edu:8443" +"AirStack" = "omniverse://airlab-storage.andrew.cmu.edu:8443/Projects/AirStack/" diff --git a/simulation/isaac-sim/sitl_integration/data/icon.png b/simulation/isaac-sim/sitl_integration/data/icon.png new file mode 100644 index 00000000..9a2d57bd Binary files /dev/null and b/simulation/isaac-sim/sitl_integration/data/icon.png differ diff --git a/simulation/isaac-sim/sitl_integration/data/preview.png b/simulation/isaac-sim/sitl_integration/data/preview.png new file mode 100644 index 00000000..2442a54f Binary files /dev/null and b/simulation/isaac-sim/sitl_integration/data/preview.png differ diff --git a/simulation/isaac-sim/sitl_integration/docs/CHANGELOG/index.html b/simulation/isaac-sim/sitl_integration/docs/CHANGELOG/index.html new file mode 100644 index 00000000..c2be4ff6 --- /dev/null +++ b/simulation/isaac-sim/sitl_integration/docs/CHANGELOG/index.html @@ -0,0 +1,2608 @@ + + + + + + + + + + + + + + + + + + + Changelog - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

Changelog

+

[0.1.0] - 2024-07-10

+

Added

+
    +
  • Initial version of TEST_EXTENSION_TITLE Extension
    • +
+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ +
+ + + +
+
+
+ +
+ Copyright © 2024 - 2030 AirLab CMU +
+ + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + +
+ +
+
+
+ +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/simulation/isaac-sim/sitl_integration/docs/index.html b/simulation/isaac-sim/sitl_integration/docs/index.html new file mode 100644 index 00000000..e97778a5 --- /dev/null +++ b/simulation/isaac-sim/sitl_integration/docs/index.html @@ -0,0 +1,2604 @@ + + + + + + + + + + + + + + + + + + + Usage - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

Usage

+

To enable this extension, run Isaac Sim with the flags --ext-folder {path_to_ext_folder} --enable {ext_directory_name}

+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ +
+ + + +
+
+
+ +
+ Copyright © 2024 - 2030 AirLab CMU +
+ + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + +
+ +
+
+
+ +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/simulation/isaac-sim/sitl_integration/drag_and_drop/Makefile b/simulation/isaac-sim/sitl_integration/drag_and_drop/Makefile new file mode 100644 index 00000000..bcb26a80 --- /dev/null +++ b/simulation/isaac-sim/sitl_integration/drag_and_drop/Makefile @@ -0,0 +1,2 @@ +all: + g++ inject.cpp -fPIC -shared -o inject.so diff --git a/simulation/isaac-sim/sitl_integration/drag_and_drop/ascent_sitl.yaml b/simulation/isaac-sim/sitl_integration/drag_and_drop/ascent_sitl.yaml new file mode 100644 index 00000000..5226507d --- /dev/null +++ b/simulation/isaac-sim/sitl_integration/drag_and_drop/ascent_sitl.yaml @@ -0,0 +1,28 @@ +session_name: Ascent Spirit +windows: + - window_name: SITL + layout: tiled + panes: + - shell_command: + - cd /AscentAeroSystemsSITLPackage + - gdb spirit_sitl -q --command=/sitl_integration/drag_and_drop/test.gdb -ex "r -S --model coaxial -I0 --base-port $BASE_PORT" + - shell_command: + - echo $ASCENT_SITL_PORT + - echo $ISAAC_SIM_PORT + - echo $AUTONOMY_STACK_PORT + - mavproxy.py --streamrate=100 --master tcp:127.0.0.1:$BASE_PORT --out udp:127.0.0.1:$ASCENT_SITL_PORT --out udp:127.0.0.1:$ISAAC_SIM_PORT --out udp:127.0.0.1:$AUTONOMY_STACK_PORT + - shell_command: + # - sleep 5 + - export ROS_DOMAIN_ID=$DOMAIN_ID + # - ros2 launch mavros apm.launch fcu_url:="udp://127.0.0.1:$AUTONOMY_STACK_PORT@$MAVROS_LAUNCH_PORT" namespace:=$NAMESPACE/interface/mavros --ros-args -p use_sim_time:=true + - > + ros2 run mavros mavros_node + --ros-args + -r __ns:=/$NAMESPACE/interface/mavros + -p fcu_url:="udp://127.0.0.1:$AUTONOMY_STACK_PORT@$MAVROS_LAUNCH_PORT" + -p tgt_system:=1 + -p tgt_component:=1 + -p fcu_protocol:=v2.0 + --params-file $(ros2 pkg prefix mavros)/share/mavros/launch/apm_pluginlists.yaml + --params-file $(ros2 pkg prefix mavros)/share/mavros/launch/apm_config.yaml + -p use_sim_time:=true \ No newline at end of file diff --git a/simulation/isaac-sim/sitl_integration/drag_and_drop/ascent_sitl_launch_tool.py b/simulation/isaac-sim/sitl_integration/drag_and_drop/ascent_sitl_launch_tool.py new file mode 100644 index 00000000..10931406 --- /dev/null +++ b/simulation/isaac-sim/sitl_integration/drag_and_drop/ascent_sitl_launch_tool.py @@ -0,0 +1,109 @@ +""" + +""" +# System tools used to launch the sitl process in the brackground +import os +import tempfile +import subprocess + + +class AscentSitlLaunchTool: + """ + A class that manages the start/stop of a sitl process. It requires only the path to the sitl installation (assuming that + sitl was already built with 'make sitl_sitl_default none'), the vehicle id and the vehicle model. + """ + + def __init__(self, sitl_dir, vehicle_id: int = 0): + """Construct the sitlLaunchTool object + + Args: + sitl_dir (str): A string with the path to the sitl-Autopilot directory + vehicle_id (int): The ID of the vehicle. Defaults to 0. + sitl_model (str): The vehicle model. Defaults to "iris". + """ + + + # Attribute that will hold the sitl process once it is running + self.sitl_process = None + + # The vehicle id (used for the mavlink port open in the system) + self.vehicle_id = vehicle_id + + self.base_port = 5760 + self.vehicle_id*10 + self.ascent_sitl_port = 14552 + self.vehicle_id*10 + self.isaac_sim_port = 14553 + self.vehicle_id*10 + self.autonomy_stack_port = 14554 + self.vehicle_id*10 + self.mavros_launch_port = 14555 + self.vehicle_id*10 + + # Configurations to whether autostart sitl (SITL) automatically or have the user launch it manually on another + # terminal + self.sitl_dir = sitl_dir + #self.sitl_script = self.sitl_dir + "/launch_ascent_sitl.bash" + self.sitl_script = '/usr/bin/bash ' + self.sitl_dir + '/ascent_sitl_tmux.bash ' + \ + str(self.base_port) + ' ' + str(self.ascent_sitl_port) + ' ' + \ + str(self.isaac_sim_port) + ' ' + str(self.autonomy_stack_port) + ' ' + str(self.mavros_launch_port) + \ + ' ' + str(self.vehicle_id) + + # Create a temporary filesystem for sitl to write data to/from (and modify the origin rcS files) + self.root_fs = tempfile.TemporaryDirectory() + + # Set the environement variables + self.environment = os.environ + # self.environment["sitl_SIM_MODEL"] = sitl_model + + def get_dronekit_address(self): + return '127.0.0.1:' + str(self.isaac_sim_port) + + def launch(self): + """ + Method that will launch a sitl instance with the specified configuration + """ + self.sitl_process = subprocess.Popen(self.sitl_script, shell=True) + ''' + self.sitl_process = subprocess.Popen( + [ + "bash", + self.sitl_dir + '/ascent_sitl_tmux.bash' + ], + cwd=self.root_fs.name, + shell=False, + env=self.environment, + ) + #''' + + def kill_sitl(self): + """ + Method that will kill a sitl instance with the specified configuration + """ + if self.sitl_process is not None: + self.sitl_process.kill() + self.sitl_process = None + + def __del__(self): + """ + If the sitl process is still running when the sitl launch tool object is whiped from memory, then make sure + we kill the sitl instance so we don't end up with hanged sitl instances + """ + + # Make sure the sitl process gets killed + if self.sitl_process: + self.kill_sitl() + + # Make sure we clean the temporary filesystem used for the simulation + self.root_fs.cleanup() + + +# ---- Code used for debugging the sitl launch tool ---- +def main(): + + script_dir = os.path.dirname(os.path.realpath(__file__)) + sitl_tool = AscentSitlLaunchTool(script_dir) + sitl_tool.launch() + + import time + + time.sleep(60) + + +if __name__ == "__main__": + main() diff --git a/simulation/isaac-sim/sitl_integration/drag_and_drop/ascent_sitl_tmux.bash b/simulation/isaac-sim/sitl_integration/drag_and_drop/ascent_sitl_tmux.bash new file mode 100644 index 00000000..e3e66d37 --- /dev/null +++ b/simulation/isaac-sim/sitl_integration/drag_and_drop/ascent_sitl_tmux.bash @@ -0,0 +1,20 @@ +#!/bin/bash + +SCRIPT_DIR=$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd ) +cd $SCRIPT_DIR/ + +#export BASE_PORT=5760 +#export ASCENT_SITL_PORT=14552 # port to talk to QGC +#export ISAAC_SIM_PORT=14553 # port for Isaac Sim +#export AUTONOMY_STACK_PORT=14554 # port for our autonomy stack + +export BASE_PORT=$1 +export ASCENT_SITL_PORT=$2 # port to talk to QGC +export ISAAC_SIM_PORT=$3 # port for Isaac Sim +export AUTONOMY_STACK_PORT=$4 # port for our autonomy stack +export MAVROS_LAUNCH_PORT=$5 +export SESSION_NAME=$6 +export DOMAIN_ID=$7 +export NAMESPACE=$8 + +tmuxp load -d ascent_sitl.yaml -s $SESSION_NAME diff --git a/simulation/isaac-sim/sitl_integration/drag_and_drop/behavior_script.py b/simulation/isaac-sim/sitl_integration/drag_and_drop/behavior_script.py new file mode 100644 index 00000000..cdac5d86 --- /dev/null +++ b/simulation/isaac-sim/sitl_integration/drag_and_drop/behavior_script.py @@ -0,0 +1,177 @@ +from omni.kit.scripting import BehaviorScript +import omni.ext +import omni.ui as ui +import omni.kit.commands + +from omni.isaac.core.utils.stage import get_current_stage +from omni.isaac.core.utils.prims import define_prim, get_prim_at_path + +import sys +import dronekit +import os +import numpy as np +from scipy.spatial.transform import Rotation +import carb +import omni +import omni.graph.core as og +import omni.replicator.core as rep +import omni.timeline +import usdrt.Sdf +from omni.isaac.core.prims import GeometryPrim, RigidPrim +from omni.isaac.core.utils import extensions, stage +from omni.isaac.core.world import World +from pxr import Gf, Usd, UsdGeom +from pxr.Gf import Quatf +from .ascent_sitl_launch_tool import AscentSitlLaunchTool +import time +import threading +import struct +import socket + +import asyncio + +#========================================================================================== +# ------------------------------------- Time Sync Server ---------------------------------- +#========================================================================================== + +mutex = threading.Lock() +client_count = 1 +current_sim_time = 0. +def get_sim_time(): + return current_sim_time + +def handle_client(conn, addr): + global client_count + print(f'Connected by {addr}') + + initial_sitl_time = -1. + initial_sim_time = -1. + + with conn: + while True: + data = conn.recv(16) + if not data: + break + + #print('data', len(data), data) + message_type, t = struct.unpack('cQ', data) + #print('message type', message_type, t) + if message_type == b't': + s = get_sim_time() + + if initial_sitl_time < 0: + initial_sitl_time = t + initial_sim_time = s + + sitl_time = t - initial_sitl_time + sim_time = (s - initial_sim_time)*1000000 + time_to_sleep = int(sitl_time - sim_time) + + conn.sendall(struct.pack('i', time_to_sleep)) + elif message_type == b'n': + print('message type', message_type, t) + with mutex: + client_count += 1 + conn.sendall(struct.pack('i', client_count)) + +def start_server(host='127.0.0.1', port=65432): + try: + s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + s.bind((host, port)) + s.listen() + print(f'Server listening on {host}:{port}') + def f(): + while True: + conn, addr = s.accept() + client_thread = threading.Thread(target=handle_client, args=(conn, addr)) + client_thread.start() + threading.Thread(target=f).start() + return 1 + except: + client = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + client.connect((host, port)) + message = struct.pack('cQ', b'n', 0) + client.sendall(message) + drone_count = struct.unpack('i', client.recv(4))[0] + return drone_count + + +#========================================================================================== +# ------------------------------------- Behavior Script ----------------------------------- +#========================================================================================== + +class TestScript(BehaviorScript): + def on_init(self): + print('on init') + #print('PRIM PATH', self.prim_path) + self.p = self.stage.GetPrimAtPath(self.prim_path) + self.prop_prim = self.stage.GetPrimAtPath(str(self.prim_path) + '/spirit_uav/meshes/mesh_17') + #print('YOOOOOOOOOOOO', str(self.prim_path) + '/spirit_uav/meshes/mesh_17', self.prop_prim) + #print(self.p.GetAttributes()) + self.p.GetAttribute('xformOp:translate').Set((0,0, 0)) + self.q_type = type(self.p.GetAttribute("xformOp:orient").Get()) + self.p.GetAttribute('xformOp:orient').Set(self.p.GetAttribute("xformOp:orient").Get()) + #print(self.p.GetAttribute("xformOp:translate").Get()) + self.dronekit_connection = None + self.initialized = False + self.drone_count = 0 + #''' + self.prop_rotation_q = Quatf(0.9999619, 0, 0.0087265, 0) # 1 degree + for i in range(7): + self.prop_rotation_q *= self.prop_rotation_q + #print(self.prop_prim.GetAttribute('xformOp:orient')) + self.prop_prim.GetAttribute('xformOp:orient').Set(self.prop_prim.GetAttribute("xformOp:orient").Get()* + self.prop_rotation_q) + #''' + print('on init done') + + def on_update(self, current_time, delta_time): + #print('update', current_time, delta_time) + global current_sim_time + current_sim_time = current_time + + if self.dronekit_connection == None: + if not self.initialized: + self.initialized = True + script_dir = os.path.dirname(os.path.realpath(__file__)) + '/' + self.drone_count = start_server() + self.sitl_tool = AscentSitlLaunchTool(script_dir, self.drone_count) + self.sitl_tool.launch() + #self.thread = threading.Thread(target=start_server) + #self.thread.start() + def f(): + self.dronekit_connection = dronekit.connect(self.sitl_tool.get_dronekit_address(), + wait_ready=True, timeout=999999, rate=120) + threading.Thread(target=f).start() + else: + r = self.dronekit_connection._roll# + np.pi / 2 + p = self.dronekit_connection._pitch + y = self.dronekit_connection._yaw + + rot = Rotation.from_euler("xyz", [r, p, y], degrees=False) + # quaternion: xyzw + q = rot.as_quat() + + o = self.q_type(q[3], q[0], q[1], q[2]) + + n, e, d = ( + self.dronekit_connection.location.local_frame.east, + self.dronekit_connection.location.local_frame.north, + self.dronekit_connection.location.local_frame.down, + ) + if d is not None: + # quaternion: wxyz + p = (e, n, -d) # enu + #self.p.set_world_pose(p, o) + self.p.GetAttribute('xformOp:translate').Set(p) + self.p.GetAttribute('xformOp:orient').Set(o) + #''' + self.prop_prim.GetAttribute('xformOp:orient').Set(self.prop_prim.GetAttribute("xformOp:orient").Get()* + self.prop_rotation_q) + #''' + else: + #print("Drone location from dronekit is None") + pass + + def on_destroy(self): + print('destroy') diff --git a/simulation/isaac-sim/sitl_integration/drag_and_drop/index.html b/simulation/isaac-sim/sitl_integration/drag_and_drop/index.html new file mode 100644 index 00000000..c0c7eb8f --- /dev/null +++ b/simulation/isaac-sim/sitl_integration/drag_and_drop/index.html @@ -0,0 +1,2600 @@ + + + + + + + + + + + + + + + + + + + Index - AirStack + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+
+ +
+ + + + +
+ + +
+ +
+ + + + + + + + + +
+
+ + + +
+
+
+ + + + + + + + + +
+
+
+ + + + +
+
+ + + + + + + +

Index

+ +

Download the AscentAeroSystemsSITLPackage.zip from Google Drive and unzip into this directory.

+ + + + + + + + + + + + + +
+
+ + + +
+ + + +
+ +
+ + + +
+
+
+ +
+ Copyright © 2024 - 2030 AirLab CMU +
+ + +
+ +
+ + + + + + + + + + + + + + + + + + + + + + + +
+ +
+
+
+ +
+
+
+
+ + + + + + + + + + \ No newline at end of file diff --git a/simulation/isaac-sim/sitl_integration/drag_and_drop/inject.cpp b/simulation/isaac-sim/sitl_integration/drag_and_drop/inject.cpp new file mode 100644 index 00000000..0ca3bd77 --- /dev/null +++ b/simulation/isaac-sim/sitl_integration/drag_and_drop/inject.cpp @@ -0,0 +1,164 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +unsigned int alarm(unsigned int seconds){ + return 0; +} + +bool enable_usleep = false; +int (*usleep_func)(useconds_t) = NULL; + +int usleep(useconds_t usec){ + //if (!usleep_func) + // usleep_func = (int(*)(useconds_t)) dlsym(RTLD_NEXT, "usleep"); + return 0; + //std::cout << "USLEEP " << usleep_counter << std::endl; + //usleep_counter++; + //return 0; + + //static int (*usleep_func)(useconds_t) = NULL; + if (!usleep_func) + usleep_func = (int(*)(useconds_t)) dlsym(RTLD_NEXT, "usleep"); + + if(enable_usleep && usec != 1000){ + //std::cout << "USLEEP: " << usec << std::endl; + usleep_func(usec); + } + + return 0; +} + +int nanosleep(const struct timespec *duration, + struct timespec * rem){ + std::cout << "NANOSLEEP 2" << std::endl; + return 0; +} + +int clock_nanosleep(clockid_t clockid, int flags, + const struct timespec *request, + struct timespec *remain) { + std::cout << "NANOSLEEP" << std::endl; + return 0; +} + + +int sockfd = -1; +struct sockaddr_in servaddr; + +static __attribute__((constructor)) void init(void){ + usleep_func = (int(*)(useconds_t)) dlsym(RTLD_NEXT, "usleep"); + /* + // Create socket + std::cout << "1" << std::endl; + if ((sockfd = socket(AF_INET, SOCK_STREAM, 0)) < 0) { + std::cerr << "Socket creation failed" << std::endl; + exit(1); + } + + std::cout << "2" << std::endl; + memset(&servaddr, 0, sizeof(servaddr)); + + // Server information + std::cout << "3" << std::endl; + servaddr.sin_family = AF_INET; + servaddr.sin_port = htons(65432); + if (inet_pton(AF_INET, "127.0.0.1", &servaddr.sin_addr) <= 0) { + std::cerr << "Invalid address/ Address not supported" << std::endl; + exit(1); + } + + // Connect to server + std::cout << "4" << std::endl; + if (connect(sockfd, (struct sockaddr *)&servaddr, sizeof(servaddr)) < 0) { + std::cerr << "Connection Failed" << std::endl; + exit(1); + } + //*/ +} + +void injected_function(unsigned long i){ + if(i < 41077729){//usleep_counter < usleep_limit) + + return; + } + else + enable_usleep = true; + + static auto start = std::chrono::high_resolution_clock::now(); + auto elapsed = std::chrono::high_resolution_clock::now() - start; + long long microseconds = std::chrono::duration_cast(elapsed).count(); + static long long prev_microseconds = microseconds; + static long long prev_i = i; + //std::cout << "injected function " << microseconds << " " << (microseconds - prev_microseconds) << " " << i << " " << (i - prev_i) << " " << (i - microseconds) << std::endl; + //std::cout << "size: " << sizeof(unsigned long) << std::endl; + prev_i = i; + prev_microseconds = microseconds; + + + if (sockfd == -1){ + // Create socket + //std::cout << "1" << std::endl; + if ((sockfd = socket(AF_INET, SOCK_STREAM, 0)) < 0) { + std::cerr << "Socket creation failed" << std::endl; + exit(1); + } + + //std::cout << "2" << std::endl; + memset(&servaddr, 0, sizeof(servaddr)); + + // Server information + //std::cout << "3" << std::endl; + servaddr.sin_family = AF_INET; + servaddr.sin_port = htons(65432); + if (inet_pton(AF_INET, "127.0.0.1", &servaddr.sin_addr) <= 0) { + std::cerr << "Invalid address/ Address not supported" << std::endl; + exit(1); + } + + // Connect to server + //std::cout << "4" << std::endl; + if (connect(sockfd, (struct sockaddr *)&servaddr, sizeof(servaddr)) < 0) { + std::cerr << "Connection Failed" << std::endl; + exit(1); + } + } + + // Send message + //* + char message_type = 't'; + uint8_t message[16];//sizeof(message_type) + sizeof(i)]; + std::memcpy(&message[0], &message_type, sizeof(message_type)); + std::memcpy(&message[sizeof(i)], &i, sizeof(i)); + send(sockfd, message, sizeof(message), 0); + //send(sockfd, &i, sizeof(i), 0); + + // Receive response + char buffer[1024] = {0}; + int time_to_sleep; + //int valread = read(sockfd, buffer, 1024); + int valread = read(sockfd, &time_to_sleep, sizeof(time_to_sleep)); + //std::cout << "Server: " << buffer << std::endl; + //std::cout << "Server: " << time_to_sleep << std::endl; + if(time_to_sleep == 1000) + time_to_sleep = 1001; + if(time_to_sleep > 0) + if(usleep_func != NULL) + usleep_func(time_to_sleep); + //*/ +} + + +static __attribute__((destructor)) void end(void){ + close(sockfd); +} diff --git a/simulation/isaac-sim/sitl_integration/drag_and_drop/inject.so b/simulation/isaac-sim/sitl_integration/drag_and_drop/inject.so new file mode 100644 index 00000000..7bd0d8db Binary files /dev/null and b/simulation/isaac-sim/sitl_integration/drag_and_drop/inject.so differ diff --git a/simulation/isaac-sim/sitl_integration/drag_and_drop/launch_ascent_sitl.bash b/simulation/isaac-sim/sitl_integration/drag_and_drop/launch_ascent_sitl.bash new file mode 100644 index 00000000..70203369 --- /dev/null +++ b/simulation/isaac-sim/sitl_integration/drag_and_drop/launch_ascent_sitl.bash @@ -0,0 +1,33 @@ +#!/bin/bash + +# Description: this script launches the AscentAeroSystems SITL package +# Author: Andrew Jong (ajong@andrew.cmu.edu) +# Copyright (c) 2024. This file is developed as part of software from the AirLab at the +# Robotics Institute at Carnegie Mellon University (https://theairlab.org). + +set -ex + +# kill all child processes on interrupt +trap 'pkill -P $$; exit' SIGINT SIGTERM + + +SCRIPT_DIR=$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd ) +ASCENT_DIR="$SCRIPT_DIR/AscentAeroSystemsSITLPackage" + +ASCENT_SITL_PORT=14552 # port to talk to QGC +ISAAC_SIM_PORT=14553 # port for Isaac Sim +AUTONOMY_STACK_PORT=14554 # port for our autonomy stack + + +# launch QGC, SITL, and mavproxy as child processes + +cd $ASCENT_DIR +# ./AscentQLinux/AscentQ & \ +./spirit_sitl -S --model coaxial -I0 & \ +\ +mavproxy.py --streamrate=100 --master tcp:127.0.0.1:5760 \ + --out udp:127.0.0.1:$ASCENT_SITL_PORT \ + --out udp:127.0.0.1:$ISAAC_SIM_PORT \ + --out udp:127.0.0.1:$AUTONOMY_STACK_PORT + +wait diff --git a/simulation/isaac-sim/sitl_integration/drag_and_drop/server_test.py b/simulation/isaac-sim/sitl_integration/drag_and_drop/server_test.py new file mode 100644 index 00000000..6460795d --- /dev/null +++ b/simulation/isaac-sim/sitl_integration/drag_and_drop/server_test.py @@ -0,0 +1,55 @@ +import socket +import threading +import time +import struct + +sim_start_time = time.time() + + +def get_sim_time(): + return (time.time() - sim_start_time)/2. + +def handle_client(conn, addr): + print(f'Connected by {addr}') + + initial_sitl_time = -1. + initial_sim_time = -1. + + with conn: + while True: + data = conn.recv(8) + print('data', data) + if not data: + break + + t = struct.unpack('Q', data)[0] + s = get_sim_time() + + if initial_sitl_time < 0: + print('initial') + initial_sitl_time = t + initial_sim_time = s + + sitl_time = t - initial_sitl_time + sim_time = (s - initial_sim_time)*1000000 + time_to_sleep = int(sitl_time - sim_time) + print('time to sleep', time_to_sleep, sitl_time, sim_time) + + #print(f'Received {data.decode()} from {addr}') + #time.sleep(0.01) + #conn.sendall(b'Hello from server') + conn.sendall(struct.pack('i', time_to_sleep)) + +def start_server(host='127.0.0.1', port=65432): + with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s: + s.bind((host, port)) + s.listen() + print(f'Server listening on {host}:{port}') + while True: + conn, addr = s.accept() + client_thread = threading.Thread(target=handle_client, args=(conn, addr)) + client_thread.start() + print(f'Active connections: {threading.active_count() - 1}') + +if __name__ == "__main__": + start_server() diff --git a/simulation/isaac-sim/sitl_integration/drag_and_drop/test.gdb b/simulation/isaac-sim/sitl_integration/drag_and_drop/test.gdb new file mode 100644 index 00000000..8096897c --- /dev/null +++ b/simulation/isaac-sim/sitl_integration/drag_and_drop/test.gdb @@ -0,0 +1,13 @@ +set environment LD_PRELOAD /sitl_integration/drag_and_drop/inject.so + +b _ZN7HALSITL9Scheduler10stop_clockEm +catch signal SIGSEGV + +commands 1 + silent + #info registers rsi + call (void)injected_function($rsi) + continue + +commands 2 + bt \ No newline at end of file diff --git a/simulation/isaac-sim/sitl_integration/inputrc b/simulation/isaac-sim/sitl_integration/inputrc new file mode 100644 index 00000000..230e66b8 --- /dev/null +++ b/simulation/isaac-sim/sitl_integration/inputrc @@ -0,0 +1,67 @@ +# /etc/inputrc - global inputrc for libreadline +# See readline(3readline) and `info rluserman' for more information. + +# Be 8 bit clean. +set input-meta on +set output-meta on + +# To allow the use of 8bit-characters like the german umlauts, uncomment +# the line below. However this makes the meta key not work as a meta key, +# which is annoying to those which don't need to type in 8-bit characters. + +# set convert-meta off + +# try to enable the application keypad when it is called. Some systems +# need this to enable the arrow keys. +# set enable-keypad on + +# see /usr/share/doc/bash/inputrc.arrows for other codes of arrow keys + +# do not bell on tab-completion +# set bell-style none +# set bell-style visible + +# some defaults / modifications for the emacs mode +$if mode=emacs + +# allow the use of the Home/End keys +"\e[1~": beginning-of-line +"\e[4~": end-of-line + +# allow the use of the Delete/Insert keys +"\e[3~": delete-char +"\e[2~": quoted-insert + +# mappings for "page up" and "page down" to step to the beginning/end +# of the history +# "\e[5~": beginning-of-history +# "\e[6~": end-of-history + +# alternate mappings for "page up" and "page down" to search the history +"\e[5~": history-search-backward +"\e[6~": history-search-forward + +# mappings for Ctrl-left-arrow and Ctrl-right-arrow for word moving +"\e[1;5C": forward-word +"\e[1;5D": backward-word +"\e[5C": forward-word +"\e[5D": backward-word +"\e\e[C": forward-word +"\e\e[D": backward-word + +$if term=rxvt +"\e[7~": beginning-of-line +"\e[8~": end-of-line +"\eOc": forward-word +"\eOd": backward-word +$endif + +# for non RH/Debian xterm, can't hurt for RH/Debian xterm +# "\eOH": beginning-of-line +# "\eOF": end-of-line + +# for freebsd console +# "\e[H": beginning-of-line +# "\e[F": end-of-line + +$endif diff --git a/sitemap.xml b/sitemap.xml new file mode 100644 index 00000000..0f8724ef --- /dev/null +++ b/sitemap.xml @@ -0,0 +1,3 @@ + + + \ No newline at end of file diff --git a/sitemap.xml.gz b/sitemap.xml.gz new file mode 100644 index 00000000..8175f97d Binary files /dev/null and b/sitemap.xml.gz differ diff --git a/stylesheets/extra.css b/stylesheets/extra.css new file mode 100644 index 00000000..c87744e8 --- /dev/null +++ b/stylesheets/extra.css @@ -0,0 +1,9 @@ +:root { + --md-primary-fg-color: #EE0F0F; + --md-primary-fg-color--light: #ECB7B7; + --md-primary-fg-color--dark: #90030C; + } + + [data-md-color-scheme="slate"] { + --md-hue: 210; + } 
# start another terminal in docker container
+docker exec -it airstack-robot-1 bash
+# in docker
+# FLY TO POSITION. Put whatever position you want
+ros2 topic pub /robot_1/interface/mavros/setpoint_position/local geometry_msgs/PoseStamped \
+    "{ header: { stamp: { sec: 0, nanosec: 0 }, frame_id: 'base_link' }, \
+    pose: { position: { x: 10.0, y: 0.0, z: 20.0 }, orientation: { x: 0.0, y: 0.0, z: 0.0, w: 1.0 } } }" -1
+