Hi,
I have been trying to launch mrpt_reactivenav2d using stage_ros + fake_navigation + map_server. (The reason being, mvsim does not seem to be compatible with MRPT 1.9.9 >= yet)
So, every node starts ok, but when I send a goal to the navigator (from a point clicked in rviz) I get an error and navigation is cancelled due to a timeout.
These are the tag nodes I think are relevant. My guess is the problem lies in the ".ini" files in the package itself since stage responds normally to my keyboard commands and the vehicle shape is received by the reactive node without trouble. Note that I am using the config files it provides the repository. Or maybe mvsim plays a role that I am not accounting for.
<node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" launch-prefix="$(arg launch_prefix)" />
<node pkg="fake_localization" type="fake_localization" name="fake_localization" output="screen">
<param name="odom_frame_id" value="odom"/>
<param name="global_frame_id" value="/map"/>
<param name="base_frame_id" value="base_link"/>
</node>
<node pkg="stage_ros" type="stageros" name="stage_ros" respawn="false" output="screen" args="$(arg world_file)" >
<param name="laser_scan_topic" value="/laser_scan"/>
<param name="laser_frame_id" value="/laser_stage" />
<param name="disable_odometry" value="$(arg disable_odometry)" /> #if true, no topic, neither tf transform are published for odometry
<param name="odom_topic" value="/odom"/>
<param name="odom_frame_id" value="/odom" />
</node>
<node pkg="mrpt_local_obstacles" type="mrpt_local_obstacles_node" name="mrpt_local_obstacles_node" output="screen">
<param name="source_topics_2dscan" value="laser_scan"/>
<param name="show_gui" value="false"/>
<param name="time_window" value="60"/>
</node>
<!--
I'm publishing the robot shape in a very simple node found here:
https://github.com/MariotoA/shape_publisher
-->
<node pkg="shape_publisher" type="shape_publisher" name="shape_publisher" output="screen" launch-prefix="xterm -e"/>
<node pkg="mrpt_reactivenav2d" type="mrpt_reactivenav2d_node" name="mrpt_reactivenav2d_node" output="screen">
<!-- *Important*: This external config file holds the most important navigation settings -->
<param name="cfg_file_reactive" value="$(find mrpt_reactivenav2d)/tutorial/reactive2d_config.ini"/>
<!-- This is to allow the reactive responsive to RVIZ GUI commands -->
<remap from="reactive_nav_goal" to="/move_base_simple/goal" />
<!-- Enable logs to ~/.ros/reactivenav.logs , use navlog-viewer to open them -->
<!--<param name="save_nav_log" value="true"/> -->
<param name="topic_robot_shape" value="/chassis_polygon" />
</node>
<node name="rviz_player" pkg="rviz" type="rviz" args="-d $(find tfg_reactive_pkg)/rviz/simbot_reactive_only.rviz"/>
This is the error trace itself:
[ INFO] [1524665257.040733976, 7.600000000]: Nav target received via topic sub: (2.555,1.869, -23.832deg) [frame_id=/map]
[ INFO] [1524665257.041667984, 7.600000000]: [navigateTo] Starting navigation to [2.554826 1.868601 -23.831614]
[MRPT_navigator|INFO |16:07:37.0436] [CAbstractNavigator::navigationStep()] Starting Navigation. Watchdog initiated...
[MRPT_navigator|DEBUG|16:07:37.0441] [CAbstractNavigator::navigationStep()] Navigation Params:
navparams. Single target:
target_coords = (0.000,0.000,0.000 deg)
target_frame_id = "map"
targetAllowedDistance = 0.500
targetIsRelative = NO
targetIsIntermediaryWaypoint = NO
targetDesiredRelSpeed = 0.05
multiple_targets:
target[0]:
target_coords = (2.555,1.869,0.000 deg)
target_frame_id = "map"
targetAllowedDistance = 0.400
targetIsRelative = NO
targetIsIntermediaryWaypoint = NO
targetDesiredRelSpeed = 0.05
restrict_PTG_indices: []
[MRPT_navigator|DEBUG|16:07:37.0530] CWaypointsNavigator::checkHasReachedTarget() called with targetDist=3.60555 return=0 waypoint: wps.timestamp_nav_started=INVALID_TIMESTAMP
[MRPT_navigator|INFO |16:07:37.0566] [CReactiveNavigationSystem::STEP1_InitPTGs] Initializing PTG#0 (CPTG_DiffDrive_C,K=1
)...
[CPTG_DiffDrive_CollisionGridBased::initialize] Starting... *** THIS MAY TAKE A WHILE, BUT MUST BE COMPUTED ONLY ONCE!! **
Initializing PTG './ReacNavGrid_000.dat.gz'...loaded from file OK
[MRPT_navigator|INFO |16:07:41.0283] Done!
[MRPT_navigator|INFO |16:07:41.0287] [CReactiveNavigationSystem::STEP1_InitPTGs] Initializing PTG#1 (CPTG_DiffDrive_alpha,av=57deg,aw=57deg
)...
[CPTG_DiffDrive_CollisionGridBased::initialize] Starting... *** THIS MAY TAKE A WHILE, BUT MUST BE COMPUTED ONLY ONCE!! **
Initializing PTG './ReacNavGrid_001.dat.gz'...loaded from file OK
[MRPT_navigator|INFO |16:07:50.2053] Done!
[MRPT_navigator|INFO |16:07:50.2057] [CReactiveNavigationSystem::STEP1_InitPTGs] Initializing PTG#2 (CPTG_DiffDrive_C,K=-1
)...
[CPTG_DiffDrive_CollisionGridBased::initialize] Starting... *** THIS MAY TAKE A WHILE, BUT MUST BE COMPUTED ONLY ONCE!! **
Initializing PTG './ReacNavGrid_002.dat.gz'...loaded from file OK
[MRPT_navigator|INFO |16:07:54.4521] Done!
[MRPT_navigator|ERROR|16:08:32.6288] [CAbstractPTGBasedReactive::performNavigationStep] Stopping robot and finishing navigation due to exception:
=============== MRPT EXCEPTION =============
void mrpt::expr::CRuntimeCompiledExpression::compile(const string&, const std::map<std::__cxx11::basic_string, double>&, const string&), line 71:
Error compiling expression (name=score: collision_free_distance
): var effective_trg_d_norm := max(0,target_d_norm-move_cur_d); if (collision_free_distance>(effective_trg_d_norm+0.05), 1.0, collision_free_distance)
. Error: ERR186 - Undefined symbol: 'target_d_norm'
:
[MRPT_navigator|DEBUG|16:08:32.8244] CWaypointsNavigator::checkHasReachedTarget() called with targetDist=3.60555 return=0 waypoint: wps.timestamp_nav_started=INVALID_TIMESTAMP
[MRPT_navigator|WARN |16:08:32.8289] Timeout approaching the target. Aborting navigation.
[MRPT_navigator|ERROR|16:08:33.0357] [CAbstractNavigator::navigationStep()] Stopping Navigation due to a NAV_ERROR state!
I am currently using xubuntu-16.04, MRPT 1.9.9 (I think. I followed this and mrpt/utils does not exist for example) and ros-kinetic.
Also, I noticed just now this is the same bug as this one, but I get two navparams for some reason, one being, I assume, the robot pose and the other the goal I send.
I've emphasized in bold text the parts of the trace that seem suspicious to me. I did not include any map files since I believe the problem does not come from there, but maybe i'm mistaken, just ask if you need them.
Any idea? By the way, newbie here, so insights or explanations are well appreciated. Thanks a lot.
Best regards.
Update:
I've tried changing a couple of things myself.
First, I've modified my shape_publisher to stop sending the shape to the reactive node after five seconds, that way the shape is most likely set at the start without continuously changing it. You can find it here.
Then I get a similar exception but with Expression name collision_free_distance
instead.
[MRPT_navigator|ERROR|19:06:34.2202] [CAbstractPTGBasedReactive::performNavigationStep] Stopping robot and finishing navigation due to exception:
=============== MRPT EXCEPTION =============
int mrpt::nav::CMultiObjectiveMotionOptimizerBase::decide(const std::vectormrpt::nav::TCandidateMovementPTG&, mrpt::nav::CMultiObjectiveMotionOptimizerBase::TResultInfo&), line 80:
Error: Expression name collision_free_distance
already exists as an input variable.:
[MRPT_navigator|DEBUG|19:06:34.8490] CWaypointsNavigator::checkHasReachedTarget() called with targetDist=3.60555 return=0 waypoint: wps.timestamp_nav_started=INVALID_TIMESTAMP
[MRPT_navigator|WARN |19:06:34.8686] Timeout approaching the target. Aborting navigation.
[MRPT_navigator|ERROR|19:06:34.8754] [CAbstractNavigator::navigationStep()] Stopping Navigation due to a NAV_ERROR state!
I've also got 2 messages in my cmd_vel topic:
linear:
x: 0.0
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.0
linear:
x: 0.0
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.0
After that, I've tried launching with this file. Using this config, surprisingly, navigation seems to take place, but is soon stopped due to a timeout.
[MRPT_navigator|INFO |18:15:27.8040] Done!
[MRPT_navigator|WARN |18:17:51.5740] Timing warning: Suspicious executionPeriod=191968.602 ms is far above the average of 100.000 ms
[MRPT_navigator|DEBUG|18:17:51.5783] CMD: (lin_vel=1.000 ang_vel=-0.814 ) speedScale=1.0000 T=57660.6ms Exec:43178.7ms|49003.9ms PTG#0
[MRPT_navigator|DEBUG|18:17:52.0406] CWaypointsNavigator::checkHasReachedTarget() called with targetDist=3.60555 return=0 waypoint: wps.timestamp_nav_started=INVALID_TIMESTAMP
[MRPT_navigator|WARN |18:28:54.2376] Timing warning: Suspicious executionPeriod=662659.381 ms is far above the average of 57660.581 ms
[MRPT_navigator|DEBUG|18:28:54.2381] CMD: (lin_vel=1.000 ang_vel=-0.814 ) speedScale=1.0000 T=239160.2ms Exec:228777.7ms|232960.8ms PTG#0
[MRPT_navigator|DEBUG|18:28:54.5894] CWaypointsNavigator::checkHasReachedTarget() called with targetDist=3.71347 return=0 waypoint: wps.timestamp_nav_started=INVALID_TIMESTAMP
[MRPT_navigator|WARN |18:28:54.5920] Timeout approaching the target. Aborting navigation.
[MRPT_navigator|ERROR|18:28:54.5954] [CAbstractNavigator::navigationStep()] Stopping Navigation due to a NAV_ERROR state!
Maybe I should edit this issue's title since velocity commands seem to be generating, just in an weird way.