Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Adjusted MoveBaseState for input waypoint in PoseStamped instead of deprecated Pose2D. #12

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

Adjusted MoveBaseState for input waypoint in PoseStamped instead of deprecated Pose2D. #12

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
48 changes: 29 additions & 19 deletions flexbe_navigation_states/src/flexbe_navigation_states/move_base_state.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,38 +4,46 @@
from flexbe_core.proxy import ProxyActionClient

from actionlib_msgs.msg import GoalStatus
from move_base_msgs.msg import *
from geometry_msgs.msg import Pose, Point, Quaternion, Pose2D
from tf import transformations
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from geometry_msgs.msg import PoseStamped, Point, Quaternion
import rospy

"""
Created on 11/19/2015

@author: Spyros Maniatopoulos

Updated on 09/04/2021
@github/tbazina
"""

class MoveBaseState(EventState):
"""
Navigates a robot to a desired position and orientation using move_base.

># waypoint Pose2D Target waypoint for navigation.
-- action_topic str move_base action topic, (/move_base)
># waypoint PoseStamped Target waypoint for navigation.

<= arrived Navigation to target pose succeeded.
<= failed Navigation to target pose failed.
<= arrived Navigation to target pose succeeded.
<= failed Navigation to target pose failed.
<= preempted Navigation to target pose preempted.
"""

def __init__(self):
def __init__(self, action_topic):
"""Constructor"""

super(MoveBaseState, self).__init__(outcomes = ['arrived', 'failed'],
input_keys = ['waypoint'])
super(MoveBaseState, self).__init__(
outcomes = ['arrived', 'failed', 'preempted'],
input_keys = ['waypoint']
)

self._action_topic = "/move_base"
self._action_topic = action_topic

self._client = ProxyActionClient({self._action_topic: MoveBaseAction})

self._arrived = False
self._failed = False
self._preempted = False


def execute(self, userdata):
Expand All @@ -45,6 +53,8 @@ def execute(self, userdata):
return 'arrived'
if self._failed:
return 'failed'
if self._preempted:
return 'preempted'

if self._client.has_result(self._action_topic):
status = self._client.get_state(self._action_topic)
Expand All @@ -56,33 +66,33 @@ def execute(self, userdata):
Logger.logwarn('Navigation failed: %s' % str(status))
self._failed = True
return 'failed'
elif status == GoalStatus.PREEMPTED:
Logger.logwarn('Navigation preempted: %s' % str(status))
self._preempted = True
return 'preempted'



def on_enter(self, userdata):
"""Create and send action goal"""

self._arrived = False
self._failed = False
self._preempted = False

# Create and populate action goal
goal = MoveBaseGoal()

pt = Point(x = userdata.waypoint.x, y = userdata.waypoint.y)
qt = transformations.quaternion_from_euler(0, 0, userdata.waypoint.theta)

goal.target_pose.pose = Pose(position = pt,
orientation = Quaternion(*qt))

goal.target_pose.header.frame_id = "odom"
# goal.target_pose.header.stamp.secs = 5.0
goal.target_pose = userdata.waypoint
goal.target_pose.header.stamp = rospy.Time.now()

# Send the action goal for execution
try:
self._client.send_goal(self._action_topic, goal)
except Exception as e:
Logger.logwarn("Unable to send navigation action goal:\n%s" % str(e))
self._failed = True

def cancel_active_goals(self):
if self._client.is_available(self._action_topic):
if self._client.is_active(self._action_topic):
Expand Down