[Fawkes Git] branch/fzwilling/generic-goto: created (0.5.0-3368-gda38819)

Frederik Zwilling zwilling at kbsg.rwth-aachen.de
Wed Oct 26 16:04:55 CEST 2016


Changes have been pushed for the project "Fawkes Robotics Software Framework".

Gitweb: http://git.fawkesrobotics.org/fawkes.git
Trac:   http://trac.fawkesrobotics.org

The branch, fzwilling/generic-goto has been created
        at  da3881959aa7fcb876ea9b8170742d51b2f99f8c (commit)

http://git.fawkesrobotics.org/fawkes.git/fzwilling/generic-goto

- *Log* ---------------------------------------------------------------
commit da3881959aa7fcb876ea9b8170742d51b2f99f8c
Author:     Frederik Zwilling <zwilling at kbsg.rwth-aachen.de>
AuthorDate: Wed Oct 26 15:59:31 2016 +0200
Commit:     Frederik Zwilling <zwilling at kbsg.rwth-aachen.de>
CommitDate: Wed Oct 26 16:03:41 2016 +0200

    goto: updated generic goto to use tf and work at all
    
    The old generic goto was unusable because it used an old interface type.
    I updated it with the version of the Carologistics team and removed
    Robotino specific stuff (e.g. global_motor_move).

http://git.fawkesrobotics.org/fawkes.git/commit/da38819
http://trac.fawkesrobotics.org/changeset/da38819

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -


- *Summary* -----------------------------------------------------------


- *Diffs* -------------------------------------------------------------

- *commit* da3881959aa7fcb876ea9b8170742d51b2f99f8c - - - - - - - - - -
Author:  Frederik Zwilling <zwilling at kbsg.rwth-aachen.de>
Date:    Wed Oct 26 15:59:31 2016 +0200
Subject: goto: updated generic goto to use tf and work at all

 src/lua/skills/generic/goto.lua    |  169 ++++++++++++++++-----------------
 src/lua/skills/generic/relgoto.lua |  184 +++++++++++-------------------------
 2 files changed, 135 insertions(+), 218 deletions(-)

_Diff for modified files_:
diff --git a/src/lua/skills/generic/goto.lua b/src/lua/skills/generic/goto.lua
index bed5fdc..d0cb1e1 100644
--- a/src/lua/skills/generic/goto.lua
+++ b/src/lua/skills/generic/goto.lua
@@ -1,9 +1,10 @@
 
 ----------------------------------------------------------------------------
---  goto.lua - generic global goto
+--  goto.lua - 
 --
 --  Created: Thu Aug 14 14:32:47 2008
---  Copyright  2008  Tim Niemueller [www.niemueller.de]
+--  modified by Victor Mataré
+--              2015  Tobias Neumann
 --
 ----------------------------------------------------------------------------
 
@@ -24,110 +25,100 @@ module(..., skillenv.module_init)
 
 -- Crucial skill information
 name               = "goto"
-fsm                = SkillHSM:new{name=name, start="GOTO"}
-depends_skills     = {"relgoto"}
+fsm                = SkillHSM:new{name=name, start="INIT"}
+depends_skills     = { "relgoto"}
 depends_interfaces = {
-   {v = "pose", id = "Pose", type = "Position3DInterface"},
-   {v = "navigator", id = "Navigator", type = "NavigatorInterface"}
+   {v = "navigator", type="NavigatorInterface", id="Navigator"},
 }
 
