strands-project / strands_navigation Goto Github PK

View Code? Open in Web Editor NEW

40.0 35.0 47.0 2.35 MB

Higher-level navigation capabilities

CMake 20.10% C++ 1.63% Python 78.27%

strands_navigation's Introduction

strands_navigation

Higher-level navigation capabilities

Here is a diagram of the interactions between the components (source):

strands_navigation's People

Stargazers

Watchers

strands_navigation's Issues

Add monitored nav node to topological navigation launch file

As described in strands-project/strands_recovery_behaviours#8 the topological nav launch file should also run

rosrun monitored_navigation monitored_nav.py

as it is required for any behaviour.

topological_map_update.py not updating

This is not urgent, just not to forget about it. I don't know why, but

$ rosrun topological_utils topological_map_update.py

does not update the topological map, while

$ rostopic pub /update_map std_msgs/Time "data:
  secs: 0
  nsecs: 0"

does.

test small width doors

G4S doors are 75cm +/- 1cm. We need to test if the robot is able to navigate through this width using the current navigation

Metapackage does not define run_depends

The metapackage strands_navigation is currently not defining any run_depends which makes it useless. Since strands_base depends on it, it should contain all the packages in here.

This leads to things like descriptions that use topological_utils but this is not installed by default.

How should we represent the charging point in the system?

I can imagine two ways to encode charging point in the system.

The way we do it now: the node 1m away from charging point is the waypoint the robot goes to. It can then call the charging action with dock to start charging and undock to leave the station.
The charging point itself is a node in the topological map and can be reached from a nearby facing node by a dock transition in the topological map and left via an undock transition.

The latter has the advantage that we can then refer to the docking station itself as a location in our tasks such that tasks can be performed whilst docked. We can easily wrap the existing docking action server into separate dock and undock action servers. Therefore I prefer the second option, but as it's a change to our current way of doing things, I'm open to discussion.

Global costmap being cleared

Probably I'm doing something wrong, but during backtrack behaviour the global costmap gets emptied somehow.

What you see in slightly darker gray is the costmap coming from the no-go map.

Output of the last time it happened:

[ INFO] [1416593474.709253931]: Got new plan
[ WARN] [1416593474.714531285]: DWA planner failed to produce path.
[ WARN] [1416593474.809003186]: Clearing costmap to unstuck robot (3.000000m).
[ INFO] [1416593475.209599130]: Got new plan
[ WARN] [1416593475.215077867]: DWA planner failed to produce path.
[ WARN] [1416593475.309008566]: Clearing costmap to unstuck robot (1.840000m).
[ INFO] [1416593475.709013111]: Got new plan
[ WARN] [1416593475.714326009]: DWA planner failed to produce path.
[ERROR] [1416593475.809008000]: Aborting because a valid control could not be found. Even after executing all re
covery behaviors
[INFO] [WallTime: 1416593475.822154] State machine transitioning 'NAVIGATION':'planner_failure'-->'RECOVER_NAVIG
ATION'
[INFO] [WallTime: 1416593475.823415] State machine starting in initial state 'CLEAR_COSTMAPS' with userdata: 
        ['goal', 'n_nav_fails']
[ INFO] [1416593480.954840476]: Requesting the map...
[ INFO] [1416593481.259107806]: Resizing costmap to 800 X 1400 at 0.020000 m/pix
[ INFO] [1416593481.356322228]: Received a 800 X 1400 map at 0.020000 m/pix
[ INFO] [1416593481.373878552]: Requesting the map...
[ INFO] [1416593481.576694367]: Resizing costmap to 800 X 1400 at 0.020000 m/pix
[ INFO] [1416593481.675289357]: Received a 800 X 1400 map at 0.020000 m/pix
[INFO] [WallTime: 1416593481.908846] Waiting for services...
[INFO] [WallTime: 1416593481.922603] Done
[INFO] [WallTime: 1416593481.928399] State machine transitioning 'CLEAR_COSTMAPS':'do_other_recovery'-->'BACKTRA
CK'
[INFO] [WallTime: 1416593481.933768] State machine transitioning 'BACKTRACK':'failure'-->'NAV_HELP'

Making topological navigation navigate to closest node when not at a node

Hi,

I am planning on making topological navigation navigate to closest node when not at a node if there is no problem with it, currently the route is planned from closest node so if the robot is not at a node it will navigate to the next waypoint in the route, this is not a problem when it happens with move_base like actions but with other actions such as ramp behaviour it is a big problem, @bfalacerda @hawesie is this change OK or do you foresee any problem.

Associate nodes with a specific map in the database

`inc` not set in navigation.

@Jailander

There are multiple possible paths to reach this line which do not set the inc variable, and thus the line fails with an exception:

[ERROR] [WallTime: 1416824559.134127] Exception in your execute callback: local variable 'inc' referenced before
 assignment
Traceback (most recent call last):
  File "/opt/ros/hydro/lib/python2.7/dist-packages/actionlib/simple_action_server.py", line 299, in executeLoop
    self.execute_callback(goal)
  File "/opt/ros/hydro/lib/topological_navigation/navigation.py", line 161, in executeCallback
    self.navigate(goal.target)
  File "/opt/ros/hydro/lib/topological_navigation/navigation.py", line 258, in navigate
    tries+=inc
