def execute(self, userdata): if self.initialTorque is None: self.watchdogTime = get_time() + self.watchdog if self._sub.has_msg(self._topic): message = self._sub.get_last_msg(self._topic) for i in range(len(message.name)): if message.name[i] == self.Joint: self.Joint = i break self.initialTorque = message.effort[self.Joint] print("Initial torque is:" + str(self.initialTorque)) else: if self._sub.has_msg(self._topic): torque = self._sub.get_last_msg(self._topic).effort[self.Joint] if abs(self.initialTorque - torque) >= self.Threshold: Logger.loginfo('Reading torque :' + str(abs(self.initialTorque - torque))) if self.timer < get_time(): userdata.torque = torque return 'threshold' else: self.timer = get_time() + self.min_time if (self.watchdogTime - get_time() <= 0): return "watchdog"
def __init__(self): ## scanning speed command publisher self.scanning_speed_pub = rospy.Publisher('/scanning_speed_cmd', Float64) ## laser center command publisher self.laser_center_pub = rospy.Publisher('/laser_center', Bool) ## robot status subscriber (to get the current speed of the laser) rospy.Subscriber('/robot_status', RobotStatusStamped, self.status_cb) ## scanning_once subscriber rospy.Subscriber('/scanning_once', Float64, self.scanning_once_cb) ## point cloud control publisher self.ptcld_ctrl_pub = rospy.Publisher('/pointcloud_control', Bool) ## scanning state self.scanning_state = ScanningFeedback.READY ## last goal time set, to avoid race condition self.last_goal_time = get_time() ## action server self.goal = ServerGoalHandle() self.action_server = actionlib.ActionServer('ScanningOnceAS', ScanningAction, self.goal_cb, auto_start=False) self.action_server.start() ## action client self.action_client = actionlib.SimpleActionClient('ScanningOnceAS', ScanningAction) self.last_angle = 0 self.last_direction = 0 rospy.Subscriber('/joint_states', JointState, self.joint_states_cb)
def __init__(self): ## scanning speed command publisher self.scanning_speed_pub = rospy.Publisher('scanning_speed_cmd', Float64, queue_size=10) ## laser center command publisher self.laser_center_pub = rospy.Publisher('laser_center', Bool, queue_size=1) ## robot status subscriber (to get the current speed of the laser) rospy.Subscriber('robot_status', RobotStatusStamped, self.status_cb, queue_size=100) ## scanning_once subscriber rospy.Subscriber('scanning_once', Float64, self.scanning_once_cb, queue_size=1) ## end of swipe subscriber (to know when to stop) rospy.Subscriber('end_of_swipe', Bool, self.end_of_swipe_cb, queue_size=1) ## point cloud control publisher self.ptcld_ctrl_pub = rospy.Publisher('pointcloud_control', Bool, queue_size=1) ## scanning state ("Not scanning", "Starting scanning", "Got first cloud") self.scanning_state = "Not scanning" self.last_goal_time = get_time()
def status_cb(self, robot_status): self.scanning_speed = robot_status.scanning_speed if (self.scanning_speed == 0.0) and \ (get_time() - self.last_goal_time>0.5): # if not moving, updating state rospy.logdebug('NTH - Laser speed 0: setting state to "Not scanning".') if self.scanning_state != ScanningFeedback.READY: self.scanning_state = ScanningFeedback.READY self.goal.set_succeeded(ScanningResult(ScanningResult.ERROR), "Aborting as laser is not moving") self.goal.publish_feedback(ScanningFeedback(self.scanning_state))
def start_scanning(self, speed): # clip speed speed = max(min(speed, 0.1), 1.2) # send command rospy.loginfo("NTH - Starting scan") rospy.logdebug("NTH - Sending scanning speed command: %f and disabling \ publication of the messy point cloud."%speed) self.ptcld_ctrl_pub.publish(False) self.scanning_speed_pub.publish(speed) self.last_goal_time = get_time() # update state self.scanning_state = ScanningFeedback.WAITING_FOR_FIRST_SWIPE self.goal.publish_feedback(ScanningFeedback(self.scanning_state))
def status_cb(self, robot_status): self.scanning_speed = robot_status.scanning_speed if (self.scanning_speed == 0.0) and \ (get_time() - self.last_goal_time>0.5): # if not moving, updating state rospy.logdebug( 'NTH - Laser speed 0: setting state to "Not scanning".') if self.scanning_state != ScanningFeedback.READY: self.scanning_state = ScanningFeedback.READY self.goal.set_succeeded(ScanningResult(ScanningResult.ERROR), "Aborting as laser is not moving") self.goal.publish_feedback( ScanningFeedback(self.scanning_state))
def start_scanning(self, speed): # clip speed speed = max(min(speed, 0.1), 1.2) # send command rospy.loginfo("NTH - Starting scan") rospy.logdebug( "NTH - Sending scanning speed command: %f and disabling \ publication of the messy point cloud." % speed) self.ptcld_ctrl_pub.publish(False) self.scanning_speed_pub.publish(speed) self.last_goal_time = get_time() # update state self.scanning_state = ScanningFeedback.WAITING_FOR_FIRST_SWIPE self.goal.publish_feedback(ScanningFeedback(self.scanning_state))
def scanning_once_cb(self, speed): if self.scanning_state != "Not scanning": # don't do anything if we're already scanning return if self.scanning_speed: # don't do anything if the laser is already moving return # clip speed speed = max(min(speed, 0.1), 1.2) # send command rospy.logdebug( "NTH - Sending scanning speed command: %f and disabling \ publication of the messy point cloud." % speed) self.ptcld_ctrl_pub.publish(False) self.scanning_speed_pub.publish(speed) self.last_goal_time = get_time() # update state self.scanning_state = "Starting scanning"
def process_scan(self, scan_msg): # Extract the interval in a restricted range to determine if the is a closed door middle_index = len(scan_msg.ranges) / 2 ranges_at_center = scan_msg.ranges[middle_index - 40:middle_index + 40] interval_of_door = self.interval(ranges_at_center) print("dist = " + str(interval_of_door)) if interval_of_door < 0.1 or interval_of_door > 0.5 or max( ranges_at_center) > 0.85: rospy.loginfo("Distance to door is more than a meter") return "open" if (self.time - get_time() <= 0): Logger.loginfo('no speech detected') return 'closed'
def on_enter(self, userdata): Logger.loginfo('entering marker state') self.time = get_time() if not self._connected: (msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic) if msg_topic == self._topic: msg_type = self._get_msg_from_path(msg_path) self._sub = ProxySubscriberCached({self._topic: msg_type}) self._connected = True Logger.loginfo( 'Successfully subscribed to previously unavailable topic %s' % self._topic) else: Logger.logwarn('Topic %s still not available, giving up.' % self._topic)
def execute(self, userdata): ''' Execute this state ''' Logger.loginfo('looking for marker ' + str(self.index)) if not self._connected: return 'not_found' if self._sub.has_msg(self._topic): message = self._sub.get_last_msg(self._topic) #Logger.loginfo(str(message)) #Logger.loginfo('there is ' + str(len(message.markers))+' in the list') for marker in message.markers: Logger.loginfo('id is ' + str(marker.id)) if int(marker.id) == int(self.index): # pose = [] # pose.append(marker.pose.pose.orientation.w) # pose.append(marker.pose.pose.orientation.x) # pose.append(marker.pose.pose.orientation.y) # pose.append(marker.pose.pose.orientation.z) # # quat = self.quat # marker.pose.pose.position.x -= 0.20 # marker.pose.pose.position.z += 0.05 # marker.pose.pose.orientation.x = -0.460612186114 # marker.pose.pose.orientation.y = -0.46897814506 # marker.pose.pose.orientation.z = 0.501742587173 # marker.pose.pose.orientation.w = 0.56227243368 userdata.pose = marker.pose.pose self._sub.remove_last_msg(self._topic) return 'done' return 'not_found' time = self.time - get_time() + 5 Logger.loginfo('marker not found ' + str(int(time)) + ' before giving up') if (time <= 0): return 'not_found'
def __init__(self): ## scanning speed command publisher self.scanning_speed_pub = rospy.Publisher('scanning_speed_cmd', Float64, queue_size=1) ## laser center command publisher self.laser_center_pub = rospy.Publisher('laser_center', Bool, queue_size=1) ## robot status subscriber (to get the current speed of the laser) rospy.Subscriber('robot_status', RobotStatusStamped, self.status_cb, queue_size=10) ## scanning_once subscriber rospy.Subscriber('scanning_once', Float64, self.scanning_once_cb, queue_size=1) ## point cloud control publisher self.ptcld_ctrl_pub = rospy.Publisher('pointcloud_control', Bool, queue_size=1) ## scanning state self.scanning_state = ScanningFeedback.READY ## last goal time set, to avoid race condition self.last_goal_time = get_time() ## action server self.goal = ServerGoalHandle() self.action_server = actionlib.ActionServer('ScanningOnceAS', ScanningAction, self.goal_cb, auto_start=False) self.action_server.start() ## action client self.action_client = actionlib.SimpleActionClient( 'ScanningOnceAS', ScanningAction) self.last_angle = 0 self.last_direction = 0 rospy.Subscriber('joint_states', JointState, self.joint_states_cb, queue_size=10)
def on_enter(self, userdata): rospy.loginfo("Waiting for door...") self.time = get_time() + self.timeout