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