UnboundLocalError: local variable 'inc' referenced before assignment

Missing NavStatistics message file missing

Traceback (most recent call last):
  File "/opt/strands/strands_hydro_catkin_ws/src/strands_navigation/topological_navigation/scripts/navigation.py", line 15, in <module>
    from strands_navigation_msgs.msg import NavStatistics
ImportError: cannot import name NavStatistics

I only see strands_navigation_msgs/NavStatus

Make strands_navigation robot-indepdent

For the release of this repo I would like to make sure it doesn't depend on the scitos hardware and therefore not depend on scitos-specific messages. Current monitored navigation uses the following scitos messages:

MotorStatus
EnableMotors
ResetMotorStop

It also uses the scitos_ptu package to control the PTU for something.

I propose that we/I implement an abstraction layer for the scitos messages or find an existing one.

Can the PTU part be replaced with the more generic PTU messages?

Code to collect timing data on topological map

Code and documentation of the correct way to do this.

Stuff being added to global costmap?

By now, Karl doesn't want to come near my desk anymore, because the global costmap contains high-cost blobs at the location of that topological node. I don't know how they got there, they don't appear in the local costmap (I render them in different colors, so I'm sure about that) and they don't get cleared when I push him there.

What could possibly add points to the global costmap? I'm confused.

Exception when helping with joystick

When karl gets stuck, I help him with the (muxed) joystick because we don't have a GUI yet. It works and when I give up joystick control, he moves on, but I see the following:

[INFO] [WallTime: 1415963551.522795] Concurrent state 'NAV_SM' returned outcome 'succeeded' on termination.
[INFO] [WallTime: 1415963551.527681] Concurrent state 'pause_nav' returned outcome 'preempted' on termination.
[INFO] [WallTime: 1415963551.531063] Concurrent state 'stuck_on_carpet' returned outcome 'preempted' on termination.
[INFO] [WallTime: 1415963551.532216] Concurrent Outcomes: {'pause_nav': 'preempted', 'NAV_SM': 'succeeded', 'stuck_on_carpet': 'preempted', 'bumper': 'preempted'}
[INFO] [WallTime: 1415963551.532414] Concurrent state 'bumper' returned outcome 'preempted' on termination.
[INFO] [WallTime: 1415963551.532795] State machine terminating 'MONITORED_NAV':'succeeded':'succeeded'
[INFO] [WallTime: 1415963551.533156] SUCCEEDED
[ERROR] [WallTime: 1415963551.534700] InvalidUserCodeError: Could not execute child termination callback: Traceback (most recent call last):
  File "/opt/ros/hydro/lib/python2.7/dist-packages/smach/concurrence.py", line 348, in _state_runner
    preempt_others = self._child_termination_cb(self._child_outcomes)
  File "/opt/ros/hydro/lib/python2.7/dist-packages/monitored_navigation/navigation.py", line 201, in child_term_cb
    if  outcome_map[name] == 'invalid':
KeyError: 'pause_nav'

Exception in thread concurrent_split:bumper:
Traceback (most recent call last):
  File "/usr/lib/python2.7/threading.py", line 551, in __bootstrap_inner
    self.run()
  File "/usr/lib/python2.7/threading.py", line 504, in run
    self.__target(*self.__args, **self.__kwargs)
  File "/opt/ros/hydro/lib/python2.7/dist-packages/smach/concurrence.py", line 350, in _state_runner
    raise smach.InvalidUserCodeError("Could not execute child termination callback: "+traceback.format_exc())
InvalidUserCodeError: <smach.exceptions.InvalidUserCodeError instance at 0x7f953c17a4d0>

[INFO] [WallTime: 1415963552.291340] setting parameters back
[INFO] [WallTime: 1415963552.292545] navigation finished on Friday, November 14 2014, at 12:12:32 hours (24/2)
[INFO] [WallTime: 1415963552.292878] Waiting for services...
[INFO] [WallTime: 1415963552.300414] Done

travel_time_estimator should use graph structure

Currently travel_time_estimator use straight line distance between start and target to estimate travel time. This is far from accurate, so it would be better if it used the graph structure e.g. reusing the search from the navigation node and working with the path length.

Create TopologicalNode message type

This is necessary to store nodes in the datacentre.

mongodb_store_cpp_client fails when testing for release

Hi I think 'mongodb_store_cpp_client' has to be removed from dependencies on message_store_map_switcher @hawesie is this right?

package dependencies and launch file for topological navigation

If you want to run topological navigation you need to have monitored navigation running. If you want to run monitored navigation running you need the human help manager (at least). These should be added as runtime dependencies of the packages.

Recovery behaviours for non-strands users?

Following on from strands-project/strands_recovery_behaviours#8 are there any non-strands/scitos specific recovery behaviours that could be added to this repo to enable new users to work with monitored navigation?

message-store-map-switcher cannot be released

There is an error whilst building it see:

https://lcas.lincoln.ac.uk/jenkins/view/Releases/job/ros-hydro-message-store-map-switcher_binarydeb_precise_amd64/2/console

@hawesie do you know why?

