class AligningVehicle(smach.State):
def __init__(self,resources):
smach.State.__init__(self, outcomes=['aligned','timed_out'],input_keys=['time_out','e_x','e_y'])
self.resources=resources
def execute(self, ud):
self.resources=Interaction()
#do this job publish data to sensor controller
rospy.loginfo("marker detected .....setting vehicle point to marker calling %s",header.RESET_POSE_SERVICE)
resetClient=rospy.ServiceProxy(header.RESET_POSE_SERVICE, krakenResetPose)
msg=krakenResetPoseRequest()
resp=resetClient(msg)
##help look here --- http://wiki.ros.org/rospy_tutorials/Tutorials/WritingServiceClient
##################--within this it is done
rospy.loginfo("aligning vehicle to set the marker")
self.ipClient=SimpleActionClient(header.MARKER_DETECT_ACTION_SERVER,markerAction)
self.ipClient.cancel_all_goals()
goal=markerGoal()
goal.order=header.ALLIGN_MARKER
self.ipClient.send_goal(goal,done_cb= self.done_cb, feedback_cb= self.feedback_cb)
self.ipClient.wait_for_result(rospy.Duration(ud.time_out))
if self.result:
rospy.loginfo("successfully aligned the vehicle with the marker ")
#again use the service to reset the vehicle's position
msg=krakenResetPoseRequest()
resp=resetClient(msg)
########################
self.ipClient.cancel_goal()
return 'aligned'
else:
self.ipClient.cancel_goal()
rospy.logerr("marker unaligned and timed out!!!")
return 'timed_out'
def feedback_cb(self,feed):
##publish this error data to the sensor and move the vehicle
msg=ipControllererror()
msg.dy=feed.errorx
msg.dx=msg.dy/tan(feed.errorangle)
self.resources.ipControllerPublisher.publish(msg)
##done moving
def done_cb(self,result):
if result.sequence==header.MARKER_ALLIGNED:
self.result=True
else:
self.result=False
def execute(self, ud):
#comment this at last
self.resources=Interaction()
#-------------
ipClient=SimpleActionClient(header.MARKER_DETECT_ACTION_SERVER, markerAction)
goal=markerGoal()
goal.order=ip_header.DETECT_MARKER
ipClient.cancel_all_goals()
ipClient.send_goal(goal, done_cb=self.doneCb)
result=ipClient.wait_for_result()
if ipClient.get_state()==GoalStatus.SUCCEEDED:
rospy.loginfo("succesfully detected marker :)")
return 'marker_found'
def execute(self, ud):
#comment this at last
#self.resources=Interaction()
#-------------
ipClient = SimpleActionClient(header.MARKER_DETECT_ACTION_SERVER,
markerAction)
rospy.loginfo("Waiting for IP marker action server")
ipClient.wait_for_server()
goal = markerGoal()
goal.order = ip_header.DETECT_MARKER
ipClient.cancel_all_goals()
ipClient.send_goal(goal, done_cb=self.doneCb)
result = ipClient.wait_for_result()
if ipClient.get_state() == GoalStatus.SUCCEEDED:
rospy.loginfo("succesfully detected marker :)")
return 'marker_found'
class ActionDispatcher(object):
def __init__(self, name):
rospy.loginfo("Starting %s" % name)
self.client = None
action_list = rospy.get_param("han_action_dispatcher")["han_actions"]
self.default_action = rospy.get_param("~default_action")
self.dyn_srv = DynServer(HanActionDispatcherConfig, self.dyn_callback)
self.servers = {}
for a in action_list.items():
name = a[0]
self.servers[name] = SimpleActionServer(
name,
MoveBaseAction,
execute_cb=(lambda x: lambda msg: self.execute_cb(
msg, x[0], x[1]["action"]))(a),
auto_start=False)
self.servers[name].register_preempt_callback(self.preempt_cb)
self.servers[name].start()
rospy.loginfo("done")
def execute_cb(self, msg, name, action):
a = action if not self.use_default else self.default_action
self.client = SimpleActionClient(a, MoveBaseAction)
rospy.logdebug("Waiting for action server:" + a)
self.client.wait_for_server()
rospy.logdebug("Sending goal to:" + a)
self.client.send_goal_and_wait(msg)
self.servers[name].set_succeeded()
self.client = None
def preempt_cb(self):
if self.client != None:
self.client.cancel_all_goals()
self.client = None
def dyn_callback(self, config, level):
self.use_default = config["use_default"]
return config
class NavStall():
def __init__(self):
rospy.init_node('nav_stall', anonymous=True)
rospy.on_shutdown(self.shutdown)
# Publisher to manually control the robot (e.g. to stop it)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)
# Subscribe to the move_base action server
self.move_base = SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait 60 seconds for the action server to become available
self.move_base.wait_for_server()
rospy.loginfo("Connected to move base server")
#initiate bucket counter variables
self.stallCounterBucket = 0
self.STOP_MAX_STALL_BUCKET_COUNT = 10
self.MAX_STALL_BUCKET_COUNT = 3
self.hasGivenUp = 0
self.current_topic = rospy.get_param("~current_topic")
self.stall_current = rospy.get_param("~stall_current", 0.1)
self.recovery_speed = rospy.get_param("~recovery_speed", 0.1)
self.recovery_time = rospy.get_param("~recovery_time", 2)
rospy.loginfo("Time: " + str(self.recovery_time))
# Wait for motor current topics to become available
rospy.loginfo("Waiting for motor current topic to become available...")
rospy.wait_for_message(self.current_topic, AnalogFloat)
#subscribe to motor current topics
rospy.Subscriber(self.current_topic, AnalogFloat, self.detect_stall)
rospy.loginfo("Stall detection started on " + self.current_topic)
def shutdown(self):
self.cancelAndStop()
'''
Implement leaky bucket for stall detection
'''
def detect_stall(self, msg):
now = rospy.Time.now()
if msg.header.stamp.secs < (now.secs-1):
#skip messages that are older than 1 sec (stale)
return
'''
if (self.hasGivenUp):
self.givenUp()
return
#if we've been stalled for too long time, just give up
if (self.stallCounterBucket > self.STOP_MAX_STALL_BUCKET_COUNT):
self.hasGivenUp = 1
self.givenUp()
return
'''
if (msg.value > self.stall_current):
self.stallCounterBucket+=2;
rospy.loginfo("Potential stall condition detected at current: " + str(msg.value) + " (stall current: " + str(self.stall_current) + ") - incremented Stall Counter to " + str(self.stallCounterBucket))
else:
if (self.stallCounterBucket > 0):
self.stallCounterBucket-=1; #decrement bucket
rospy.loginfo("Decremented Stall Counter to " + str(self.stallCounterBucket))
if (self.stallCounterBucket > self.MAX_STALL_BUCKET_COUNT):
rospy.logwarn("Stall conditition detected! Trying to recover...")
self.cancelAndStop()
self.recover()
rospy.logwarn("Stall recovery completed.")
def recover(self):
rospy.loginfo("Initiating recovery... Speed: " + str(self.recovery_speed) + " Time: " + str(self.recovery_time))
cmd_vel = Twist()
#move back for one second at low speed
cmd_vel.linear.x = -self.recovery_speed
#need to repeat this multiple times, as base_controller will timeout after 0.5 sec
for x in range(0, self.recovery_time*4):
self.cmd_vel_pub.publish(cmd_vel)
rospy.sleep(0.25)
#stop
rospy.loginfo("Stopping the robot after recovery move...")
self.cmd_vel_pub.publish(Twist())
rospy.sleep(1)
def cancelAndStop(self):
rospy.loginfo("Canceling all goals...")
self.move_base.cancel_all_goals()
rospy.sleep(1)
rospy.loginfo("Stopping the robot...")
self.cmd_vel_pub.publish(Twist())
rospy.sleep(1)
'''
class NavStall():
def __init__(self):
rospy.init_node('nav_stall', anonymous=True)
rospy.on_shutdown(self.shutdown)
# Publisher to manually control the robot (e.g. to stop it)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
# Subscribe to the move_base action server
self.move_base = SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait 60 seconds for the action server to become available
self.move_base.wait_for_server()
rospy.loginfo("Connected to move base server")
#initiate bucket counter variables
self.stallCounterBucket = 0
self.STOP_MAX_STALL_BUCKET_COUNT = 10
self.MAX_STALL_BUCKET_COUNT = 3
self.hasGivenUp = 0
self.current_topic = rospy.get_param("~current_topic")
self.stall_current = rospy.get_param("~stall_current", 0.1)
self.recovery_speed = rospy.get_param("~recovery_speed", 0.1)
self.recovery_time = rospy.get_param("~recovery_time", 2)
rospy.loginfo("Time: " + str(self.recovery_time))
# Wait for motor current topics to become available
rospy.loginfo("Waiting for motor current topic to become available...")
rospy.wait_for_message(self.current_topic, AnalogFloat)
#subscribe to motor current topics
rospy.Subscriber(self.current_topic, AnalogFloat, self.detect_stall)
rospy.loginfo("Stall detection started on " + self.current_topic)
def shutdown(self):
self.cancelAndStop()
'''
Implement leaky bucket for stall detection
'''
def detect_stall(self, msg):
now = rospy.Time.now()
if msg.header.stamp.secs < (now.secs-1):
#skip messages that are older than 1 sec (stale)
return
'''
if (self.hasGivenUp):
self.givenUp()
return
#if we've been stalled for too long time, just give up
if (self.stallCounterBucket > self.STOP_MAX_STALL_BUCKET_COUNT):
self.hasGivenUp = 1
self.givenUp()
return
'''
if (msg.value > self.stall_current):
self.stallCounterBucket+=2;
rospy.loginfo("Potential stall condition detected at current: " + str(msg.value) + " (stall current: " + str(self.stall_current) + ") - incremented Stall Counter to " + str(self.stallCounterBucket))
else:
if (self.stallCounterBucket > 0):
self.stallCounterBucket-=1; #decrement bucket
rospy.loginfo("Decremented Stall Counter to " + str(self.stallCounterBucket))
if (self.stallCounterBucket > self.MAX_STALL_BUCKET_COUNT):
rospy.logwarn("Stall conditition detected! Trying to recover...")
self.cancelAndStop()
self.recover()
rospy.logwarn("Stall recovery completed.")
def recover(self):
rospy.loginfo("Initiating recovery... Speed: " + str(self.recovery_speed) + " Time: " + str(self.recovery_time))
cmd_vel = Twist()
#move back for one second at low speed
cmd_vel.linear.x = -self.recovery_speed
#need to repeat this multiple times, as base_controller will timeout after 0.5 sec
for x in range(0, self.recovery_time*4):
self.cmd_vel_pub.publish(cmd_vel)
rospy.sleep(0.25)
#stop
rospy.loginfo("Stopping the robot after recovery move...")
self.cmd_vel_pub.publish(Twist())
rospy.sleep(1)
def cancelAndStop(self):
rospy.loginfo("Canceling all goals...")
self.move_base.cancel_all_goals()
rospy.sleep(1)
rospy.loginfo("Stopping the robot...")
self.cmd_vel_pub.publish(Twist())
rospy.sleep(1)
'''
class AligningVehicle(smach.State):
def __init__(self, resources):
smach.State.__init__(self,
outcomes=['aligned', 'timed_out'],
input_keys=['time_out', 'e_x', 'e_y'])
self.resources = resources
def execute(self, ud):
self.resources = Interaction()
#do this job publish data to sensor controller
rospy.loginfo(
"marker detected .....setting vehicle point to marker calling %s",
header.RESET_POSE_SERVICE)
resetClient = rospy.ServiceProxy(header.RESET_POSE_SERVICE,
krakenResetPose)
msg = krakenResetPoseRequest()
resp = resetClient(msg)
##help look here --- http://wiki.ros.org/rospy_tutorials/Tutorials/WritingServiceClient
##################--within this it is done
rospy.loginfo("aligning vehicle to set the marker")
self.ipClient = SimpleActionClient(header.MARKER_DETECT_ACTION_SERVER,
markerAction)
self.ipClient.cancel_all_goals()
goal = markerGoal()
goal.order = header.ALLIGN_MARKER
self.ipClient.send_goal(goal,
done_cb=self.done_cb,
feedback_cb=self.feedback_cb)
self.ipClient.wait_for_result(rospy.Duration(ud.time_out))
if self.result:
rospy.loginfo("successfully aligned the vehicle with the marker ")
#again use the service to reset the vehicle's position
msg = krakenResetPoseRequest()
resp = resetClient(msg)
########################
self.ipClient.cancel_goal()
return 'aligned'
else:
self.ipClient.cancel_goal()
rospy.logerr("marker unaligned and timed out!!!")
return 'timed_out'
def feedback_cb(self, feed):
##publish this error data to the sensor and move the vehicle
msg = ipControllererror()
msg.dy = feed.errorx
msg.dx = msg.dy / tan(feed.errorangle)
self.resources.ipControllerPublisher.publish(msg)
##done moving
def done_cb(self, result):
if result.sequence == header.MARKER_ALLIGNED:
self.result = True
else:
self.result = False
