mirrors/rviz_navi_multi_goals_pub_plugin
1
0
Fork 0
mirror of https://github.com/autolaborcenter/rviz_navi_multi_goals_pub_plugin.git synced 2026-01-11 19:10:02 +08:00
Code Issues Packages Projects Releases Wiki Activity
Files
master
rviz_navi_multi_goals_pub_p…/develop.html
doubleTing 979d0da0c9 Create develop.html
2020-11-09 17:45:41 +08:00

1518 lines
48 KiB
HTML
Raw Permalink Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
<!DOCTYPE html><html><head><meta charset="utf-8"><meta name="viewport" content="width=device-width, initial-scale=1"><style>body {
max-width: 980px;
border: 1px solid #ddd;
outline: 1300px solid #fff;
margin: 16px auto;
}
body .markdown-body
{
padding: 45px;
}
@font-face {
font-family: fontawesome-mini;
src: url(data:font/woff;charset=utf-8;base64,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) format('woff');
}
.markdown-body {
font-family: sans-serif;
-ms-text-size-adjust: 100%;
-webkit-text-size-adjust: 100%;
color: #333333;
overflow: hidden;
font-family: "Helvetica Neue", Helvetica, "Segoe UI", Arial, freesans, sans-serif;
font-size: 16px;
line-height: 1.6;
word-wrap: break-word;
}
.markdown-body a {
background: transparent;
}
.markdown-body a:active,
.markdown-body a:hover {
outline: 0;
}
.markdown-body b,
.markdown-body strong {
font-weight: bold;
}
.markdown-body mark {
background: #ff0;
color: #000;
font-style: italic;
font-weight: bold;
}
.markdown-body sub,
.markdown-body sup {
font-size: 75%;
line-height: 0;
position: relative;
vertical-align: baseline;
}
.markdown-body sup {
top: -0.5em;
}
.markdown-body sub {
bottom: -0.25em;
}
.markdown-body h1 {
font-size: 2em;
margin: 0.67em 0;
}
.markdown-body img {
border: 0;
}
.markdown-body hr {
-moz-box-sizing: content-box;
box-sizing: content-box;
height: 0;
}
.markdown-body pre {
overflow: auto;
}
.markdown-body code,
.markdown-body kbd,
.markdown-body pre,
.markdown-body samp {
font-family: monospace, monospace;
font-size: 1em;
}
.markdown-body input {
color: inherit;
font: inherit;
margin: 0;
}
.markdown-body html input[disabled] {
cursor: default;
}
.markdown-body input {
line-height: normal;
}
.markdown-body input[type="checkbox"] {
box-sizing: border-box;
padding: 0;
}
.markdown-body table {
border-collapse: collapse;
border-spacing: 0;
}
.markdown-body td,
.markdown-body th {
padding: 0;
}
.markdown-body .codehilitetable {
border: 0;
border-spacing: 0;
}
.markdown-body .codehilitetable tr {
border: 0;
}
.markdown-body .codehilitetable pre,
.markdown-body .codehilitetable div.codehilite {
margin: 0;
}
.markdown-body .linenos,
.markdown-body .code,
.markdown-body .codehilitetable td {
border: 0;
padding: 0;
}
.markdown-body td:not(.linenos) .linenodiv {
padding: 0 !important;
}
.markdown-body .code {
width: 100%;
}
.markdown-body .linenos div pre,
.markdown-body .linenodiv pre,
.markdown-body .linenodiv {
border: 0;
-webkit-border-radius: 0;
-moz-border-radius: 0;
border-radius: 0;
-webkit-border-top-left-radius: 3px;
-webkit-border-bottom-left-radius: 3px;
-moz-border-radius-topleft: 3px;
-moz-border-radius-bottomleft: 3px;
border-top-left-radius: 3px;
border-bottom-left-radius: 3px;
}
.markdown-body .code div pre,
.markdown-body .code div {
border: 0;
-webkit-border-radius: 0;
-moz-border-radius: 0;
border-radius: 0;
-webkit-border-top-right-radius: 3px;
-webkit-border-bottom-right-radius: 3px;
-moz-border-radius-topright: 3px;
-moz-border-radius-bottomright: 3px;
border-top-right-radius: 3px;
border-bottom-right-radius: 3px;
}
.markdown-body * {
-moz-box-sizing: border-box;
box-sizing: border-box;
}
.markdown-body input {
font: 13px Helvetica, arial, freesans, clean, sans-serif, "Segoe UI Emoji", "Segoe UI Symbol";
line-height: 1.4;
}
.markdown-body a {
color: #4183c4;
text-decoration: none;
}
.markdown-body a:hover,
.markdown-body a:focus,
.markdown-body a:active {
text-decoration: underline;
}
.markdown-body hr {
height: 0;
margin: 15px 0;
overflow: hidden;
background: transparent;
border: 0;
border-bottom: 1px solid #ddd;
}
.markdown-body hr:before,
.markdown-body hr:after {
display: table;
content: " ";
}
.markdown-body hr:after {
clear: both;
}
.markdown-body h1,
.markdown-body h2,
.markdown-body h3,
.markdown-body h4,
.markdown-body h5,
.markdown-body h6 {
margin-top: 15px;
margin-bottom: 15px;
line-height: 1.1;
}
.markdown-body h1 {
font-size: 30px;
}
.markdown-body h2 {
font-size: 21px;
}
.markdown-body h3 {
font-size: 16px;
}
.markdown-body h4 {
font-size: 14px;
}
.markdown-body h5 {
font-size: 12px;
}
.markdown-body h6 {
font-size: 11px;
}
.markdown-body blockquote {
margin: 0;
}
.markdown-body ul,
.markdown-body ol {
padding: 0;
margin-top: 0;
margin-bottom: 0;
}
.markdown-body ol ol,
.markdown-body ul ol {
list-style-type: lower-roman;
}
.markdown-body ul ul ol,
.markdown-body ul ol ol,
.markdown-body ol ul ol,
.markdown-body ol ol ol {
list-style-type: lower-alpha;
}
.markdown-body dd {
margin-left: 0;
}
.markdown-body code,
.markdown-body pre,
.markdown-body samp {
font-family: Consolas, "Liberation Mono", Menlo, Courier, monospace;
font-size: 12px;
}
.markdown-body pre {
margin-top: 0;
margin-bottom: 0;
}
.markdown-body kbd {
background-color: #e7e7e7;
background-image: -moz-linear-gradient(#fefefe, #e7e7e7);
background-image: -webkit-linear-gradient(#fefefe, #e7e7e7);
background-image: linear-gradient(#fefefe, #e7e7e7);
background-repeat: repeat-x;
border-radius: 2px;
border: 1px solid #cfcfcf;
color: #000;
padding: 3px 5px;
line-height: 10px;
font: 11px Consolas, "Liberation Mono", Menlo, Courier, monospace;
display: inline-block;
}
.markdown-body>*:first-child {
margin-top: 0 !important;
}
.markdown-body>*:last-child {
margin-bottom: 0 !important;
}
.markdown-body .headerlink {
font: normal 400 16px fontawesome-mini;
vertical-align: middle;
margin-left: -16px;
float: left;
display: inline-block;
text-decoration: none;
opacity: 0;
color: #333;
}
.markdown-body .headerlink:focus {
outline: none;
}
.markdown-body h1 .headerlink {
margin-top: 0.8rem;
}
.markdown-body h2 .headerlink,
.markdown-body h3 .headerlink {
margin-top: 0.6rem;
}
.markdown-body h4 .headerlink {
margin-top: 0.2rem;
}
.markdown-body h5 .headerlink,
.markdown-body h6 .headerlink {
margin-top: 0;
}
.markdown-body .headerlink:hover,
.markdown-body h1:hover .headerlink,
.markdown-body h2:hover .headerlink,
.markdown-body h3:hover .headerlink,
.markdown-body h4:hover .headerlink,
.markdown-body h5:hover .headerlink,
.markdown-body h6:hover .headerlink {
opacity: 1;
text-decoration: none;
}
.markdown-body h1 {
padding-bottom: 0.3em;
font-size: 2.25em;
line-height: 1.2;
border-bottom: 1px solid #eee;
}
.markdown-body h2 {
padding-bottom: 0.3em;
font-size: 1.75em;
line-height: 1.225;
border-bottom: 1px solid #eee;
}
.markdown-body h3 {
font-size: 1.5em;
line-height: 1.43;
}
.markdown-body h4 {
font-size: 1.25em;
}
.markdown-body h5 {
font-size: 1em;
}
.markdown-body h6 {
font-size: 1em;
color: #777;
}
.markdown-body p,
.markdown-body blockquote,
.markdown-body ul,
.markdown-body ol,
.markdown-body dl,
.markdown-body table,
.markdown-body pre,
.markdown-body .admonition {
margin-top: 0;
margin-bottom: 16px;
}
.markdown-body hr {
height: 4px;
padding: 0;
margin: 16px 0;
background-color: #e7e7e7;
border: 0 none;
}
.markdown-body ul,
.markdown-body ol {
padding-left: 2em;
}
.markdown-body ul ul,
.markdown-body ul ol,
.markdown-body ol ol,
.markdown-body ol ul {
margin-top: 0;
margin-bottom: 0;
}
.markdown-body li>p {
margin-top: 16px;
}
.markdown-body dl {
padding: 0;
}
.markdown-body dl dt {
padding: 0;
margin-top: 16px;
font-size: 1em;
font-style: italic;
font-weight: bold;
}
.markdown-body dl dd {
padding: 0 16px;
margin-bottom: 16px;
}
.markdown-body blockquote {
padding: 0 15px;
color: #777;
border-left: 4px solid #ddd;
}
.markdown-body blockquote>:first-child {
margin-top: 0;
}
.markdown-body blockquote>:last-child {
margin-bottom: 0;
}
.markdown-body table {
display: block;
width: 100%;
overflow: auto;
word-break: normal;
word-break: keep-all;
}
.markdown-body table th {
font-weight: bold;
}
.markdown-body table th,
.markdown-body table td {
padding: 6px 13px;
border: 1px solid #ddd;
}
.markdown-body table tr {
background-color: #fff;
border-top: 1px solid #ccc;
}
.markdown-body table tr:nth-child(2n) {
background-color: #f8f8f8;
}
.markdown-body img {
max-width: 100%;
-moz-box-sizing: border-box;
box-sizing: border-box;
}
.markdown-body code,
.markdown-body samp {
padding: 0;
padding-top: 0.2em;
padding-bottom: 0.2em;
margin: 0;
font-size: 85%;
background-color: rgba(0,0,0,0.04);
border-radius: 3px;
}
.markdown-body code:before,
.markdown-body code:after {
letter-spacing: -0.2em;
content: "\00a0";
}
.markdown-body pre>code {
padding: 0;
margin: 0;
font-size: 100%;
word-break: normal;
white-space: pre;
background: transparent;
border: 0;
}
.markdown-body .codehilite {
margin-bottom: 16px;
}
.markdown-body .codehilite pre,
.markdown-body pre {
padding: 16px;
overflow: auto;
font-size: 85%;
line-height: 1.45;
background-color: #f7f7f7;
border-radius: 3px;
}
.markdown-body .codehilite pre {
margin-bottom: 0;
word-break: normal;
}
.markdown-body pre {
word-wrap: normal;
}
.markdown-body pre code {
display: inline;
max-width: initial;
padding: 0;
margin: 0;
overflow: initial;
line-height: inherit;
word-wrap: normal;
background-color: transparent;
border: 0;
}
.markdown-body pre code:before,
.markdown-body pre code:after {
content: normal;
}
/* Admonition */
.markdown-body .admonition {
-webkit-border-radius: 3px;
-moz-border-radius: 3px;
position: relative;
border-radius: 3px;
border: 1px solid #e0e0e0;
border-left: 6px solid #333;
padding: 10px 10px 10px 30px;
}
.markdown-body .admonition table {
color: #333;
}
.markdown-body .admonition p {
padding: 0;
}
.markdown-body .admonition-title {
font-weight: bold;
margin: 0;
}
.markdown-body .admonition>.admonition-title {
color: #333;
}
.markdown-body .attention>.admonition-title {
color: #a6d796;
}
.markdown-body .caution>.admonition-title {
color: #d7a796;
}
.markdown-body .hint>.admonition-title {
color: #96c6d7;
}
.markdown-body .danger>.admonition-title {
color: #c25f77;
}
.markdown-body .question>.admonition-title {
color: #96a6d7;
}
.markdown-body .note>.admonition-title {
color: #d7c896;
}
.markdown-body .admonition:before,
.markdown-body .attention:before,
.markdown-body .caution:before,
.markdown-body .hint:before,
.markdown-body .danger:before,
.markdown-body .question:before,
.markdown-body .note:before {
font: normal normal 16px fontawesome-mini;
-moz-osx-font-smoothing: grayscale;
-webkit-user-select: none;
-moz-user-select: none;
-ms-user-select: none;
user-select: none;
line-height: 1.5;
color: #333;
position: absolute;
left: 0;
top: 0;
padding-top: 10px;
padding-left: 10px;
}
.markdown-body .admonition:before {
content: "\f056\00a0";
color: 333;
}
.markdown-body .attention:before {
content: "\f058\00a0";
color: #a6d796;
}
.markdown-body .caution:before {
content: "\f06a\00a0";
color: #d7a796;
}
.markdown-body .hint:before {
content: "\f05a\00a0";
color: #96c6d7;
}
.markdown-body .danger:before {
content: "\f057\00a0";
color: #c25f77;
}
.markdown-body .question:before {
content: "\f059\00a0";
color: #96a6d7;
}
.markdown-body .note:before {
content: "\f040\00a0";
color: #d7c896;
}
.markdown-body .admonition::after {
content: normal;
}
.markdown-body .attention {
border-left: 6px solid #a6d796;
}
.markdown-body .caution {
border-left: 6px solid #d7a796;
}
.markdown-body .hint {
border-left: 6px solid #96c6d7;
}
.markdown-body .danger {
border-left: 6px solid #c25f77;
}
.markdown-body .question {
border-left: 6px solid #96a6d7;
}
.markdown-body .note {
border-left: 6px solid #d7c896;
}
.markdown-body .admonition>*:first-child {
margin-top: 0 !important;
}
.markdown-body .admonition>*:last-child {
margin-bottom: 0 !important;
}
/* progress bar*/
.markdown-body .progress {
display: block;
width: 300px;
margin: 10px 0;
height: 24px;
-webkit-border-radius: 3px;
-moz-border-radius: 3px;
border-radius: 3px;
background-color: #ededed;
position: relative;
box-shadow: inset -1px 1px 3px rgba(0, 0, 0, .1);
}
.markdown-body .progress-label {
position: absolute;
text-align: center;
font-weight: bold;
width: 100%; margin: 0;
line-height: 24px;
color: #333;
text-shadow: 1px 1px 0 #fefefe, -1px -1px 0 #fefefe, -1px 1px 0 #fefefe, 1px -1px 0 #fefefe, 0 1px 0 #fefefe, 0 -1px 0 #fefefe, 1px 0 0 #fefefe, -1px 0 0 #fefefe, 1px 1px 2px #000;
-webkit-font-smoothing: antialiased !important;
white-space: nowrap;
overflow: hidden;
}
.markdown-body .progress-bar {
height: 24px;
float: left;
-webkit-border-radius: 3px;
-moz-border-radius: 3px;
border-radius: 3px;
background-color: #96c6d7;
box-shadow: inset 0 1px 0 rgba(255, 255, 255, .5), inset 0 -1px 0 rgba(0, 0, 0, .1);
background-size: 30px 30px;
background-image: -webkit-linear-gradient(
135deg, rgba(255, 255, 255, .4) 27%,
transparent 27%,
transparent 52%, rgba(255, 255, 255, .4) 52%,
rgba(255, 255, 255, .4) 77%,
transparent 77%, transparent
);
background-image: -moz-linear-gradient(
135deg,
rgba(255, 255, 255, .4) 27%, transparent 27%,
transparent 52%, rgba(255, 255, 255, .4) 52%,
rgba(255, 255, 255, .4) 77%, transparent 77%,
transparent
);
background-image: -ms-linear-gradient(
135deg,
rgba(255, 255, 255, .4) 27%, transparent 27%,
transparent 52%, rgba(255, 255, 255, .4) 52%,
rgba(255, 255, 255, .4) 77%, transparent 77%,
transparent
);
background-image: -o-linear-gradient(
135deg,
rgba(255, 255, 255, .4) 27%, transparent 27%,
transparent 52%, rgba(255, 255, 255, .4) 52%,
rgba(255, 255, 255, .4) 77%, transparent 77%,
transparent
);
background-image: linear-gradient(
135deg,
rgba(255, 255, 255, .4) 27%, transparent 27%,
transparent 52%, rgba(255, 255, 255, .4) 52%,
rgba(255, 255, 255, .4) 77%, transparent 77%,
transparent
);
}
.markdown-body .progress-100plus .progress-bar {
background-color: #a6d796;
}
.markdown-body .progress-80plus .progress-bar {
background-color: #c6d796;
}
.markdown-body .progress-60plus .progress-bar {
background-color: #d7c896;
}
.markdown-body .progress-40plus .progress-bar {
background-color: #d7a796;
}
.markdown-body .progress-20plus .progress-bar {
background-color: #d796a6;
}
.markdown-body .progress-0plus .progress-bar {
background-color: #c25f77;
}
.markdown-body .candystripe-animate .progress-bar{
-webkit-animation: animate-stripes 3s linear infinite;
-moz-animation: animate-stripes 3s linear infinite;
animation: animate-stripes 3s linear infinite;
}
@-webkit-keyframes animate-stripes {
0% {
background-position: 0 0;
}
100% {
background-position: 60px 0;
}
}
@-moz-keyframes animate-stripes {
0% {
background-position: 0 0;
}
100% {
background-position: 60px 0;
}
}
@keyframes animate-stripes {
0% {
background-position: 0 0;
}
100% {
background-position: 60px 0;
}
}
.markdown-body .gloss .progress-bar {
box-shadow:
inset 0 4px 12px rgba(255, 255, 255, .7),
inset 0 -12px 0 rgba(0, 0, 0, .05);
}
/* MultiMarkdown Critic Blocks */
.markdown-body .critic_mark {
background: #ff0;
}
.markdown-body .critic_delete {
color: #c82829;
text-decoration: line-through;
}
.markdown-body .critic_insert {
color: #718c00 ;
text-decoration: underline;
}
.markdown-body .critic_comment {
color: #8e908c;
font-style: italic;
}
.markdown-body .headeranchor {
font: normal normal 16px fontawesome-mini;
line-height: 1;
display: inline-block;
text-decoration: none;
-webkit-font-smoothing: antialiased;
-moz-osx-font-smoothing: grayscale;
-webkit-user-select: none;
-moz-user-select: none;
-ms-user-select: none;
user-select: none;
}
.headeranchor:before {
content: '\e157';
}
.markdown-body .task-list-item {
list-style-type: none;
}
.markdown-body .task-list-item+.task-list-item {
margin-top: 3px;
}
.markdown-body .task-list-item input {
margin: 0 4px 0.25em -20px;
vertical-align: middle;
}
/* Media */
@media only screen and (min-width: 480px) {
.markdown-body {
font-size:14px;
}
}
@media only screen and (min-width: 768px) {
.markdown-body {
font-size:16px;
}
}
@media print {
.markdown-body * {
background: transparent !important;
color: black !important;
filter:none !important;
-ms-filter: none !important;
}
.markdown-body {
font-size:12pt;
max-width:100%;
outline:none;
border: 0;
}
.markdown-body a,
.markdown-body a:visited {
text-decoration: underline;
}
.markdown-body .headeranchor-link {
display: none;
}
.markdown-body a[href]:after {
content: " (" attr(href) ")";
}
.markdown-body abbr[title]:after {
content: " (" attr(title) ")";
}
.markdown-body .ir a:after,
.markdown-body a[href^="javascript:"]:after,
.markdown-body a[href^="#"]:after {
content: "";
}
.markdown-body pre {
white-space: pre;
white-space: pre-wrap;
word-wrap: break-word;
}
.markdown-body pre,
.markdown-body blockquote {
border: 1px solid #999;
padding-right: 1em;
page-break-inside: avoid;
}
.markdown-body .progress,
.markdown-body .progress-bar {
-moz-box-shadow: none;
-webkit-box-shadow: none;
box-shadow: none;
}
.markdown-body .progress {
border: 1px solid #ddd;
}
.markdown-body .progress-bar {
height: 22px;
border-right: 1px solid #ddd;
}
.markdown-body tr,
.markdown-body img {
page-break-inside: avoid;
}
.markdown-body img {
max-width: 100% !important;
}
.markdown-body p,
.markdown-body h2,
.markdown-body h3 {
orphans: 3;
widows: 3;
}
.markdown-body h2,
.markdown-body h3 {
page-break-after: avoid;
}
}
</style><style>/*GitHub*/
.codehilite {background-color:#fff;color:#333333;}
.codehilite .hll {background-color:#ffffcc;}
.codehilite .c{color:#999988;font-style:italic}
.codehilite .err{color:#a61717;background-color:#e3d2d2}
.codehilite .k{font-weight:bold}
.codehilite .o{font-weight:bold}
.codehilite .cm{color:#999988;font-style:italic}
.codehilite .cp{color:#999999;font-weight:bold}
.codehilite .c1{color:#999988;font-style:italic}
.codehilite .cs{color:#999999;font-weight:bold;font-style:italic}
.codehilite .gd{color:#000000;background-color:#ffdddd}
.codehilite .ge{font-style:italic}
.codehilite .gr{color:#aa0000}
.codehilite .gh{color:#999999}
.codehilite .gi{color:#000000;background-color:#ddffdd}
.codehilite .go{color:#888888}
.codehilite .gp{color:#555555}
.codehilite .gs{font-weight:bold}
.codehilite .gu{color:#800080;font-weight:bold}
.codehilite .gt{color:#aa0000}
.codehilite .kc{font-weight:bold}
.codehilite .kd{font-weight:bold}
.codehilite .kn{font-weight:bold}
.codehilite .kp{font-weight:bold}
.codehilite .kr{font-weight:bold}
.codehilite .kt{color:#445588;font-weight:bold}
.codehilite .m{color:#009999}
.codehilite .s{color:#dd1144}
.codehilite .n{color:#333333}
.codehilite .na{color:teal}
.codehilite .nb{color:#0086b3}
.codehilite .nc{color:#445588;font-weight:bold}
.codehilite .no{color:teal}
.codehilite .ni{color:purple}
.codehilite .ne{color:#990000;font-weight:bold}
.codehilite .nf{color:#990000;font-weight:bold}
.codehilite .nn{color:#555555}
.codehilite .nt{color:navy}
.codehilite .nv{color:teal}
.codehilite .ow{font-weight:bold}
.codehilite .w{color:#bbbbbb}
.codehilite .mf{color:#009999}
.codehilite .mh{color:#009999}
.codehilite .mi{color:#009999}
.codehilite .mo{color:#009999}
.codehilite .sb{color:#dd1144}
.codehilite .sc{color:#dd1144}
.codehilite .sd{color:#dd1144}
.codehilite .s2{color:#dd1144}
.codehilite .se{color:#dd1144}
.codehilite .sh{color:#dd1144}
.codehilite .si{color:#dd1144}
.codehilite .sx{color:#dd1144}
.codehilite .sr{color:#009926}
.codehilite .s1{color:#dd1144}
.codehilite .ss{color:#990073}
.codehilite .bp{color:#999999}
.codehilite .vc{color:teal}
.codehilite .vg{color:teal}
.codehilite .vi{color:teal}
.codehilite .il{color:#009999}
.codehilite .gc{color:#999;background-color:#EAF2F5}
</style><title>develop</title></head><body><article class="markdown-body"><h1 id="slam">源码解析 | SLAM 多点导航开发思路介绍<a class="headerlink" href="#slam" title="Permanent link"></a></h1>
<h3 id="_1">开发背景：<a class="headerlink" href="#_1" title="Permanent link"></a></h3>
<p>在使用 ROS Navigation &amp; RViz 进行 2D Nav Goal 导航的时候，我们会遇到这些情况：</p>
<ol>
<li>给定导航的目标点只能设置一个，当有多点任务时需要等待一个个任务结束后，再次手动给目标</li>
<li>无法暂停或取消任务</li>
<li>任务不可循环</li>
</ol>
<h3 id="_2">开发目的：<a class="headerlink" href="#_2" title="Permanent link"></a></h3>
<p>完成多目标点导航，可以对导航环节进行操控，如可循环、取消、重置任务等。</p>
<h3 id="_3">开发思路：<a class="headerlink" href="#_3" title="Permanent link"></a></h3>
<ol>
<li>2D Nav Goal 的单点导航是如何实现的？</li>
</ol>
<p>我们可以知道导航目标是通过 RViz 工具栏中 2D Nav Goal发布出去的。</p>
<p>通过查看 RViz的配置文件或者 Panels-&gt;Add New Panel-&gt; Tool Property ，可以了解当使用2D Nav Goal 在地图上拉了一个箭头（给定目标点时），其实是向话题 /move_base_simple/goal 发布了 <a href="http://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html">geometry_msgs/PoseStamped</a> 类型的消息，这个是目标点的位姿，包含坐标点与朝向。</p>
<p>根据 NodeGraph 可以看到话题 /move_base_simple/goal 被导航包的节点 /move_base 订阅了，进而发给 Navigation 中的各个话题，完成导航。</p>
<p><img alt="" src="/D:/%E5%B7%A5%E4%BD%9C%E9%A1%B9%E7%9B%AE/2020%E5%B9%B4/%E5%8D%8E%E6%B8%85/%E9%A1%B9%E7%9B%AE/%E4%BA%A7%E5%93%81/AP1/%E6%96%87%E6%A1%A3/rviz_navi_multi_goals_pub_plugin/images/dev1.png" /></p>
<ol start="2">
<li>如何基于单点实现多点导航？</li>
</ol>
<p>要在单点基础上实现多目标点导航的话，就要设计一个关于多个导航目标点消息geometry_msgs/PoseStamped的数据结构，并对多个目标点进行处理，完成导航。</p>
<p>实现多点的方法有多种，在不打破 ROS Navigation 包的完整性的前提下，我选择在2D Nav Goal的 RViz节点和 /move_base 节点中间添加了一个话题 /move_base_simple/goal_temp，将原本发送给 /move_base_simple/goal 的消息，转发给/move_base_simple/goal_temp，通过此话题来积攒多个 2D Nav Goal（任务队列），并根据任务完成的状态反馈，按顺序将每个导航目标点消息 geometry_msgs/PoseStamped 再发送给话题/move_base_simple/goal，以完成多任务中的单次目标点的导航（如下图示）。</p>
<p><img alt="" src="/D:/%E5%B7%A5%E4%BD%9C%E9%A1%B9%E7%9B%AE/2020%E5%B9%B4/%E5%8D%8E%E6%B8%85/%E9%A1%B9%E7%9B%AE/%E4%BA%A7%E5%93%81/AP1/%E6%96%87%E6%A1%A3/rviz_navi_multi_goals_pub_plugin/images/dev2.jpg" /></p>
<ol start="3">
<li>如何来发布多点任务？</li>
</ol>
<p>像 2D Nav Goal 一样，我们也可以在 RViz 中开发可视化的操作栏，这要使用到 RViz plugin ， ROS中的可视化工具绝大部分都是基于Qt进行开发的，此前古月居有过详细介绍，可参考<a href="https://zhuanlan.zhihu.com/p/39390512">这篇文章</a>。</p>
<h3 id="_4">最终效果<a class="headerlink" href="#_4" title="Permanent link"></a></h3>
<p>首先，我们来看一下最终的实现效果。</p>
<p>MultiNaviGoalsPanel是多点SLAM导航任务的可视化操作区，包括任务点列表、循环、重置、取消、开始任务。</p>
<p>通过 RViz plugin 设计的Mark Display，能够显示的目标点的标号及箭头（朝向）。</p>
<p><img alt="" src="/D:/%E5%B7%A5%E4%BD%9C%E9%A1%B9%E7%9B%AE/2020%E5%B9%B4/%E5%8D%8E%E6%B8%85/%E9%A1%B9%E7%9B%AE/%E4%BA%A7%E5%93%81/AP1/%E6%96%87%E6%A1%A3/rviz_navi_multi_goals_pub_plugin/images/dev3.png" /></p>
<h3 id="_5">源代码<a class="headerlink" href="#_5" title="Permanent link"></a></h3>
<p>项目地址：<a href="https://github.com/autolaborcenter/rviz_navi_multi_goals_pub_plugin">https://github.com/autolaborcenter/rviz_navi_multi_goals_pub_plugin</a></p>
<h4 id="_6">代码实现<a class="headerlink" href="#_6" title="Permanent link"></a></h4>
<h4 id="1-multi_navi_goal_panelh">1. 头文件 multi_navi_goal_panel.h<a class="headerlink" href="#1-multi_navi_goal_panelh" title="Permanent link"></a></h4>
<p>Qt说明：</p>
<ul>
<li>文字编辑——QLineEdit</li>
<li>按键——QPushButton</li>
<li>列表——QTableWidget</li>
<li>复选框——QCheckBox</li>
<li>文字显示——QString</li>
</ul>
<p>ROS说明:</p>
<p>Publisher：</p>
<ul>
<li>发送每个目标点消息给/move_base_simple/goal的goal_pub_</li>
<li>发送取消指令消息给/move_base/cancel的cancel_pub_</li>
<li>发送文字和箭头标记的mark_pub_。</li>
</ul>
<p>Subsrciber：</p>
<ul>
<li>订阅来自rviz中2D Nav Goal的导航目标点消息的goal_sub_</li>
<li>订阅目前导航状态的status_sub_</li>
</ul>
<div class="codehilite"><pre>#ifndef MULTI_NAVI_GOAL_PANEL_H
#define MULTI_NAVI_GOAL_PANEL_H
#include &lt;string&gt;
#include &lt;ros/ros.h&gt;
#include &lt;ros/console.h&gt;
#include &lt;rviz/panel.h&gt;//plugin基类的头文件
#include &lt;QPushButton&gt;//Qt按钮
#include &lt;QTableWidget&gt;//Qt表格
#include &lt;QCheckBox&gt;//Qt复选框
#include &lt;visualization_msgs/Marker.h&gt;
#include &lt;geometry_msgs/PoseArray.h&gt;
#include &lt;geometry_msgs/Point.h&gt;
#include &lt;geometry_msgs/PoseStamped.h&gt;
#include &lt;std_msgs/String.h&gt;
#include &lt;actionlib_msgs/GoalStatus.h&gt;
#include &lt;actionlib_msgs/GoalStatusArray.h&gt;
#include &lt;tf/transform_datatypes.h&gt;
namespace navi_multi_goals_pub_rviz_plugin {
class MultiNaviGoalsPanel : public rviz::Panel {
Q_OBJECT
public:
explicit MultiNaviGoalsPanel(QWidget *parent = 0);
public Q_SLOTS:
void setMaxNumGoal(const QString &amp;maxNumGoal);//设置最大可设置的目标点数量
void writePose(geometry_msgs::Pose pose);//将目标点位姿写入任务列表
void markPose(const geometry_msgs::PoseStamped::ConstPtr &amp;pose);//在地图中标记目标位姿
void deleteMark();//删除标记
protected Q_SLOTS:
void updateMaxNumGoal(); // 更新最大可设置的目标点数量
void initPoseTable(); // 初始化目标点表格
void updatePoseTable(); // 更新目标点表格
void startNavi(); // 开始第一个目标点任务导航
void cancelNavi(); // 取消现在进行中的导航
void goalCntCB(const geometry_msgs::PoseStamped::ConstPtr &amp;pose); // 目标数量子回调函数
void statusCB(const actionlib_msgs::GoalStatusArray::ConstPtr &amp;statuses); // 状态子回调函数
void checkCycle();//确认循环
void completeNavi(); // 第一个任务到达后，继续进行剩下任务点的导航任务
void cycleNavi();
bool checkGoal(std::vector&lt;actionlib_msgs::GoalStatus&gt; status_list); // 检查是否到达目标点
static void startSpin(); // 启用ROS订阅
protected:
QLineEdit *output_maxNumGoal_editor_;
QPushButton *output_maxNumGoal_button_, *output_reset_button_, *output_startNavi_button_, *output_cancel_button_;
QTableWidget *poseArray_table_;
QCheckBox *cycle_checkbox_;
QString output_maxNumGoal_;
// The ROS node handle.
ros::NodeHandle nh_;
ros::Publisher goal_pub_, cancel_pub_, marker_pub_;
ros::Subscriber goal_sub_, status_sub_;
// 多目标点任务栏定义
int maxNumGoal_;
int curGoalIdx_ = 0, cycleCnt_ = 0;
bool permit_ = false, cycle_ = false, arrived_ = false;
geometry_msgs::PoseArray pose_array_;
actionlib_msgs::GoalID cur_goalid_;
};
} // end namespace navi-multi-goals-pub-rviz-plugin
#endif // MULTI_NAVI_GOAL_PANEL_H
</pre></div>
<h4 id="2-cpp-multi_navi_goal_panelcpp">2. cpp文件 multi_navi_goal_panel.cpp<a class="headerlink" href="#2-cpp-multi_navi_goal_panelcpp" title="Permanent link"></a></h4>
<div class="codehilite"><pre>#include &lt;cstdio&gt;
#include &lt;ros/console.h&gt;
#include &lt;fstream&gt;
#include &lt;sstream&gt;
#include &lt;QPainter&gt;
#include &lt;QLineEdit&gt;
#include &lt;QVBoxLayout&gt;
#include &lt;QLabel&gt;
#include &lt;QTimer&gt;
#include &lt;QDebug&gt;
#include &lt;QtWidgets/QTableWidget&gt;
#include &lt;QtWidgets/qheaderview.h&gt;
#include &quot;multi_navi_goal_panel.h&quot;
namespace navi_multi_goals_pub_rviz_plugin {
MultiNaviGoalsPanel::MultiNaviGoalsPanel(QWidget *parent)
: rviz::Panel(parent), nh_(), maxNumGoal_(1) {
goal_sub_ = nh_.subscribe&lt;geometry_msgs::PoseStamped&gt;(&quot;move_base_simple/goal_temp&quot;, 1,
boost::bind(&amp;MultiNaviGoalsPanel::goalCntCB, this, _1));
status_sub_ = nh_.subscribe&lt;actionlib_msgs::GoalStatusArray&gt;(&quot;move_base/status&quot;, 1,
boost::bind(&amp;MultiNaviGoalsPanel::statusCB, this,
_1));
goal_pub_ = nh_.advertise&lt;geometry_msgs::PoseStamped&gt;(&quot;move_base_simple/goal&quot;, 1);
cancel_pub_ = nh_.advertise&lt;actionlib_msgs::GoalID&gt;(&quot;move_base/cancel&quot;, 1);
marker_pub_ = nh_.advertise&lt;visualization_msgs::Marker&gt;(&quot;visualization_marker&quot;, 1);
QVBoxLayout *root_layout = new QVBoxLayout;
// create a panel about &quot;maxNumGoal&quot;
QHBoxLayout *maxNumGoal_layout = new QHBoxLayout;
maxNumGoal_layout-&gt;addWidget(new QLabel(&quot;目标最大数量&quot;));
output_maxNumGoal_editor_ = new QLineEdit;
maxNumGoal_layout-&gt;addWidget(output_maxNumGoal_editor_);
output_maxNumGoal_button_ = new QPushButton(&quot;确定&quot;);
maxNumGoal_layout-&gt;addWidget(output_maxNumGoal_button_);
root_layout-&gt;addLayout(maxNumGoal_layout);
cycle_checkbox_ = new QCheckBox(&quot;循环&quot;);
root_layout-&gt;addWidget(cycle_checkbox_);
// creat a QTable to contain the poseArray
poseArray_table_ = new QTableWidget;
initPoseTable();
root_layout-&gt;addWidget(poseArray_table_);
//creat a manipulate layout
QHBoxLayout *manipulate_layout = new QHBoxLayout;
output_reset_button_ = new QPushButton(&quot;重置&quot;);
manipulate_layout-&gt;addWidget(output_reset_button_);
output_cancel_button_ = new QPushButton(&quot;取消&quot;);
manipulate_layout-&gt;addWidget(output_cancel_button_);
output_startNavi_button_ = new QPushButton(&quot;开始导航!&quot;);
manipulate_layout-&gt;addWidget(output_startNavi_button_);
root_layout-&gt;addLayout(manipulate_layout);
setLayout(root_layout);
// set a Qtimer to start a spin for subscriptions
QTimer *output_timer = new QTimer(this);
output_timer-&gt;start(200);
// 设置信号与槽的连接
connect(output_maxNumGoal_button_, SIGNAL(clicked()), this,
SLOT(updateMaxNumGoal()));
connect(output_maxNumGoal_button_, SIGNAL(clicked()), this,
SLOT(updatePoseTable()));
connect(output_reset_button_, SIGNAL(clicked()), this, SLOT(initPoseTable()));
connect(output_cancel_button_, SIGNAL(clicked()), this, SLOT(cancelNavi()));
connect(output_startNavi_button_, SIGNAL(clicked()), this, SLOT(startNavi()));
connect(cycle_checkbox_, SIGNAL(clicked(bool)), this, SLOT(checkCycle()));
connect(output_timer, SIGNAL(timeout()), this, SLOT(startSpin()));
}
// 更新maxNumGoal命名
void MultiNaviGoalsPanel::updateMaxNumGoal() {
setMaxNumGoal(output_maxNumGoal_editor_-&gt;text());
}
// set up the maximum number of goals
void MultiNaviGoalsPanel::setMaxNumGoal(const QString &amp;new_maxNumGoal) {
// 检查maxNumGoal是否发生改变.
if (new_maxNumGoal != output_maxNumGoal_) {
output_maxNumGoal_ = new_maxNumGoal;
// 如果命名为空，不发布任何信息
if (output_maxNumGoal_ == &quot;&quot;) {
nh_.setParam(&quot;maxNumGoal_&quot;, 1);
maxNumGoal_ = 1;
} else {
// velocity_publisher_ = nh_.advertise&lt;geometry_msgs::Twist&gt;(output_maxNumGoal_.toStdString(), 1);
nh_.setParam(&quot;maxNumGoal_&quot;, output_maxNumGoal_.toInt());
maxNumGoal_ = output_maxNumGoal_.toInt();
}
Q_EMIT configChanged();
}
}
// initialize the table of pose
void MultiNaviGoalsPanel::initPoseTable() {
ROS_INFO(&quot;Initialize&quot;);
curGoalIdx_ = 0, cycleCnt_ = 0;
permit_ = false, cycle_ = false;
poseArray_table_-&gt;clear();
pose_array_.poses.clear();
deleteMark();
poseArray_table_-&gt;setRowCount(maxNumGoal_);
poseArray_table_-&gt;setColumnCount(3);
poseArray_table_-&gt;setEditTriggers(QAbstractItemView::NoEditTriggers);
poseArray_table_-&gt;horizontalHeader()-&gt;setSectionResizeMode(QHeaderView::Stretch);
QStringList pose_header;
pose_header &lt;&lt; &quot;x&quot; &lt;&lt; &quot;y&quot; &lt;&lt; &quot;yaw&quot;;
poseArray_table_-&gt;setHorizontalHeaderLabels(pose_header);
cycle_checkbox_-&gt;setCheckState(Qt::Unchecked);
}
// delete marks in the map
void MultiNaviGoalsPanel::deleteMark() {
visualization_msgs::Marker marker_delete;
marker_delete.action = visualization_msgs::Marker::DELETEALL;
marker_pub_.publish(marker_delete);
}
//update the table of pose
void MultiNaviGoalsPanel::updatePoseTable() {
poseArray_table_-&gt;setRowCount(maxNumGoal_);
// pose_array_.poses.resize(maxNumGoal_);
QStringList pose_header;
pose_header &lt;&lt; &quot;x&quot; &lt;&lt; &quot;y&quot; &lt;&lt; &quot;yaw&quot;;
poseArray_table_-&gt;setHorizontalHeaderLabels(pose_header);
poseArray_table_-&gt;show();
}
// call back function for counting goals
void MultiNaviGoalsPanel::goalCntCB(const geometry_msgs::PoseStamped::ConstPtr &amp;pose) {
if (pose_array_.poses.size() &lt; maxNumGoal_) {
pose_array_.poses.push_back(pose-&gt;pose);
pose_array_.header.frame_id = pose-&gt;header.frame_id;
writePose(pose-&gt;pose);
markPose(pose);
} else {
ROS_ERROR(&quot;Beyond the maximum number of goals: %d&quot;, maxNumGoal_);
}
}
// write the poses into the table
void MultiNaviGoalsPanel::writePose(geometry_msgs::Pose pose) {
poseArray_table_-&gt;setItem(pose_array_.poses.size() - 1, 0,
new QTableWidgetItem(QString::number(pose.position.x, &#39;f&#39;, 2)));
poseArray_table_-&gt;setItem(pose_array_.poses.size() - 1, 1,
new QTableWidgetItem(QString::number(pose.position.y, &#39;f&#39;, 2)));
poseArray_table_-&gt;setItem(pose_array_.poses.size() - 1, 2,
new QTableWidgetItem(
QString::number(tf::getYaw(pose.orientation) * 180.0 / 3.14, &#39;f&#39;, 2)));
}
// when setting a Navi Goal, it will set a mark on the map
void MultiNaviGoalsPanel::markPose(const geometry_msgs::PoseStamped::ConstPtr &amp;pose) {
if (ros::ok()) {
visualization_msgs::Marker arrow;
visualization_msgs::Marker number;
arrow.header.frame_id = number.header.frame_id = pose-&gt;header.frame_id;
arrow.ns = &quot;navi_point_arrow&quot;;
number.ns = &quot;navi_point_number&quot;;
arrow.action = number.action = visualization_msgs::Marker::ADD;
arrow.type = visualization_msgs::Marker::ARROW;
number.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
arrow.pose = number.pose = pose-&gt;pose;
number.pose.position.z += 1.0;
arrow.scale.x = 1.0;
arrow.scale.y = 0.2;
number.scale.z = 1.0;
arrow.color.r = number.color.r = 1.0f;
arrow.color.g = number.color.g = 0.98f;
arrow.color.b = number.color.b = 0.80f;
arrow.color.a = number.color.a = 1.0;
arrow.id = number.id = pose_array_.poses.size();
number.text = std::to_string(pose_array_.poses.size());
marker_pub_.publish(arrow);
marker_pub_.publish(number);
}
}
// check whether it is in the cycling situation
void MultiNaviGoalsPanel::checkCycle() {
cycle_ = cycle_checkbox_-&gt;isChecked();
}
// start to navigate, and only command the first goal
void MultiNaviGoalsPanel::startNavi() {
curGoalIdx_ = curGoalIdx_ % pose_array_.poses.size();
if (!pose_array_.poses.empty() &amp;&amp; curGoalIdx_ &lt; maxNumGoal_) {
geometry_msgs::PoseStamped goal;
goal.header = pose_array_.header;
goal.pose = pose_array_.poses.at(curGoalIdx_);
goal_pub_.publish(goal);
ROS_INFO(&quot;Navi to the Goal%d&quot;, curGoalIdx_ + 1);
poseArray_table_-&gt;item(curGoalIdx_, 0)-&gt;setBackgroundColor(QColor(255, 69, 0));
poseArray_table_-&gt;item(curGoalIdx_, 1)-&gt;setBackgroundColor(QColor(255, 69, 0));
poseArray_table_-&gt;item(curGoalIdx_, 2)-&gt;setBackgroundColor(QColor(255, 69, 0));
curGoalIdx_ += 1;
permit_ = true;
} else {
ROS_ERROR(&quot;Something Wrong&quot;);
}
}
// complete the remaining goals
void MultiNaviGoalsPanel::completeNavi() {
if (curGoalIdx_ &lt; pose_array_.poses.size()) {
geometry_msgs::PoseStamped goal;
goal.header = pose_array_.header;
goal.pose = pose_array_.poses.at(curGoalIdx_);
goal_pub_.publish(goal);
ROS_INFO(&quot;Navi to the Goal%d&quot;, curGoalIdx_ + 1);
poseArray_table_-&gt;item(curGoalIdx_, 0)-&gt;setBackgroundColor(QColor(255, 69, 0));
poseArray_table_-&gt;item(curGoalIdx_, 1)-&gt;setBackgroundColor(QColor(255, 69, 0));
poseArray_table_-&gt;item(curGoalIdx_, 2)-&gt;setBackgroundColor(QColor(255, 69, 0));
curGoalIdx_ += 1;
permit_ = true;
} else {
ROS_ERROR(&quot;All goals are completed&quot;);
permit_ = false;
}
}
// command the goals cyclically
void MultiNaviGoalsPanel::cycleNavi() {
if (permit_) {
geometry_msgs::PoseStamped goal;
goal.header = pose_array_.header;
goal.pose = pose_array_.poses.at(curGoalIdx_ % pose_array_.poses.size());
goal_pub_.publish(goal);
ROS_INFO(&quot;Navi to the Goal%lu, in the %dth cycle&quot;, curGoalIdx_ % pose_array_.poses.size() + 1,
cycleCnt_ + 1);
bool even = ((cycleCnt_ + 1) % 2 != 0);
QColor color_table;
if (even) color_table = QColor(255, 69, 0); else color_table = QColor(100, 149, 237);
poseArray_table_-&gt;item(curGoalIdx_ % pose_array_.poses.size(), 0)-&gt;setBackgroundColor(color_table);
poseArray_table_-&gt;item(curGoalIdx_ % pose_array_.poses.size(), 1)-&gt;setBackgroundColor(color_table);
poseArray_table_-&gt;item(curGoalIdx_ % pose_array_.poses.size(), 2)-&gt;setBackgroundColor(color_table);
curGoalIdx_ += 1;
cycleCnt_ = curGoalIdx_ / pose_array_.poses.size();
}
}
// cancel the current command
void MultiNaviGoalsPanel::cancelNavi() {
if (!cur_goalid_.id.empty()) {
cancel_pub_.publish(cur_goalid_);
ROS_ERROR(&quot;Navigation have been canceled&quot;);
}
}
// call back for listening current state
void MultiNaviGoalsPanel::statusCB(const actionlib_msgs::GoalStatusArray::ConstPtr &amp;statuses) {
bool arrived_pre = arrived_;
arrived_ = checkGoal(statuses-&gt;status_list);
if (arrived_) { ROS_ERROR(&quot;%d,%d&quot;, int(arrived_), int(arrived_pre)); }
if (arrived_ &amp;&amp; arrived_ != arrived_pre &amp;&amp; ros::ok() &amp;&amp; permit_) {
if (cycle_) cycleNavi();
else completeNavi();
}
}
//check the current state of goal
bool MultiNaviGoalsPanel::checkGoal(std::vector&lt;actionlib_msgs::GoalStatus&gt; status_list) {
bool done;
if (!status_list.empty()) {
for (auto &amp;i : status_list) {
if (i.status == 3) {
done = true;
ROS_INFO(&quot;completed Goal%d&quot;, curGoalIdx_);
} else if (i.status == 4) {
ROS_ERROR(&quot;Goal%d is Invalid, Navi to Next Goal%d&quot;, curGoalIdx_, curGoalIdx_ + 1);
return true;
} else if (i.status == 0) {
done = true;
} else if (i.status == 1) {
cur_goalid_ = i.goal_id;
done = false;
} else done = false;
}
} else {
ROS_INFO(&quot;Please input the Navi Goal&quot;);
done = false;
}
return done;
}
// spin for subscribing
void MultiNaviGoalsPanel::startSpin() {
if (ros::ok()) {
ros::spinOnce();
}
}
} // end namespace navi-multi-goals-pub-rviz-plugin
// 声明此类是一个rviz的插件
#include &lt;pluginlib/class_list_macros.h&gt;
PLUGINLIB_EXPORT_CLASS(navi_multi_goals_pub_rviz_plugin::MultiNaviGoalsPanel, rviz::Panel)
</pre></div>
<h3 id="_7">使用<a class="headerlink" href="#_7" title="Permanent link"></a></h3>
<p>使用请参考<a href="/D:/%E5%B7%A5%E4%BD%9C%E9%A1%B9%E7%9B%AE/2020%E5%B9%B4/%E5%8D%8E%E6%B8%85/%E9%A1%B9%E7%9B%AE/%E4%BA%A7%E5%93%81/AP1/%E6%96%87%E6%A1%A3/rviz_navi_multi_goals_pub_plugin/README.md">使用说明</a>。</p></article></body></html>
Reference in New Issue View Git Blame Copy Permalink