Not turning on the spot.

We have small corridors. Often, it would be really necessary for Karl when he's on a topological node to turn on the spot and then start moving. He doesn't want to do that, he always wants to turn+drive forward, which fails because there's not enough space.

Is this intended? Where can I debug and tune it?

Waypoint RVIZ Gui problem

After #43, when I try to move a waypoint in RViz, I get:

[ERROR] [WallTime: 1399280078.407084] bad callback: <bound method InteractiveMarkerServer.processFeedback of <interactive_markers.interactive_marker_server.InteractiveMarkerServer instance at 0x2aec050>>
Traceback (most recent call last):
  File "/opt/ros/hydro/lib/python2.7/dist-packages/rospy/topics.py", line 682, in _invoke_callback
    cb(msg)
  File "/opt/ros/hydro/lib/python2.7/dist-packages/interactive_markers/interactive_marker_server.py", line 356, in processFeedback
    marker_context.default_feedback_cb(feedback)
  File "/opt/strands/strands_catkin_ws/src/strands_navigation/topological_utils/scripts/visualise_map.py", line 292, in _marker_feedback
    self.topo_map.update_node_waypoint(feedback.marker_name, feedback.pose)
  File "/opt/strands/strands_catkin_ws/src/strands_navigation/topological_navigation/src/topological_navigation/topological_map.py", line 40, in update_node_waypoint
    available[0][0].pose = new_pose
IndexError: list index out of range

The way point does not update.

Topological edges to unknown nodes

If someone would rename nodes in the topological map txt file and misspelled one of the nodes while defining the edges, at least the visualiser will just pick a random node to draw an edge to.
E.g.:

node:
         MyWayPoint1
         ...
         edges:
                 WrongNodeName, move_base

Where WrongNodeName does not exist. It instead just connects to another one. I did not test that for the actual topological navigation but maybe an error output would be nice if there is an edge to an undefined node.

Navigation when someone is approaching from behind

@TobKoer reported the following from the AAF test run

"Overtaking the robot or avoiding it seems to be unproblematic, but when moving a bed through the corridors BEHIND the robot, it moves too slow. Maybe we should take up the idea of having a camera in the back of Werner again?!"

Can we add something like this for topological transitions along corridors?

get topological navigation to use monitored nav

As we discussed before, the topological navigation should always call the monitored_navigation action server and give it a string saying which continuous nav actionlib server to execute and a "move_base goal" (we assume all continuous nav actionlib server receive the same type of goals as move_base). My latest pull request gets the monitored_navigation to a point where this can be done, we just have to edit the topological_navigation. @Jailander , is the ramp_climb (and others) ready to receive the same type of goals as move_base?

previous_positions_service should be moved into this repo?

The build fails because this package is not being found, see https://lcas.lincoln.ac.uk/jenkins/job/devel-hydro-strands_navigation/ARCH_PARAM=amd64,UBUNTU_PARAM=precise,label=devel/1/console

should it be moved here?

Recovery actions when non-move base actions are being executed

Hi, I was wondering if it will be possible to add a recovery action sending the robot to navigate to the WayPoint of the node it is currently in when using Ramp Climb and possibly Door pass, this seems to be one of the biggest problems for linda since she sometimes call these actions not facing the Ramp/Door so she gets stuck

Switch to db-based map?

Do we want to switch to the message store-backed message_store_map_switcher instead of the standard map_server for systems?

move_base died after door pass

IN STATE PASS

GETTING FURTHER AWAY FROM GOAL

('leftMinim: %f, rightMinin:  %f \n', 0.385387889684557, 0.7524614556617635)
('base_cmd.linear.x = %f, base_cmd.angular.z= %f\n', 0.15, -0.734147131954413)
('leftMinim > 0.1 && rightMinim >0.1, updating: base_cmd.linear.x = %f, base_cmd.angular.z= %f\n', 0.2, 0)
('publishing to cmd_vel: base_cmd.linear.x = %f, base_cmd.angular.z= %f\n', 0.2, 0)
CHECKING PREEMPTED

IN STATE PASS

GETTING FURTHER AWAY FROM GOAL

('leftMinim: %f, rightMinin:  %f \n', 0.431747093383087, 0.7284459360947235)
('base_cmd.linear.x = %f, base_cmd.angular.z= %f\n', 0.15, -0.5933976854232731)
('leftMinim > 0.1 && rightMinim >0.1, updating: base_cmd.linear.x = %f, base_cmd.angular.z= %f\n', 0.2, 0)
getting further away from goal, going to state FAIL

('publishing to cmd_vel: base_cmd.linear.x = %f, base_cmd.angular.z= %f\n', 0.2, 0)
STOPPING

STOPPED

DOOR PASS FAILED

[INFO] [WallTime: 1416738587.911160] State machine transitioning 'NAVIGATION':'planner_failure'-->'RECOVER_NAVIGATION'
[INFO] [WallTime: 1416738587.911951] State machine starting in initial state 'CLEAR_COSTMAPS' with userdata: 
        ['goal', 'n_nav_fails']
