Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
I
iri_ana_nav_module
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Model registry
Operate
Environments
Monitor
Incidents
Service Desk
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
labrobotica
ros
robots
ana
iri_ana_nav_module
Commits
a143f5ef
Commit
a143f5ef
authored
5 years ago
by
José Enrique Domínguez Vidal
Browse files
Options
Downloads
Patches
Plain Diff
Added enable_timeout_nav_3d(), disable_timeout_nav_3d() and update_timeout_nav_3d() functions
parent
533ffd38
No related branches found
No related tags found
No related merge requests found
Changes
2
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
include/iri_ana_nav_module/iri_ana_nav_module_bt.h
+29
-0
29 additions, 0 deletions
include/iri_ana_nav_module/iri_ana_nav_module_bt.h
src/iri_ana_nav_module_bt.cpp
+44
-1
44 additions, 1 deletion
src/iri_ana_nav_module_bt.cpp
with
73 additions
and
1 deletion
include/iri_ana_nav_module/iri_ana_nav_module_bt.h
+
29
−
0
View file @
a143f5ef
...
@@ -531,6 +531,35 @@ class CIriAnaNavModuleBT
...
@@ -531,6 +531,35 @@ class CIriAnaNavModuleBT
*
*
*/
*/
BT
::
NodeStatus
costmap_is_auto_clear_enabled
();
BT
::
NodeStatus
costmap_is_auto_clear_enabled
();
/**
* \brief Enable navigation timeout
*
* This function enables the navigation timeout using as input the duration
* in seconds of that timeout.
*
* \return a BT:NodeStatus::SUCCESS if the input is valid.
*
*/
BT
::
NodeStatus
enable_timeout
(
BT
::
TreeNode
&
self
);
/**
* \brief Disable navigation timeout
*
* This function disables the navigation timeout.
*
* \return a BT:NodeStatus::SUCCESS always.
*
*/
BT
::
NodeStatus
disable_timeout
();
/**
* \brief Update navigation timeout
*
* This function updates the navigation timeout using as input the duration
* in seconds of the new timeout.
*
* \return a BT:NodeStatus::SUCCESS if the input is valid.
*
*/
BT
::
NodeStatus
update_timeout
(
BT
::
TreeNode
&
self
);
};
};
...
...
This diff is collapsed.
Click to expand it.
src/iri_ana_nav_module_bt.cpp
+
44
−
1
View file @
a143f5ef
...
@@ -26,6 +26,8 @@ void CIriAnaNavModuleBT::init(IriBehaviorTreeFactory &factory)
...
@@ -26,6 +26,8 @@ void CIriAnaNavModuleBT::init(IriBehaviorTreeFactory &factory)
BT
::
PortsList
frame_port
=
{
BT
::
InputPort
<
std
::
string
>
(
"frame_id"
)
};
BT
::
PortsList
frame_port
=
{
BT
::
InputPort
<
std
::
string
>
(
"frame_id"
)
};
BT
::
PortsList
current_pose_port
=
{
BT
::
OutputPort
<
double
>
(
"current_x_nav"
),
BT
::
OutputPort
<
double
>
(
"current_y_nav"
),
BT
::
OutputPort
<
double
>
(
"current_yaw_nav"
)
};
BT
::
PortsList
current_pose_port
=
{
BT
::
OutputPort
<
double
>
(
"current_x_nav"
),
BT
::
OutputPort
<
double
>
(
"current_y_nav"
),
BT
::
OutputPort
<
double
>
(
"current_yaw_nav"
)
};
BT
::
PortsList
costmap_port
=
{
BT
::
InputPort
<
double
>
(
"rate_hz"
)
};
BT
::
PortsList
costmap_port
=
{
BT
::
InputPort
<
double
>
(
"rate_hz"
)
};
BT
::
PortsList
enable_timeout_port
=
{
BT
::
InputPort
<
double
>
(
"timeout"
)
};
BT
::
PortsList
update_timeout_port
=
{
BT
::
InputPort
<
double
>
(
"timeout"
)
};
// detect huge variation in path
// detect huge variation in path
BT
::
PortsList
path_distance_port
=
{
BT
::
OutputPort
<
double
>
(
"path_distance"
)
};
BT
::
PortsList
path_distance_port
=
{
BT
::
OutputPort
<
double
>
(
"path_distance"
)
};
...
@@ -60,6 +62,10 @@ void CIriAnaNavModuleBT::init(IriBehaviorTreeFactory &factory)
...
@@ -60,6 +62,10 @@ void CIriAnaNavModuleBT::init(IriBehaviorTreeFactory &factory)
factory
.
registerSimpleAction
(
"costmaps_enable_auto_clear_nav_3d"
,
std
::
bind
(
&
CIriAnaNavModuleBT
::
costmaps_enable_auto_clear
,
this
,
std
::
placeholders
::
_1
),
costmap_port
);
factory
.
registerSimpleAction
(
"costmaps_enable_auto_clear_nav_3d"
,
std
::
bind
(
&
CIriAnaNavModuleBT
::
costmaps_enable_auto_clear
,
this
,
std
::
placeholders
::
_1
),
costmap_port
);
factory
.
registerSimpleAction
(
"costmaps_disable_auto_clear_nav_3d"
,
std
::
bind
(
&
CIriAnaNavModuleBT
::
costmaps_disable_auto_clear
,
this
));
factory
.
registerSimpleAction
(
"costmaps_disable_auto_clear_nav_3d"
,
std
::
bind
(
&
CIriAnaNavModuleBT
::
costmaps_disable_auto_clear
,
this
));
factory
.
registerSimpleAction
(
"reset_goal_variables_nav_3d"
,
std
::
bind
(
&
CIriAnaNavModuleBT
::
reset_goal_variables
,
this
));
factory
.
registerSimpleAction
(
"reset_goal_variables_nav_3d"
,
std
::
bind
(
&
CIriAnaNavModuleBT
::
reset_goal_variables
,
this
));
factory
.
registerSimpleAction
(
"enable_timeout_nav_3d"
,
std
::
bind
(
&
CIriAnaNavModuleBT
::
enable_timeout
,
this
,
std
::
placeholders
::
_1
),
enable_timeout_port
);
factory
.
registerSimpleAction
(
"disable_timeout_nav_3d"
,
std
::
bind
(
&
CIriAnaNavModuleBT
::
disable_timeout
,
this
));
factory
.
registerSimpleAction
(
"update_timeout_nav_3d"
,
std
::
bind
(
&
CIriAnaNavModuleBT
::
update_timeout
,
this
,
std
::
placeholders
::
_1
),
update_timeout_port
);
// detect huge variation in path
// detect huge variation in path
factory
.
registerSimpleAction
(
"get_current_path_distance_nav_3d"
,
std
::
bind
(
&
CIriAnaNavModuleBT
::
get_current_path_distance
,
this
,
std
::
placeholders
::
_1
),
path_distance_port
);
factory
.
registerSimpleAction
(
"get_current_path_distance_nav_3d"
,
std
::
bind
(
&
CIriAnaNavModuleBT
::
get_current_path_distance
,
this
,
std
::
placeholders
::
_1
),
path_distance_port
);
factory
.
registerSimpleAction
(
"get_current_velocity_nav_3d"
,
std
::
bind
(
&
CIriAnaNavModuleBT
::
get_current_velocity
,
this
,
std
::
placeholders
::
_1
),
velocity_port
);
factory
.
registerSimpleAction
(
"get_current_velocity_nav_3d"
,
std
::
bind
(
&
CIriAnaNavModuleBT
::
get_current_velocity
,
this
,
std
::
placeholders
::
_1
),
velocity_port
);
...
@@ -406,6 +412,44 @@ BT::NodeStatus CIriAnaNavModuleBT::reset_goal_variables()
...
@@ -406,6 +412,44 @@ BT::NodeStatus CIriAnaNavModuleBT::reset_goal_variables()
return
BT
::
NodeStatus
::
SUCCESS
;
return
BT
::
NodeStatus
::
SUCCESS
;
}
}
BT
::
NodeStatus
CIriAnaNavModuleBT
::
enable_timeout
(
BT
::
TreeNode
&
self
)
{
BT
::
Optional
<
double
>
timeout_input
=
self
.
getInput
<
double
>
(
"timeout"
);
if
(
!
timeout_input
)
{
ROS_ERROR
(
"CIriAnaNavModuleBT:enable_timeout Incorrect or missing required input [timeout]"
);
return
BT
::
NodeStatus
::
FAILURE
;
}
double
timeout_desired
=
timeout_input
.
value
();
this
->
nav
.
enable_timeout
(
timeout_desired
);
return
BT
::
NodeStatus
::
SUCCESS
;
}
BT
::
NodeStatus
CIriAnaNavModuleBT
::
disable_timeout
()
{
this
->
nav
.
disable_timeout
();
return
BT
::
NodeStatus
::
SUCCESS
;
}
BT
::
NodeStatus
CIriAnaNavModuleBT
::
update_timeout
(
BT
::
TreeNode
&
self
)
{
BT
::
Optional
<
double
>
timeout_input
=
self
.
getInput
<
double
>
(
"timeout"
);
if
(
!
timeout_input
)
{
ROS_ERROR
(
"CIriAnaNavModuleBT:update_timeout Incorrect or missing required input [timeout]"
);
return
BT
::
NodeStatus
::
FAILURE
;
}
double
timeout_new
=
timeout_input
.
value
();
this
->
nav
.
update_timeout
(
timeout_new
);
return
BT
::
NodeStatus
::
SUCCESS
;
}
BT
::
NodeStatus
CIriAnaNavModuleBT
::
get_current_path_distance
(
BT
::
TreeNode
&
self
)
BT
::
NodeStatus
CIriAnaNavModuleBT
::
get_current_path_distance
(
BT
::
TreeNode
&
self
)
{
{
ROS_DEBUG
(
"CIriAnaNavModuleBT:get_current_path_distance"
);
ROS_DEBUG
(
"CIriAnaNavModuleBT:get_current_path_distance"
);
...
@@ -704,7 +748,6 @@ BT::NodeStatus CIriAnaNavModuleBT::set_goal_frame(BT::TreeNode& self)
...
@@ -704,7 +748,6 @@ BT::NodeStatus CIriAnaNavModuleBT::set_goal_frame(BT::TreeNode& self)
ROS_INFO
(
"CIriAnaNavModuleBT:set_goal_frame"
);
ROS_INFO
(
"CIriAnaNavModuleBT:set_goal_frame"
);
BT
::
Optional
<
std
::
string
>
frame_id_input
=
self
.
getInput
<
std
::
string
>
(
"frame_id"
);
BT
::
Optional
<
std
::
string
>
frame_id_input
=
self
.
getInput
<
std
::
string
>
(
"frame_id"
);
if
(
!
frame_id_input
)
if
(
!
frame_id_input
)
{
{
ROS_ERROR
(
"CIriAnaNavModuleBT:set_goal_frame Incorrect or missing required input [frame_id]"
);
ROS_ERROR
(
"CIriAnaNavModuleBT:set_goal_frame Incorrect or missing required input [frame_id]"
);
...
...
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment