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