[ INFO] [1416738593.093443994]: Requesting the map...
[ INFO] [1416738593.400545269]: Received a 4000 X 4000 map at 0.050000 m/pix
[ INFO] [1416738593.419713628]: Requesting the map...
[ INFO] [1416738593.854712333]: Received a 4000 X 4000 map at 0.050000 m/pix
[ WARN] [1416738593.887566309]: Map update loop missed its desired rate of 5.0000Hz... the loop actually took 0.3482 seconds
[WARN] [WallTime: 1416738594.107786] Issue clearing costmaps.
[INFO] [WallTime: 1416738594.108352] State machine transitioning 'CLEAR_COSTMAPS':'do_other_recovery'-->'BACKTRACK'
[move_base-7] process has died [pid 2247, exit code -11, cmd /opt/ros/hydro/lib/move_base/move_base /move_base/local_costmap/voxel_grid:=/voxel_grid __name:=move_base __log:=/home/strands/.ros/log/4d069c4
e-71a1-11e4-9515-00032d22587c/move_base-7.log].
log file: /home/strands/.ros/log/4d069c4e-71a1-11e4-9515-00032d22587c/move_base-7*.log
Got the previous position:  -3.51143007073 ,  -1.97553955057 ,  6.93889390391e-18
Managed to republish pointcloud.
[ERROR] [WallTime: 1416738598.358417] Exception in your execute callback: unable to connect to service: [Errno 111] Connection refused
Traceback (most recent call last):
  File "/opt/ros/hydro/lib/python2.7/dist-packages/actionlib/simple_action_server.py", line 299, in executeLoop
    self.execute_callback(goal)
  File "/opt/ros/hydro/lib/backtrack_behaviour/backtrack_server.py", line 163, in execute
    config = self.move_base_reconfig_client.update_configuration(params)
  File "/opt/ros/hydro/lib/python2.7/dist-packages/dynamic_reconfigure/client.py", line 212, in update_configuration
    msg    = self._set_service(config).config
  File "/opt/ros/hydro/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 435, in __call__
    return self.call(*args, **kwds)
  File "/opt/ros/hydro/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 505, in call
    raise ServiceException("unable to connect to service: %s"%e)
ServiceException: unable to connect to service: [Errno 111] Connection refused

Preempting and aborting actions via topological/monitored navigation

I'm just trying to work out how to make the complete task system robust to errors and changes in demands on the robot.

So, some questions:

@Jailander will the topological navigation action server ever abort? If so, under want conditions?
@Jailander will the topological navigation action server respond to preemption from the client (i.e. the task executor)?
@bfalacerda same questions really: will the monitored navigation ever abort, under what conditions?
@bfalacerda ca the monitored navigation be preempted?

Reimplement navigation node as policy executor

Ignore until after the marathon ;)

Given that policy execution seems to be the future of executing topological navigation actions, we should update the current navigation node to work as an policy executor, e.g. using search to create a policy for the goal it is given. This could be a nice student/intern project.

Indigo release

Please can whoever is releasing this repo create an Indigo release too? I'll be happy to bug fix once it's up.

Monitored navigation with ramp and door actions

Seems to be a mismatch in message types.

