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"
Beispiel #2
0
	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)
Beispiel #3
0
 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()
Beispiel #4
0
	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))
Beispiel #5
0
	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))
Beispiel #6
0
 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))
Beispiel #7
0
    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))
Beispiel #8
0
    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'
Beispiel #10
0
    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)
Beispiel #11
0
    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'
Beispiel #12
0
    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