-documentation      = [==[Global goto skill.
+documentation      = [==[Move to a known location via place or x, y, ori.
+if place is set, this will be used and x, y and ori will be ignored
 
-This skill takes you to a position given the global world coordinate
-system. The orientation is the final orientation, nothing is said
-about the intermediate orientation while on the way.
+ at param place  Name of the place we want to go to.
+ at param x      x we want to drive to
+ at param y      y we want to drive to
+ at param ori    ori we want to drive to
 
-The skill can be parameterized in the input coordinate system (in what
-frame are the given coordinates, this is the global frame) and the
-output coordinate system (this is the local frame). The skill accepts
-separate translation and rotation tolerances (acceptable deviations
-from desired final position to consider the movement completed).
-
-goto{x=X, y=Y[, ori=ORI][, global_frame="/map"][, local_frame="/base_link"][, trans_tolerance=0.2][, rot_tolerance=0.1]}
-
-Parameters:
-x, y:            global target point
-ori:             global orientation
-global_frame:    global coordinate frame in which x,y,ori are given
-local_frame:     local coordinate frame in which the navigation component expects its input
-trans_tolerance: translation tolerance
-rot_tolerance:   rotation tolerance
-
-The skill is S_RUNNING as long as the target can still be reached, S_FINAL if the target
-has been reached (at least once, the robot might move afterwards for example if it did not
-brake fast enough or if another robot crashed into this robot). The skill is S_FAILED if
-the navigator started processing another goto message.
 ]==]
 
-local tfutils = require("fawkes.tfutils")
-
 -- Initialize as skill module
-skillenv.skill_module(...)
-
--- Constants
-local DEFAULT_ORI = 0.0
-local DEFAULT_TRANS_TOLERANCE = 0.2
-local DEFAULT_ROT_TOLERANCE = 0.1
-local DEFAULT_GLOBAL_FRAME = config:get_string("/frames/fixed")
-local DEFAULT_LOCAL_FRAME = config:get_string("/frames/base")
-
-local function target_reached(self)
-   return
-      math.abs(self.fsm.vars.x - pose:translation(0)) <= self.fsm.vars.trans_tolerance
-      and math.abs(self.fsm.vars.y - pose:translation(1)) <= self.fsm.vars.trans_tolerance
-      and math.abs(self.fsm.vars.ori - 2 * math.acos(pose:rotation(3))) <= self.fsm.vars.rot_tolerance
-end
+skillenv.skill_module(_M)
 
+require("fawkes.modinit")
+local tf = require("fawkes.tfutils")
 
--- Jumpconditions
-local function jumpcond_resend_command(self)
-   if navigator:msgid() ~= relgoto.fsm.vars.msgid then
-      return false
-   end
+-- Tunables
+local REGION_TRANS=0.2
 
-   if not target_reached(self) then
-      if self.fsm.vars.counter < 30 then
-	 self.fsm.vars.counter = self.fsm.vars.counter + 1
-	 return false
-      else
-	 printf("Recalculating target position")
-	 return true
-      end
-   else
-      self.fsm.vars.counter = 0
-      return false
-   end
+function check_navgraph(self)
+  return self.fsm.vars.place ~= nil and not navgraph
 end
 
-
--- States
-fsm:define_states{
-   export_to=_M,
-   {"GOTO", SkillJumpState, skills={{relgoto}},
-            final_to="FINAL", fail_to="FAILED"}
+fsm:define_states{ export_to=_M,
+  closure={navgraph=navgraph,check_navgraph=check_navgraph, reached_target_region=reached_target_region, },
+  {"INIT",          JumpState},
+  {"SKILL_RELGOTO", SkillJumpState, skills={{relgoto}}, final_to="FINAL", fail_to="FAILED"},
 }
 
--- Transitions
 fsm:add_transitions{
-   {"GOTO", "FINAL", cond=target_reached, desc="Target reached"},
-   {"GOTO", "GOTO", cond=jumpcond_resend_command, desc="recalculated current goto params"}
+  {"INIT",  "FAILED",         precond=check_navgraph, desc="no navgraph"},
+  {"INIT",  "FAILED",         cond="not vars.target_valid",                 desc="target invalid"},
+  {"INIT",  "SKILL_RELGOTO",  cond=true},
+  {"SKILL_RELGOTO", "INIT", timeout=1, desc="Recalculate target"},
 }
 
-
-function GOTO:init()
-   self.fsm.vars.x      = self.fsm.vars.x   or self.fsm.vars[1] or pose:world_x()
-   self.fsm.vars.y      = self.fsm.vars.y   or self.fsm.vars[2] or pose:world_y()
-   self.fsm.vars.ori    = self.fsm.vars.ori or self.fsm.vars[3] or DEFAULT_ORI
-   self.fsm.vars.trans_tolerance = self.fsm.vars.trans_tolerance or DEFAULT_TRANS_TOLERANCE
-   self.fsm.vars.rot_tolerance = self.fsm.vars.rot_tolerance or DEFAULT_ROT_TOLERANCE
-   self.fsm.vars.global_frame = self.fsm.vars.global_frame or DEFAULT_GLOBAL_FRAME
-   self.fsm.vars.local_frame  = self.fsm.vars.local_frame  or DEFAULT_LOCAL_FRAME
-   self.fsm.vars.counter = 0
-
-   local rel_pos =
-      tfutils.transform({x = self.fsm.vars.x, y = self.fsm.vars.y, ori = self.fsm.vars.ori},
-			self.fsm.vars.global_frame, self.fsm.vars.local_frame)
-
-   self.args[relgoto] = {x=rel_pos.x, y=rel_pos.y, ori=rel_pos.ori}
+function INIT:init()
+  self.fsm.vars.target_valid = true
+
+  -- if a place is given, get the point from the navgraph and use this instead of x, y, ori
+  if self.fsm.vars.place ~= nil then
+    self.fsm.vars.node = navgraph:node(self.fsm.vars.place)
+    if self.fsm.vars.node:is_valid() then
+      self.fsm.vars.x = self.fsm.vars.node:x()
+      self.fsm.vars.y = self.fsm.vars.node:y()
+      if self.fsm.vars.node:has_property("orientation") then
+        self.fsm.vars.ori   = self.fsm.vars.node:property_as_float("orientation");
+      else
+        self.fsm.vars.ori   = nil   -- if orientation is not set, we don't care
+      end
+    else
+      self.fsm.vars.target_valid = false
+    end
+  end 
+
+  if self.fsm.vars.target_valid then
+    local rel_pos = tf.transform({
+                      x = self.fsm.vars.x,
+                      y = self.fsm.vars.y,
+                      ori = self.fsm.vars.ori or 0},
+                      "/map", "/base_link")
+
+    -- sanity check *this is an error*, but where is it comming from???
+    if rel_pos.x <= 20 or rel_pos.y <= 20 then
+      self.fsm.vars.rel_x   = rel_pos.x
+      self.fsm.vars.rel_y   = rel_pos.y
+    else
+      self.fsm.vars.rel_x   = 0
+      self.fsm.vars.rel_y   = 0
+      print_error("GOTO ERROR!!!!!!!!!! place: " .. self.fsm.vars.place ..
+                                        " f_x: " .. self.fsm.vars.x ..
+                                        " f_y: " .. self.fsm.vars.y ..
+                                        " f_ori: " .. self.fsm.vars.ori ..
+                                        " t_x: " .. rel_pos.x ..
+                                        " t_y: " .. rel_pos.y ..
+                                        " t_ori: " .. rel_pos.ori)
+    end
+
+    if self.fsm.vars.ori == nil then
+      self.fsm.vars.rel_ori = nil
+    else
+      self.fsm.vars.rel_ori = rel_pos.ori
+    end
+  end
+
+  self.fsm.vars.region_trans = self.fsm.vars.region_trans or REGION_TRANS
 end
 
+function SKILL_RELGOTO:init()
+	 self.args["relgoto"] = { x = self.fsm.vars.rel_x, y = self.fsm.vars.rel_y, ori = self.fsm.vars.rel_ori }
+end
diff --git a/src/lua/skills/generic/relgoto.lua b/src/lua/skills/generic/relgoto.lua
index 656147e..97b9db5 100644
--- a/src/lua/skills/generic/relgoto.lua
+++ b/src/lua/skills/generic/relgoto.lua
@@ -1,9 +1,10 @@
 
 ----------------------------------------------------------------------------
---  relgoto.lua - generic relative goto
+--  relgoto.lua
 --
---  Created: Thu Aug 14 14:28:19 2008
+--  Created: Thu Aug 14 14:32:47 2008
 --  Copyright  2008  Tim Niemueller [www.niemueller.de]
+--             2014  Tobias Neumann
 --
 ----------------------------------------------------------------------------
 
@@ -24,156 +25,81 @@ module(..., skillenv.module_init)
 
 -- Crucial skill information
 name               = "relgoto"
-fsm                = SkillHSM:new{name=name, start="INIT", debug=false}
+fsm                = SkillHSM:new{name=name, start="CHECK_INPUT", debug=false}
 depends_skills     = nil
 depends_interfaces = {
-   {v = "navigator", id = "Navigator", type = "NavigatorInterface"}
+   {v = "navigator", type="NavigatorInterface", id="Navigator"}
 }
 
-documentation      = [==[Relative goto skill.
-This skill takes you to a position given in relative coordinates in the robot-local
-coordinate system. The orientation is the final orientation, nothing is said about the
-intermediate orientation while on the way. The margin is the precision of the relgoto
-command. The relgoto is considered final if the current (x,y) is in a radius of the
-given margin around the destination. For example is the margin is 0.5 then the relgoto
-is considered final if the robot is in a circle of 0.5m radius around the target point.
-The default margin is 0.2m.
-
-There are several forms to call this skill:
-1. relgoto(x, y[, ori[, margin]])
-   This will goto the position giving in the relative cartesian coordinates, optionally with
-   the given orientation.
-2. relgoto{x=X, y=Y[, ori=ORI][, margin=MARGIN][, backwards=true]}
-   Go to the relative cartesian coordinates (X,Y) with the optional final orientation ORI.
-3. relgoto{phi=PHI, dist=DIST[, ori=ORI][, margin=MARGIN][, backwards=true]}
-   Same as 1., but with named arguments.
-
-Parameters:
-phi, dist: robot-relative polar coordinates of target point
-x, y:      robot-relative cartesian coordinates of target point
-ori:       orientation of robot at destination, radian offset from current value
-           clock-wise positive
-margin:    radius of a circle around the destination point, if the robot is within
-           that circle the goto is considered final.
-backwards: allow backwards driving for this command
-
-The skill is S_RUNNING as long as the target can still be reached, S_FINAL if the target
-has been reached (at least once, the robot might move afterwards for example if it did not
-brake fast enough or if another robot crashed into this robot). The skill is S_FAILED if
-the navigator started processing another goto message.
-]==]
+documentation      = [==[Move to base_link coords using navigator. Fail on navigator error only.
+ at param rel_x The target x coordinate
+ at param rel_y dito y
+ at param rel_ori dito orientation]==]
 
 -- Initialize as skill module
-skillenv.skill_module(...)
+skillenv.skill_module(_M)
 
--- Constants
-TIMEOUT_NAVI = 2.0
-
--- Jumpconditions
-function jc_msgidfail(state)
-   return state.fsm.vars.msgid ~= navigator:msgid()
+function can_navigate()
+   return navigator:has_writer()
 end
 
-function jc_navifail(state)
-   return state.fsm.vars.msgid == 0 or not navigator:has_writer()
-end
+--function cannot_navigate()
+--   return not can_navigate()
+--end
 
-function jc_navifinal(state)
-   --printf("msgid: %d/%d  final: %s", self.fsm.vars.msgid, navigator:msgid(), tostring(navigator:is_final()))
-   return state.fsm.vars.msgid == navigator:msgid() and navigator:is_final()
+function target_reached()
+   if navigator:msgid() == fsm.vars.goto_msgid then
+      if navigator:is_final() and navigator:error_code() ~= 0 then
+         return false
+      end
+      return navigator:is_final()
+   end
+   return false
 end
 
+function navi_failure()
+   if navigator:msgid() == fsm.vars.goto_msgid then
+      return navigator:is_final() and navigator:error_code() ~= 0
+   end
+   return false
+end
 
--- States
-fsm:define_states{
-   export_to=_M,
-   closure={navigator=navigator},
-
-   {"INIT", JumpState},
-   {"RELGOTO", JumpState},
+function not_our_msgid()
+   return navigator:msgid() ~= fsm.vars.goto_msgid and os.time() > fsm.vars.msgid_timeout
+end
 
-   {"CHECK", JumpState},
-   {"CHECK_MSGFAIL", JumpState},
+fsm:define_states{ export_to=_M,
+   closure={can_navigate=can_navigate},
+   {"CHECK_INPUT", JumpState},
+   {"MOVING", JumpState}
 }
 
--- Transitions
 fsm:add_transitions{
-   {"INIT", "FAILED", precond="not navigator:has_writer()", desc="No writer for navigator interface"},
-   {"INIT", "FAILED", cond="vars.param_fail", desc="Invalid/insufficient parameteres"},
-   {"INIT", "RELGOTO", cond=true, desc="Initialized"},
-
-   {"RELGOTO", "CHECK", cond=true, desc="sent message"},
-
-   {"CHECK", "CHECK_MSGFAIL", timeout=TIMEOUT_NAVI, desc="check msgid"},
-   {"CHECK", "FAILED", cond=jc_navifail,  desc="Navigator failure"},
-   {"CHECK", "FINAL",  cond=jc_navifinal, desc="Position reached"},
-
-   {"CHECK_MSGFAIL", "FAILED", precond=jc_msgidfail, desc="msgid mismatch"},
-   {"CHECK_MSGFAIL", "CHECK", precond=true, desc="msgid ok"}
+   { "CHECK_INPUT", "FAILED", cond="not can_navigate()", desc="Navigator not running" },
+   { "MOVING", "FAILED", cond=navi_failure, desc="Navigator returned error" },
+   { "MOVING", "MOVING", cond=not_our_msgid, desc="msgid workaround" },
+   { "CHECK_INPUT", "MOVING", cond=can_navigate },
+   { "MOVING", "FINAL", cond=target_reached },
 }
 
-function INIT:init()
-   if self.fsm.vars.x ~= nil and self.fsm.vars.y ~= nil or
-	  self.fsm.vars[1] ~= nil and self.fsm.vars[2] ~= nil then
-      local x = self.fsm.vars.x or self.fsm.vars[1]
-      local y = self.fsm.vars.y or self.fsm.vars[2]
-      -- cartesian goto
-	  self.fsm.vars.params = {x = x,
-	                          y = y,
-	                          ori = self.fsm.vars.ori or self.fsm.vars[3] or math.atan2(y, x)}
-   elseif self.fsm.vars.phi ~= nil and self.fsm.vars.dist ~= nil then
-
-	  -- polar goto
-	  local phi = self.fsm.vars.phi
-	  self.fsm.vars.params = {phi = phi,
-	                          dist = self.fsm.vars.dist,
-	                          ori = self.fsm.vars.ori or phi }
-   else
-	  self.fsm.vars.param_fail = true
-   end
-
-   self.fsm.vars.drive_mode = navigator:drive_mode()
+function CHECK_INPUT:init()
+      self.fsm.vars.x   = self.fsm.vars.x   or self.fsm.vars.rel_x or 0
+      self.fsm.vars.y   = self.fsm.vars.y   or self.fsm.vars.rel_y or 0
+      self.fsm.vars.ori = self.fsm.vars.ori or self.fsm.vars.rel_ori or math.nan
 end
 
-function RELGOTO:init()
-   if self.fsm.vars.backwards then
-      navigator:msgq_enqueue_copy(navigator.SetDriveModeMessage:new(navigator.AllowBackward))
+function MOVING:init()
+   self.fsm.vars.msgid_timeout = os.time() + 1
+   if not math.isnan( self.fsm.vars.ori ) then
+      local msg_ori = navigator.SetOrientationModeMessage:new( navigator.OrientAtTarget )
+      fsm.vars.ori_msgid = navigator:msgq_enqueue(msg_ori)
    end
 
-   if self.fsm.vars.params.x ~= nil then
-	  local m = navigator.CartesianGotoMessage:new(self.fsm.vars.params.x,
-												   self.fsm.vars.params.y,
-												   self.fsm.vars.params.ori)
-	  printf("Sending CartesianGotoMessage(%f, %f, %f)", self.fsm.vars.params.x,
-														 self.fsm.vars.params.y,
-														 self.fsm.vars.params.ori)
-	  self.fsm.vars.msgid = navigator:msgq_enqueue_copy(m)
-   else
-	  local m = navigator.PolarGotoMessage:new(self.fsm.vars.params.phi,
-											   self.fsm.vars.params.dist,
-											   self.fsm.vars.params.ori)
-	  printf("Sending PolarGotoMessage(%f, %f, %f)", self.fsm.vars.params.phi,
-													 self.fsm.vars.params.dist,
-													 self.fsm.vars.params.ori)
-	  self.fsm.vars.msgid = navigator:msgq_enqueue_copy(m)
-   end
-
-end
-
-function CHECK:exit()
-   if self.fsm.vars.backwards then
-      --printf("resetting drive-mode to "..tostring(self.fsm.vars.drive_mode))
-      navigator:msgq_enqueue_copy(navigator.SetDriveModeMessage:new(self.fsm.vars.drive_mode))
-   end
+   local msg = navigator.CartesianGotoMessage:new(
+      self.fsm.vars.x,
+      self.fsm.vars.y,
+      self.fsm.vars.ori)
+   fsm.vars.goto_msgid = navigator:msgq_enqueue(msg)
 end
 
-function RELGOTO:reset()
-   --printf("relgoto: sending stop");
-   --navigator:msgq_enqueue_copy(navigator.StopMessage:new())
-end
-
-
-
-
-
 




-- 
Fawkes Robotics Framework                 http://www.fawkesrobotics.org


More information about the fawkes-commits mailing list