[ERROR] [1398871311.755978903]: Client [/monitored_nav] wants topic /rampClimbingServer/feedback to have datatype/md5sum [move_base_msgs/M
oveBaseActionFeedback/7d1870ff6e0decea702b943b5af0b42e], but our version has [strands_navigation_msgs/MonitoredNavigationActionFeedback/aa
e20e09065c3809e8a8e87c4c8953fd]. Dropping connection.
[WARN] [WallTime: 1398871311.996712] Could not process inbound connection: topic types do not match: [strands_navigation_msgs/MonitoredNav
igationActionGoal] vs. [move_base_msgs/MoveBaseActionGoal]{'topic': '/rampClimbingServer/goal', 'tcp_nodelay': '0', 'md5sum': '3f98791f075
d5e3e8fee804d9939d167', 'type': 'strands_navigation_msgs/MonitoredNavigationActionGoal', 'callerid': '/ramp_climbing'}
[ERROR] [1398871312.759241929]: Client [/monitored_nav] wants topic /rampClimbingServer/feedback to have datatype/md5sum [move_base_msgs/M
oveBaseActionFeedback/7d1870ff6e0decea702b943b5af0b42e], but our version has [strands_navigation_msgs/MonitoredNavigationActionFeedback/aa
e20e09065c3809e8a8e87c4c8953fd]. Dropping connection.
[ERROR] [1398871313.761835581]: Client [/monitored_nav] wants topic /rampClimbingServer/feedback to have datatype/md5sum [move_base_msgs/M
oveBaseActionFeedback/7d1870ff6e0decea702b943b5af0b42e], but our version has [strands_navigation_msgs/MonitoredNavigationActionFeedback/aa
e20e09065c3809e8a8e87c4c8953fd]. Dropping connection.
[ERROR] [1398871315.765261187]: Client [/monitored_nav] wants topic /rampClimbingServer/feedback to have datatype/md5sum [move_base_msgs/M
oveBaseActionFeedback/7d1870ff6e0decea702b943b5af0b42e], but our version has [strands_navigation_msgs/MonitoredNavigationActionFeedback/aa
e20e09065c3809e8a8e87c4c8953fd]. Dropping connection.
[ERROR] [1398871319.770717079]: Client [/monitored_nav] wants topic /rampClimbingServer/feedback to have datatype/md5sum [move_base_msgs/M
oveBaseActionFeedback/7d1870ff6e0decea702b943b5af0b42e], but our version has [strands_navigation_msgs/MonitoredNavigationActionFeedback/aa
e20e09065c3809e8a8e87c4c8953fd]. Dropping connection.
[ERROR] [1398871327.779522472]: Client [/monitored_nav] wants topic /rampClimbingServer/feedback to have datatype/md5sum [move_base_msgs/MoveBaseActionFeedback/7d1870ff6e0decea702b943b5af0b42e], but our version has [strands_navigation_msgs/MonitoredNavigationActionFeedback/aae20e09065c3809e8a8e87c4c8953fd]. Dropping connection.

Refactor type names to obey naming conventions.

edge.msg node.msg vertex.msg should all be capitalised.

topological nav should not depend on scitos_msgs

I see the dependeny on the package.xml https://github.com/strands-project/strands_navigation/blob/hydro-devel/topological_navigation/package.xml#L22 but I don't really know where it's needed, I'm guessing it's some old thing that just stayed in there.

Anyway, we shouldn't have strands_navigation depending on it, so it needs to be removed

[nav_goald_generator] Missing import for NavGoals service

Traceback (most recent call last):
  File "/opt/ros/hydro/lib/nav_goals_generator/nav_goals_generator.py", line 307, in <module>
    NavGoalsGenerator()
  File "/opt/ros/hydro/lib/nav_goals_generator/nav_goals_generator.py", line 85, in __init__
    self.ser = rospy.Service('/nav_goals', NavGoals, self.nav_goals_service)
NameError: global name 'NavGoals' is not defined

Must have happened when the service was moved inside the package. Will fix.

Change node influence area to be a polygon

For more flexibility and integration with other behaviours.

I believe @kunzel has some code for checking whether a robot pose is within a polygon which can be used for this.

IndexError: list index out of range

[INFO] [WallTime: 1416758059.995891] Preempt on state machine requested!                                                                        │···························································
[INFO] [WallTime: 1416758059.996490] Concurrent state 'magnetic_strip' returned outcome 'preempted' on termination.                             │···························································
[INFO] [WallTime: 1416758060.004407] Concurrent state 'service_pause_nav' returned outcome 'preempted' on termination.                          │···························································
[INFO] [WallTime: 1416758060.010669] Concurrent state 'pad_pause_nav' returned outcome 'preempted' on termination.                              │···························································
[INFO] [WallTime: 1416758060.015968] Concurrent state 'stuck_on_carpet' returned outcome 'preempted' on termination.                            │···························································
[INFO] [WallTime: 1416758060.017863] Concurrent state 'bumper' returned outcome 'preempted' on termination.                                     │···························································
[INFO] [WallTime: 1416758060.295990] going to waypoint in node resulted in                                                                      │···························································
False                                                                                                                                           │···························································
[INFO] [WallTime: 1416758060.413156] /topological_navigation: Navigating From WayPoint6 to WayPoint5                                            │···························································
[ERROR] [WallTime: 1416758060.413945] Exception in your execute callback: list index out of range                                               │···························································
Traceback (most recent call last):                                                                                                              │···························································
  File "/opt/ros/hydro/lib/python2.7/dist-packages/actionlib/simple_action_server.py", line 299, in executeLoop                                 │···························································
    self.execute_callback(goal)                                                                                                                 │···························································
  File "/opt/ros/hydro/lib/topological_navigation/navigation.py", line 161, in executeCallback                                                  │···························································
    self.navigate(goal.target)                                                                                                                  │···························································
  File "/opt/ros/hydro/lib/topological_navigation/navigation.py", line 224, in navigate                                                         │···························································
    route = self.search_route(Onode, target)                                                                                                    │···························································
  File "/opt/ros/hydro/lib/topological_navigation/navigation.py", line 299, in search_route                                                     │···························································
    children=to_expand[exp_index]._get_Children()                                                                                               │···························································
IndexError: list index out of range                                                                                                             │···························································
                                                                                                                                                │···························································
[INFO] [WallTime: 1416758060.723843] Waiting for services..

Karl going towards obstacles

Sometimes, Karl sees an obstacle, say front left of him, and instead of driving to the right (as according to the global plan), he decides to turn left, RIGHT INTO THE OBSTACLE. This is very strange behaviour I've been observing during the past previous weeks already.

Here is an example of it, the green arrows are the odometry, i.e. how he drove:

Assuming closest node is current node

Bob is not at any node, but wants to get to the ChargingNode. ChargingNode is the closest node (but not current). Top nav assumes that he's there already. Is that correct?

Convert topological map to MDP for optimal route planning

Error processing request: field goal.text must be of type str

When the robot's bumper hit something, I saw this ERROR (with a bit of context):

[INFO] [WallTime: 1416324189.110502] State machine transitioning 'MONITORED_NAV':'bumper_fail'-->'bumper_recover
'
[INFO] [WallTime: 1416324189.111786] State machine starting in initial state 'RESTART_BUMPER' with userdata: 
        ['recovered']
[INFO] [WallTime: 1416324190.113456] State machine transitioning 'RESTART_BUMPER':'restarted'-->'BUMPER_HELP'
[INFO] [WallTime: 1416324230.314577] State machine transitioning 'BUMPER_HELP':'try_restart'-->'RESTART_BUMPER'
[INFO] [WallTime: 1416324231.316258] State machine transitioning 'RESTART_BUMPER':'restarted'-->'BUMPER_HELP'
[INFO] [WallTime: 1416324271.355499] State machine transitioning 'BUMPER_HELP':'try_restart'-->'RESTART_BUMPER'
[INFO] [WallTime: 1416324272.357357] State machine transitioning 'RESTART_BUMPER':'restarted'-->'BUMPER_HELP'
[INFO] [WallTime: 1416324290.286259] Preempt on state machine requested!
[ERROR] [WallTime: 1416324290.454105] Error processing request: field goal.text must be of type str
['Traceback (most recent call last):\n', '  File "/opt/ros/hydro/lib/python2.7/dist-packages/rospy/impl/tcpros_s
ervice.py", line 623, in _handle_request\n    response = convert_return_to_response(self.handler(request), self.response_class)\n', '  File "/opt/ros/hydro/lib/python2.7/dist-packages/monitored_navigation/help_manager.py", line 50, in forward_callback\n    helper.process_help_request(req)\n', '  File "/opt/ros/hydro/lib/python2.7/dist-packages/monitored_navigation/ui_helper.py", line 21, in process_help_request\n    self.help_failed(req.failed_component, req.interaction_service,req.n_fails)\n', '  File "/opt/ros/hydro/lib/python2.7/dist-packages/strands_human_help/help_speech.py", line 53, in help_failed\n    self.call_speech(self.help_failed)\n', '  File "/opt/ros/hydro/lib/python2.7/dist-packages/strands_human_help/help_speech.py", line 57, in call_speech\n    self.speaker.send_goal(maryttsGoal(text=text))\n', '  File "/opt/ros/hydro/lib/python2.7/dist-packages/actionlib/simple_action_client.py", line 92, in send_goal\n    self.gh = self.action_client.send_goal(goal, self._handle_transition, self._handle_feedback)\n', '  File "/opt/ros/hydro/lib/python2.7/dist-packages/actionlib/action_client.py", line 542, in send_goal\n    return self.manager.init_goal(goal, transition_cb, feedback_cb)\n', '  File "/opt/ros/hydro/lib/python2.7/dist-packages/actionlib/action_client.py", line 458, in init_goal\n    self.send_goal_fn(action_goal)\n', '  File "/opt/ros/hydro/lib/python2.7/dist-packages/rospy/topics.py", line 802, in publish\n    raise ROSSerializationException(str(e))\n', 'ROSSerializationException: field goal.text must be of type str\n']
[WARN] [WallTime: 1416324290.454684] No means of asking for human help available.

update to use ros_datacentre

this repo is not using the new ros_datacentre. The interactions with the db should be updated, as in https://github.com/strands-project/autonomous_patrolling/blob/hydro-devel/waypoint_recorder/scripts/insert_in_db.py and https://github.com/strands-project/autonomous_patrolling/blob/hydro-devel/waypoint_patroller/src/waypoint_patroller/patroller.py

@Jailander do you have time to do this? I can't really test the topological navigation until it is done, so if you don't have time I can look into it.

correct email addresses in `package.xml`

in order for the right people to be notified by jenkins, please make sure that the maintainers are correctly noted in the package.xml

Navigation state machine gets stuck

When on the charging station, given a new goalnode off the station, the state machine hung like so: @bfalacerda

[INFO] [WallTime: 1416587118.235077] Concurrent state 'bumper' returned outcome 'preempted' on termination.                                                              [51/1977]
[INFO] [WallTime: 1416587118.238656] Concurrent state 'stuck_on_carpet' returned outcome 'preempted' on termination.
[INFO] [WallTime: 1416587118.240053] Concurrent Outcomes: {'stuck_on_carpet': 'preempted', 'magnetic_strip': 'preempted', 'service_pause_nav': 'preempted', 'bumper': 'preempted',
 'pad_pause_nav': 'preempted', 'NAV_SM': 'succeeded'}
[INFO] [WallTime: 1416587118.240952] State machine terminating 'MONITORED_NAV':'succeeded':'succeeded'
[INFO] [WallTime: 1416587118.241649] SUCCEEDED
[INFO] [WallTime: 1416587118.592642] setting parameters back
[INFO] [WallTime: 1416587118.593079] navigation finished on Friday, November 21 2014, at 16:25:18 hours (48/9)
[INFO] [WallTime: 1416587118.593431] Waiting for services...
[INFO] [WallTime: 1416587118.601426] Done
[INFO] [WallTime: 1416587137.647372] /topological_navigation: Navigating From ChargingPoint to WayPoint16
[INFO] [WallTime: 1416587137.648016] 6 Nodes on route
Do move_base to ChargingPoint
[INFO] [WallTime: 1416587137.722315] State machine starting in initial state 'MONITORED_NAV' with userdata: 
        ['goal', 'result']
[INFO] [WallTime: 1416587137.722747] Concurrence starting with userdata: 
        ['goal']
[INFO] [WallTime: 1416587137.731941] State machine starting in initial state 'NAVIGATION' with userdata: 
        ['goal', 'n_nav_fails']
[ERROR] [1416587137.736321748]: Quaternion has length close to zero... discarding as navigation goal
[INFO] [WallTime: 1416587137.741386] State machine transitioning 'NAVIGATION':'planner_failure'-->'RECOVER_NAVIGATION'
[INFO] [WallTime: 1416587137.750265] State machine starting in initial state 'CLEAR_COSTMAPS' with userdata: 
        ['goal', 'n_nav_fails']
[ INFO] [1416587142.825377373]: Requesting the map...
[ INFO] [1416587143.140471395]: Received a 4000 X 4000 map at 0.050000 m/pix
[ INFO] [1416587143.168593684]: Requesting the map...
[ WARN] [1416587143.363818602]: Map update loop missed its desired rate of 5.0000Hz... the loop actually took 0.2048 seconds
[ INFO] [1416587143.600470677]: Received a 4000 X 4000 map at 0.050000 m/pix
[INFO] [WallTime: 1416587144.002322] Waiting for services...
[INFO] [WallTime: 1416587144.010832] Done
[INFO] [WallTime: 1416587144.042484] State machine terminating 'CLEAR_COSTMAPS':'try_nav':'recovered_without_help'
[INFO] [WallTime: 1416587144.043485] State machine terminating 'RECOVER_NAVIGATION':'recovered_without_help':'recovered_without_help'
[INFO] [WallTime: 1416587144.044158] Concurrent state 'NAV_SM' returned outcome 'recovered_without_help' on termination.
[INFO] [WallTime: 1416587144.048207] Concurrent state 'magnetic_strip' returned outcome 'preempted' on termination.
[INFO] [WallTime: 1416587144.055992] Concurrent state 'service_pause_nav' returned outcome 'preempted' on termination.
[INFO] [WallTime: 1416587144.059743] Concurrent state 'pad_pause_nav' returned outcome 'preempted' on termination.
[INFO] [WallTime: 1416587144.061339] Concurrent state 'bumper' returned outcome 'preempted' on termination.
[INFO] [WallTime: 1416587144.064483] Concurrent state 'stuck_on_carpet' returned outcome 'preempted' on termination.
[INFO] [WallTime: 1416587144.066205] Concurrent Outcomes: {'stuck_on_carpet': 'preempted', 'magnetic_strip': 'preempted', 'service_pause_nav': 'preempted', 'bumper': 'preempted',
 'pad_pause_nav': 'preempted', 'NAV_SM': 'recovered_without_help'}
[INFO] [WallTime: 1416587144.067269] State machine terminating 'MONITORED_NAV':'recovered_without_help':'recovered_without_help'
[INFO] [WallTime: 1416587144.068019] ABORTED
[ WARN] [1416587144.111240096]: Map update loop missed its desired rate of 5.0000Hz... the loop actually took 0.3523 seconds
[INFO] [WallTime: 1416587144.429280] 6 Nodes on route
Do move_base to ChargingPoint
[INFO] [WallTime: 1416587144.477085] State machine starting in initial state 'MONITORED_NAV' with userdata: 
        ['goal', 'result']
[INFO] [WallTime: 1416587144.477684] Concurrence starting with userdata: 
        ['goal']
[INFO] [WallTime: 1416587144.485272] State machine starting in initial state 'NAVIGATION' with userdata: 
        ['goal', 'n_nav_fails']
[ERROR] [1416587144.498562724]: Quaternion has length close to zero... discarding as navigation goal
[INFO] [WallTime: 1416587144.503284] State machine transitioning 'NAVIGATION':'planner_failure'-->'RECOVER_NAVIGATION'
[INFO] [WallTime: 1416587144.505060] State machine starting in initial state 'CLEAR_COSTMAPS' with userdata: 
        ['goal', 'n_nav_fails']
[ INFO] [1416587149.625316366]: Requesting the map...
[ INFO] [1416587149.945208857]: Received a 4000 X 4000 map at 0.050000 m/pix
[ INFO] [1416587149.972409603]: Requesting the map...
[ WARN] [1416587150.197383020]: Map update loop missed its desired rate of 5.0000Hz... the loop actually took 0.2384 seconds
[ INFO] [1416587150.273944860]: Received a 4000 X 4000 map at 0.050000 m/pix
[move_base-4] process has died [pid 7914, exit code -11, cmd /opt/ros/hydro/lib/move_base/move_base __name:=move_base __log:=/home/strands/.ros/log/e1233e40-7193-11e4-a881-00032d22587c/move_base-4.log].
log file: /home/strands/.ros/log/e1233e40-7193-11e4-a881-00032d22587c/move_base-4*.log
[ERROR] [WallTime: 1416587151.134984] InvalidUserCodeError: Could not execute state 'CLEAR_COSTMAPS' of type '<strands_monitored_nav_states.recover_nav_states.ClearCostmaps object at 0x24e0c10>': Traceback (most recent call last):
  File "/opt/ros/hydro/lib/python2.7/dist-packages/smach/state_machine.py", line 241, in _update_once
    self._remappings[self._current_label]))
  File "/opt/ros/hydro/lib/python2.7/dist-packages/strands_monitored_nav_states/recover_nav_states.py", line 49, in execute
    self.clear_costmaps()
  File "/opt/ros/hydro/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 435, in __call__
    return self.call(*args, **kwds)
  File "/opt/ros/hydro/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 525, in call
    raise ServiceException("transport error completing service call: %s"%(str(e)))
ServiceException: transport error completing service call: unable to receive data from sender, check sender's logs for details

[ERROR] [WallTime: 1416587151.135459] State 'RECOVER_NAVIGATION' failed to execute.
[ERROR] [WallTime: 1416587151.136015] InvalidStateError: Could not execute child state 'NAV_SM': Traceback (most recent call last):
  File "/opt/ros/hydro/lib/python2.7/dist-packages/smach/concurrence.py", line 326, in _state_runner
    self._remappings[label]))
  File "/opt/ros/hydro/lib/python2.7/dist-packages/smach/state_machine.py", line 347, in execute
    container_outcome = self._update_once()
  File "/opt/ros/hydro/lib/python2.7/dist-packages/smach/state_machine.py", line 244, in _update_once
    raise ex
InvalidUserCodeError: <smach.exceptions.InvalidUserCodeError instance at 0x7faac4048a70>

Exception in thread concurrent_split:NAV_SM:
Traceback (most recent call last):
  File "/usr/lib/python2.7/threading.py", line 551, in __bootstrap_inner
    self.run()
  File "/usr/lib/python2.7/threading.py", line 504, in run
    self.__target(*self.__args, **self.__kwargs)
  File "/opt/ros/hydro/lib/python2.7/dist-packages/smach/concurrence.py", line 331, in _state_runner
    raise smach.InvalidStateError(("Could not execute child state '%s': " % label)+traceback.format_exc())
InvalidStateError: <smach.exceptions.InvalidStateError instance at 0x7faac4050560>

[INFO] [WallTime: 1416587151.143210] Concurrent state 'stuck_on_carpet' returned outcome 'preempted' on termination.
[INFO] [WallTime: 1416587151.144440] Concurrent state 'magnetic_strip' returned outcome 'preempted' on termination.
[INFO] [WallTime: 1416587151.149296] Concurrent state 'service_pause_nav' returned outcome 'preempted' on termination.
[INFO] [WallTime: 1416587151.150996] Concurrent state 'pad_pause_nav' returned outcome 'preempted' on termination.
[INFO] [WallTime: 1416587151.151991] Concurrent state 'bumper' returned outcome 'preempted' on termination.

topological navigation actiondefinition should go to strands_msgs repo

All the other message, service and action definitions are there:

https://github.com/strands-project/strands_msgs/tree/hydro-devel/strands_navigation_msgs

AttributeError: 'TopologicalNavServer' object has no attribute 'stat'

[ INFO] [1416825893.912783959]: Got new plan                                                                                                                                       │························
[ INFO] [1416825894.409034706]: Got new plan                                                                                                                                       │························
[INFO] [WallTime: 1416825894.601518] new node reached WayPoint10                                                                                                                   │························
[INFO] [WallTime: 1416825894.602231] new node reached WayPoint10                                                                                                                   │························
[ERROR] [WallTime: 1416825894.602922] bad callback: <bound method TopologicalNavServer.currentNodeCallback of <__main__.TopologicalNavServer object at 0x2530410>>                 │························
Traceback (most recent call last):                                                                                                                                                 │························
  File "/opt/ros/hydro/lib/python2.7/dist-packages/rospy/topics.py", line 682, in _invoke_callback                                                                                 │························
    cb(msg)                                                                                                                                                                        │························
  File "/opt/ros/hydro/lib/topological_navigation/navigation.py", line 199, in currentNodeCallback                                                                                 │························
    self.stat.set_at_node()                                                                                                                                                        │························AttributeError: 'TopologicalNavServer' object has no attribute 'stat'                                                                                                              │························
                                                                                                                                                                                   │························
[ INFO] [1416825894.909078515]: Got new plan                                                                                                                                       │························
[ INFO] [1416825895.409004786]: Got new plan

Distance for topological route

I'm trying to recreate the distance travelled by the robot at AAF. It looks like we didn't log odom or robot pose so I plan to do it just from the locations of tasks. To do this I need to know the distance between one node and another. I can do this easily by straight line distance, but this ignores intermediate nodes. So, is there code anywhere to report the distance of a multi-node route? @Jailander @bfalacerda

Prepare for release

For the release process:

merge in content from autonomous patrolling
all topological to be under one package + _msgs if needed anywhere else
reduce the number of packages
rename the message_store_map_switcher

Visualise topological map

It would be good to have a visualisation script for topological nodes and edge.

strands-project / strands_navigation Goto Github PK

strands_navigation's Introduction

strands_navigation

strands_navigation's People

Stargazers

Watchers

Forkers

strands_navigation's Issues

Recommend Projects

Recommend Topics

Recommend Org

Jobs